From 9ae913917401bc8798c1f803b960c136301e9fb1 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 13 Sep 2022 11:56:15 -0400 Subject: [PATCH 001/252] Corrections to readme and default MIP_TIMESTAMP_TYPE in cmake. (#22) * Update README. * Default MIP_TIMESTAMP_TYPE to uint64_t to get it into the cmake cache rather than relying on the default #ifndef in case it changes. * Add note about timeout_type typedef to timestamp_type. --- CMakeLists.txt | 2 +- README.md | 23 ++++++++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e1261efd2..07e8578c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,7 +60,7 @@ endif() option(WITH_SERIAL "Build serial connection support into the library and examples" ON) option(WITH_TCP "Build TCP connection support into the library and exampels" ON) -set(MIP_TIMESTAMP_TYPE "" CACHE STRING "Override the type used for received data timestamps and timeouts (must be unsigned or at least 64 bits).") +set(MIP_TIMESTAMP_TYPE "uint64_t" CACHE STRING "Override the type used for received data timestamps and timeouts (must be unsigned or at least 64 bits).") option(BUILD_PACKAGE "Whether to build a package from the resulting binaries" OFF) diff --git a/README.md b/README.md index a2082bb9e..75c391f3c 100644 --- a/README.md +++ b/README.md @@ -94,6 +94,7 @@ The following options may be specified when configuring the build with CMake (e. * BUILD_DOCUMENTATION_QUIET - Suppress standard doxygen output (default enabled). * MIP_DISABLE_CPP - Ignores .hpp/.cpp files during the build and does not add them to the project. * BUILD_PACKAGE - Adds a `package` target to the project that will build a `.deb`, `.rpm`, or `.7z` file containing the library +* MIP_TIMESTAMP_TYPE - Overrides the default timestamp type. See the timestamps section below. ### Compilation with CMake @@ -121,11 +122,11 @@ The second, `mip_interface_send_to_device()`, must pass the provided data bytes See https://lord-microstrain.github.io/mip_sdk_documentation/latest/mip_interface.html for details on how to implement these functions. #### C++ -For C++ applications, these functions are implemented by the `MipDeviceInterface` class, which takes a `Connection` object responsible +For C++ applications, these functions are implemented by the `DeviceInterface` class, which takes a `Connection` object responsible for reading and writing to the device. Create a class derived from `Connection` and implement the pure virtual `recvFromDevice` and `sendToDevice` methods. -If you do not wish to use the `MipDeviceInterface` class, do not compile the corresponding source file and create the +If you do not wish to use the `DeviceInterface` class, do not compile the corresponding source file and create the C functions yourself. Declare them functions as `extern "C"` to avoid linking problems between the C and C++ code. ### Command Results (mip_cmd_result / MipCmdResult) @@ -142,6 +143,7 @@ Command results are divided into two categories: ### Timestamps and Timeouts +#### Timestamp type Timestamps (`timestamp_type` / `Timestamp`) represent the local time when data was received or a packet was parsed. These timestamps are used to implement command timeouts and provide the user with an approximate timestamp of received data. It is not intended to be a precise timestamp or used for synchronization, and it generally cannot be used instead of the timestamps from the connected MIP device. @@ -149,17 +151,24 @@ In particular, if you limit the maximum number of packets processed per `update` Because different applications may keep track of time differently (especially on embedded platforms), it is up to the user to provide the current time whenever data is received from the device. On a PC, this might come from the poxis `time()` function or from the -`std::chrono` library. On ARM systems, it is often derived from the Systick timer. +`std::chrono` library. On ARM systems, it is often derived from the Systick timer. It should be a monotonically increasing value; +jumps backwards in time will cause problems. -By default, timestamps are `typedef`'d to `uint32_t` and are typically in milliseconds. The value is allowed to wrap around as long -as the time between wraparounds is longer than twice the longest timeout needed. If higher precision is needed or wraparound can't -be tolerated by your application, define it to `uint64_t` instead. +By default, timestamps are `typedef`'d to `uint64_t`. Typically timestamps are in milliseconds. Embedded systems may wish to use +`uint32_t` or even `uint16_t` instead. The value is allowed to wrap around as long as the time between wraparounds is longer than +twice the longest timeout needed. If higher precision is needed or wraparound can't be tolerated by your application, define it to +`uint64_t`. It must be a standard unsigned integer type. + +#### Command Timeouts Timeouts for commands are broken down into two parts. * A "base reply timeout" applies to all commands. This is useful to compensate for communication latency, such as over a TCP socket. * "Additional time" which applies per command, because some commands may take longer to complete. -Currently, only the C++ api offers a way to set the additional time parameter. +Currently, only the C++ api offers a way to set the additional time parameter, and only when using the `runCommand` function taking +the command structure and the `additionalTime` parameter. + +The `timeout_type` / `Timeout` typedef is an alias to the timestamp type. ### C and C++ APIs From dc6b9cf20ee335f380bf41bf708ad83206664ec3 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Tue, 13 Sep 2022 12:02:38 -0400 Subject: [PATCH 002/252] Auto links docs to releases (#19) * Auto links docs to releases * Escapes backslash because groovy... --- CMakeLists.txt | 2 ++ Jenkinsfile | 27 +++++++++++++++++++++++++-- README.md | 4 ++-- 3 files changed, 29 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 07e8578c7..b279966d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -231,6 +231,8 @@ if(BUILD_DOCUMENTATION) message(FATAL_ERROR "Doxygen is required to build documentation.") endif() + set(DOXYGEN_PROJECT_NUMBER "${MIP_GIT_VERSION}") + set(DOXYGEN_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/documentation") set(DOXYGEN_IMAGE_PATH "${CMAKE_CURRENT_LIST_DIR}/docs") diff --git a/Jenkinsfile b/Jenkinsfile index f3b6b125f..671a0bba9 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -99,6 +99,14 @@ pipeline { repo="LORD-MicroStrain/libmip" archive_dir="${WORKSPACE}/../builds/${BUILD_NUMBER}/archive/" artifacts=$(find "${archive_dir}" -type f) + + # Generate a release notes file + documentation_link="https://lord-microstrain.github.io/mip_sdk_documentation/${release_name}" + release_notes_file="/tmp/mip-sdk-release-notes-${release_name}.md" + echo "## Useful Links" > ${release_notes_file} + echo "* [Documentation](${documentation_link})" >> ${release_notes_file} + + # Deploy the artifacts to Github gh release delete \ -y \ -R "${repo}" \ @@ -108,7 +116,9 @@ pipeline { --title "${release_name}" \ --target "${BRANCH_NAME}" \ --generate-notes \ + --notes-file "${release_notes_file}" \ "${release_name}" ${artifacts} + rm "${release_notes_file}" # Commit the documentation to the github pages branch export GIT_ASKPASS="${HOME}/.git-askpass" @@ -119,6 +129,9 @@ pipeline { mkdir -p "${docs_dir}" pushd "${docs_dir}" unzip "${docs_zip}" -d "${docs_dir}" + if ! grep -q -E "^\\* \\[${release_name}\\]\\(.*\\)$" "${docs_dir}/README.md"; then + echo "* [${release_name}](${documentation_link})" >> "${docs_dir}/README.md" + fi git add --all git commit -m "Adds documentation for ${release_name}" git push origin main @@ -135,8 +148,14 @@ pipeline { archive_dir="${WORKSPACE}/../builds/${BUILD_NUMBER}/archive/" artifacts=$(find "${archive_dir}" -type f) if git describe --exact-match --tags HEAD &> /dev/null; then - # Deploy the artifacts to Github + # Generate a release notes file tag=$(git describe --exact-match --tags HEAD) + documentation_link="https://lord-microstrain.github.io/mip_sdk_documentation/${tag}" + release_notes_file="/tmp/mip-sdk-release-notes-${tag}.md" + echo "## Useful Links" > ${release_notes_file} + echo "* [Documentation](${documentation_link})" >> ${release_notes_file} + + # Deploy the artifacts to Github gh release delete \ -y \ -R "${repo}" \ @@ -145,8 +164,9 @@ pipeline { -R "${repo}" \ --title "${tag}" \ --target "${BRANCH_NAME}" \ - --notes "" \ + --notes-file "${release_notes_file}" \ "${tag}" ${artifacts} + rm "${release_notes_file}" # Commit the documentation to the github pages branch export GIT_ASKPASS="${HOME}/.git-askpass" @@ -157,6 +177,9 @@ pipeline { mkdir -p "${docs_dir}" pushd "${docs_dir}" unzip "${docs_zip}" -d "${docs_dir}" + if ! grep -q -E "^\\* \\[${tag}\\]\\(.*\\)$" "${docs_dir}/README.md"; then + echo "* [${tag}](${documentation_link})" >> "${docs_dir}/README.md" + fi git add --all git commit -m "Adds documentation for ${tag}" git push origin main diff --git a/README.md b/README.md index 75c391f3c..c35ed8858 100644 --- a/README.md +++ b/README.md @@ -43,7 +43,7 @@ The examples take two parameters for the device connection: Documentation ------------- -https://lord-microstrain.github.io/mip_sdk_documentation/latest/index.html +Documentation for all released versions can be found [here](https://lord-microstrain.github.io/mip_sdk_documentation) Communications Interfaces @@ -119,7 +119,7 @@ a serial port or TCP socket. The second, `mip_interface_send_to_device()`, must pass the provided data bytes directly to the connected MIP device. -See https://lord-microstrain.github.io/mip_sdk_documentation/latest/mip_interface.html for details on how to implement these functions. +See [`mip_interface`](https://lord-microstrain.github.io/mip_sdk_documentation/latest/mip_interface.html) for details on how to implement these functions. #### C++ For C++ applications, these functions are implemented by the `DeviceInterface` class, which takes a `Connection` object responsible From d5e244518804d92c68b7b9149ffa3a437dfcb085 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Tue, 13 Sep 2022 16:35:32 -0400 Subject: [PATCH 003/252] Release script (#23) * Moves release process to script * Checks the proper command to see if there are changes to git and adds some debug info * Makes git askpass script executable * moves askpass file to project directory since we can't execute it when it's in /tmp on Jenkins * Changes branch check to develop --- Jenkinsfile | 87 +++++------------------------------- scripts/release.sh | 109 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 120 insertions(+), 76 deletions(-) create mode 100755 scripts/release.sh diff --git a/Jenkinsfile b/Jenkinsfile index 671a0bba9..4b0051ceb 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -95,47 +95,14 @@ pipeline { node("linux-amd64") { withCredentials([string(credentialsId: 'MICROSTRAIN_BUILD_GH_TOKEN', variable: 'GH_TOKEN')]) { sh ''' - release_name="latest" - repo="LORD-MicroStrain/libmip" + # Release to github archive_dir="${WORKSPACE}/../builds/${BUILD_NUMBER}/archive/" - artifacts=$(find "${archive_dir}" -type f) - - # Generate a release notes file - documentation_link="https://lord-microstrain.github.io/mip_sdk_documentation/${release_name}" - release_notes_file="/tmp/mip-sdk-release-notes-${release_name}.md" - echo "## Useful Links" > ${release_notes_file} - echo "* [Documentation](${documentation_link})" >> ${release_notes_file} - - # Deploy the artifacts to Github - gh release delete \ - -y \ - -R "${repo}" \ - "${release_name}" || echo "No existing release named ${release_name}" - gh release create \ - -R "${repo}" \ - --title "${release_name}" \ + ./scripts/release.sh \ + --artifacts "$(find "${archive_dir}" -type f)" \ --target "${BRANCH_NAME}" \ - --generate-notes \ - --notes-file "${release_notes_file}" \ - "${release_name}" ${artifacts} - rm "${release_notes_file}" - - # Commit the documentation to the github pages branch - export GIT_ASKPASS="${HOME}/.git-askpass" - docs_zip=$(find "${archive_dir}" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq) - docs_dir="${WORKSPACE}/mip_sdk_documentation/${release_name}" - git clone -b "main" "https://github.com/LORD-MicroStrain/mip_sdk_documentation.git" mip_sdk_documentation - rm -rf "${docs_dir}" - mkdir -p "${docs_dir}" - pushd "${docs_dir}" - unzip "${docs_zip}" -d "${docs_dir}" - if ! grep -q -E "^\\* \\[${release_name}\\]\\(.*\\)$" "${docs_dir}/README.md"; then - echo "* [${release_name}](${documentation_link})" >> "${docs_dir}/README.md" - fi - git add --all - git commit -m "Adds documentation for ${release_name}" - git push origin main - popd + --release "latest" \ + --docs-zip "$(find "${archive_dir}" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq)" \ + --generate-notes ''' } } @@ -144,46 +111,14 @@ pipeline { withCredentials([string(credentialsId: 'MICROSTRAIN_BUILD_GH_TOKEN', variable: 'GH_TOKEN')]) { sh ''' # Release to the latest version if the master commit matches up with the commit of that version - repo="LORD-MicroStrain/libmip" archive_dir="${WORKSPACE}/../builds/${BUILD_NUMBER}/archive/" - artifacts=$(find "${archive_dir}" -type f) if git describe --exact-match --tags HEAD &> /dev/null; then - # Generate a release notes file - tag=$(git describe --exact-match --tags HEAD) - documentation_link="https://lord-microstrain.github.io/mip_sdk_documentation/${tag}" - release_notes_file="/tmp/mip-sdk-release-notes-${tag}.md" - echo "## Useful Links" > ${release_notes_file} - echo "* [Documentation](${documentation_link})" >> ${release_notes_file} - - # Deploy the artifacts to Github - gh release delete \ - -y \ - -R "${repo}" \ - "${tag}" || echo "No existing release named ${tag}" - gh release create \ - -R "${repo}" \ - --title "${tag}" \ + # Publish a release + ./scripts/release.sh \ + --artifacts "$(find "${archive_dir}" -type f)" \ --target "${BRANCH_NAME}" \ - --notes-file "${release_notes_file}" \ - "${tag}" ${artifacts} - rm "${release_notes_file}" - - # Commit the documentation to the github pages branch - export GIT_ASKPASS="${HOME}/.git-askpass" - docs_zip=$(find "${archive_dir}" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq) - docs_dir="${WORKSPACE}/mip_sdk_documentation/${tag}" - git clone -b "main" "https://github.com/LORD-MicroStrain/mip_sdk_documentation.git" mip_sdk_documentation - rm -rf "${docs_dir}" - mkdir -p "${docs_dir}" - pushd "${docs_dir}" - unzip "${docs_zip}" -d "${docs_dir}" - if ! grep -q -E "^\\* \\[${tag}\\]\\(.*\\)$" "${docs_dir}/README.md"; then - echo "* [${tag}](${documentation_link})" >> "${docs_dir}/README.md" - fi - git add --all - git commit -m "Adds documentation for ${tag}" - git push origin main - popd + --release "$(git describe --exact-match --tags HEAD)" \ + --docs-zip "$(find "${archive_dir}" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq)" else echo "Not releasing from ${BRANCH_NAME} since the current commit does not match the latest released version commit" fi diff --git a/scripts/release.sh b/scripts/release.sh new file mode 100755 index 000000000..3b24216dc --- /dev/null +++ b/scripts/release.sh @@ -0,0 +1,109 @@ +#!/bin/bash + +# Exit on error +set -ex + +# On Jenkins, log all commands +if [ "${ISHUDSONBUILD}" == "True" ]; then + set -x +fi + +# Get some arguments from the user +generate_notes_flag="" +while [[ $# -gt 0 ]]; do + case $1 in + --artifacts) + artifacts="$2" + shift # past argument + shift # past value + ;; + --docs-zip) + docs_zip="$2" + shift # past argument + shift # past value + ;; + --target) + target="$2" + shift # past argument + shift # past value + ;; + --release) + release_name="$2" + shift # past argument + shift # past value + ;; + --generate-notes) + generate_notes_flag="--generate-notes" + shift # past argument + ;; + *) + shift # past argument + ;; + esac +done +if [ -z "${artifacts}" ] || [ -z "${docs_zip}" ] || [ -z "${release_name}" ] || [ -z "${target}" ]; then + echo "Script must be called with --target, --docs-zip, --artifacts and --release" + exit 1 +fi + +# Some constants and other important variables +repo="LORD-MicroStrain/mip_sdk" +script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" &> /dev/null && pwd)" +project_dir="${script_dir}/.." +tmp_dir="/tmp" +docs_dir="${tmp_dir}/mip_sdk_documentation" +docs_release_dir="${docs_dir}/${release_name}" + +# Find the commit that this project is built on +pushd "${project_dir}" +mip_sdk_commit="$(git rev-parse HEAD)" +popd + +# Generate a release notes file +documentation_link="https://lord-microstrain.github.io/mip_sdk_documentation/${release_name}" +release_notes_file="${tmp_dir}/mip-sdk-release-notes-${release_name}.md" +echo "## Useful Links" > ${release_notes_file} +echo "* [Documentation](${documentation_link})" >> ${release_notes_file} + +# Deploy the artifacts to Github +gh release delete \ + -y \ + -R "${repo}" \ + "${release_name}" || echo "No existing release named ${release_name}. Creating now..." +gh release create \ + -R "${repo}" \ + --title "${release_name}" \ + --target "${target}" \ + ${generate_notes_flag} \ + --notes-file "${release_notes_file}" \ + "${release_name}" ${artifacts} +rm -f "${release_notes_file}" + +# Commit the documentation to the github pages branch +rm -rf "${docs_dir}" +git clone -b "main" "https://github.com/LORD-MicroStrain/mip_sdk_documentation.git" "${docs_dir}" +rm -rf "${docs_release_dir}" +mkdir -p "${docs_release_dir}" +pushd "${docs_release_dir}" +unzip "${docs_zip}" -d "${docs_release_dir}" + +# If the tag is not already in the readme, add it +if ! grep -q -E "^\* \[${release_name}\]\(.*\)$" "${docs_dir}/README.md"; then + echo "* [${release_name}](${documentation_link})" >> "${docs_dir}/README.md" +fi + +# Only commit if there are changes +if ! git diff-index --quiet HEAD --; then + git add --all + git commit -m "Adds/updates documentation for release ${release_name} at ${repo}@${mip_sdk_commit}." + + # Set up the auth for github assuming that a valid token is in the environment at "GH_TOKEN" + git_askpass_file="${project_dir}/.mip-sdk-git-askpass" + echo 'echo ${GH_TOKEN}' > "${git_askpass_file}" + chmod 700 "${git_askpass_file}" + GIT_ASKPASS="${git_askpass_file}" git push origin main + rm "${git_askpass_file}" +else + echo "No changes to commit to documentation" +fi +popd \ No newline at end of file From 3329bf52e7f6ef2a540d36f832815c22cfb9da68 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Fri, 16 Sep 2022 16:50:13 -0400 Subject: [PATCH 004/252] Adds version header (#21) * Adds version header * Updates based on pull request feedback --- .gitignore | 2 ++ CMakeLists.txt | 18 +++++++++++++++--- cmake/mip_version.h.in | 10 ++++++++++ src/mip/mip_all.h | 2 ++ src/mip/mip_all.hpp | 3 ++- 5 files changed, 31 insertions(+), 4 deletions(-) create mode 100644 cmake/mip_version.h.in diff --git a/.gitignore b/.gitignore index f5ade5226..929cc6600 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ build-doc/ .vscode/ int/ .idea/ + +src/mip/mip_version.h \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index b279966d2..fcde1f1af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ project( set(SRC_DIR "${CMAKE_CURRENT_LIST_DIR}/src") set(EXT_DIR "${CMAKE_CURRENT_LIST_DIR}/ext") +set(MIP_CMAKE_DIR "${CMAKE_CURRENT_LIST_DIR}/cmake") include_directories( "${SRC_DIR}" @@ -53,6 +54,18 @@ else() endif() endif() +# Massage the version number a little so we can use it in a couple places +string(REGEX REPLACE "^v?([0-9]+)\\.([0-9]+)\\.([0-9]+).*" "\\1.\\2.\\3" MIP_GIT_VERSION_CLEAN ${MIP_GIT_VERSION}) +string(REPLACE "." ";" MIP_GIT_VERSION_LIST ${MIP_GIT_VERSION_CLEAN}) +list(GET MIP_GIT_VERSION_LIST 0 MIP_GIT_VERSION_MAJOR) +list(GET MIP_GIT_VERSION_LIST 1 MIP_GIT_VERSION_MINOR) +list(GET MIP_GIT_VERSION_LIST 2 MIP_GIT_VERSION_PATCH) + +# Generate the version header file +set(VERSION_IN_FILE "${MIP_CMAKE_DIR}/mip_version.h.in") +set(VERSION_OUT_FILE "${MIP_DIR}/mip_version.h") +configure_file(${VERSION_IN_FILE} ${VERSION_OUT_FILE}) + # # Build options # @@ -82,6 +95,7 @@ set(UTILS_SOURCES # set(MIP_SOURCES + "${VERSION_OUT_FILE}" "${MIP_DIR}/mip_cmdqueue.c" "${MIP_DIR}/mip_cmdqueue.h" "${MIP_DIR}/mip_dispatch.c" @@ -402,9 +416,7 @@ if(BUILD_PACKAGE) set(CPACK_PACKAGE_CONTACT "Rob Fisher ") set(CPACK_COMPONENTS_ALL "mip") - # Massage the version number a little so we can set the proper version - string(REGEX REPLACE "^v([0-9]+)\\.([0-9]+)\\.([0-9]+).*" "\\1.\\2.\\3" MIP_GIT_VERSION_CPACK ${MIP_GIT_VERSION}) - set(CPACK_PACKAGE_VERSION ${MIP_GIT_VERSION_CPACK}) + set(CPACK_PACKAGE_VERSION ${MIP_GIT_VERSION_CLEAN}) # Shared configuration set(MIP_FILE_NAME_PREFIX "mipsdk_${MIP_GIT_VERSION}_${MIP_ARCH}") diff --git a/cmake/mip_version.h.in b/cmake/mip_version.h.in new file mode 100644 index 000000000..95cdf4df6 --- /dev/null +++ b/cmake/mip_version.h.in @@ -0,0 +1,10 @@ +#pragma once + +// Full version of the MIP SDK including a git commit hash if this build was not built on a tag +#define MIP_SDK_VERSION_FULL "${MIP_GIT_VERSION}" + +// Semantic version information of the MIP SDK +#define MIP_SDK_VERSION "${MIP_GIT_VERSION_CLEAN}" +#define MIP_SDK_VERSION_MAJOR ${MIP_GIT_VERSION_MAJOR} +#define MIP_SDK_VERSION_MINOR ${MIP_GIT_VERSION_MINOR} +#define MIP_SDK_VERSION_PATCH ${MIP_GIT_VERSION_PATCH} \ No newline at end of file diff --git a/src/mip/mip_all.h b/src/mip/mip_all.h index 5b14d8c26..f6274a795 100644 --- a/src/mip/mip_all.h +++ b/src/mip/mip_all.h @@ -28,3 +28,5 @@ #include "definitions/data_gnss.h" #include "definitions/data_filter.h" +//SDK version +#include "mip_version.h" \ No newline at end of file diff --git a/src/mip/mip_all.hpp b/src/mip/mip_all.hpp index 5dc37a50d..90b7a6885 100644 --- a/src/mip/mip_all.hpp +++ b/src/mip/mip_all.hpp @@ -32,4 +32,5 @@ #include "mip.hpp" #include "mip_device.hpp" - +//SDK version +#include "mip_version.h" From 3b8908448f30608fba547b03b82e34ad8475dbb7 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Mon, 19 Sep 2022 17:51:35 -0400 Subject: [PATCH 005/252] Fixes type in intermediary callback for member function version of registerPacketCallback (#25) --- src/mip/mip_device.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 956a5359b..cd5068867 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -360,10 +360,10 @@ void DeviceInterface::registerPacketCallback(C::mip_dispatch_handler& handler, u template void DeviceInterface::registerPacketCallback(C::mip_dispatch_handler& handler, uint8_t descriptorSet, bool afterFields, Object* object) { - auto callback = [](void* pointer, const Packet& packet, Timestamp timestamp) + auto callback = [](void* pointer, const mip::C::mip_packet* packet, Timestamp timestamp) { Object* obj = static_cast(pointer); - (obj->*Callback)(Packet(packet), timestamp); + (obj->*Callback)(Packet(*packet), timestamp); }; registerPacketCallback(handler, descriptorSet, afterFields, callback, object); From edd53748ddf37d3020b0572bf9b98e5a12a3da12 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Wed, 21 Sep 2022 16:48:50 -0400 Subject: [PATCH 006/252] Adds descriptor set as a parameter to the callback for registerDataCallback (#26) * Adds descriptor set as a parameter to the callback for registerDataCallback * Updates based on PR feedback --- src/mip/mip_device.hpp | 127 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 126 insertions(+), 1 deletion(-) diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index cd5068867..a972abf80 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -229,9 +229,15 @@ class DeviceInterface : public C::mip_interface template void registerDataCallback(C::mip_dispatch_handler& handler, void* userData=nullptr, uint8_t descriptorSet=DataField::DESCRIPTOR_SET); + template + void registerDataCallback(C::mip_dispatch_handler& handler, void* userData=nullptr, uint8_t descriptorSet=DataField::DESCRIPTOR_SET); + template void registerDataCallback(C::mip_dispatch_handler& handler, Object* object, uint8_t descriptorSet=DataField::DESCRIPTOR_SET); + template + void registerDataCallback(C::mip_dispatch_handler& handler, Object* object, uint8_t descriptorSet=DataField::DESCRIPTOR_SET); + template void registerExtractor(C::mip_dispatch_handler& handler, DataField* field, uint8_t descriptorSet=DataField::DESCRIPTOR_SET); @@ -454,7 +460,6 @@ void DeviceInterface::registerFieldCallback(C::mip_dispatch_handler& handler, ui registerFieldCallback(handler, descriptorSet, fieldDescriptor, callback, object); } - //////////////////////////////////////////////////////////////////////////////// ///@brief Registers a data callback (free function version). /// @@ -514,6 +519,65 @@ void DeviceInterface::registerDataCallback(C::mip_dispatch_handler& handler, voi registerFieldCallback(handler, descriptorSet, DataField::FIELD_DESCRIPTOR, callback, userData); } +//////////////////////////////////////////////////////////////////////////////// +///@brief Registers a data callback (free function version). +/// +///@tparam Callback A pointer to the function to call. This must be a constant +/// function pointer. +/// +///@param handler +/// This must exist as long as the hander remains registered. +/// +///@param userData +/// Optional data to pass to the callback function. +/// +///@param descriptorSet +/// If specified, overrides the descriptor set. Intended to be used with +/// with shared data quantities. +/// +/// Example usage: +///@code{.cpp} +/// void handle_packet(void* context, uint8_t descriptorSet, const Packet& packet, Timestamp timestamp) +/// { +/// // Use the packet data +/// } +/// +/// DeviceInterface device; +/// DispatchHandler handler; +/// +/// void setup() +/// { +/// // Set up device... +/// +/// device.registerDataCallback<&handle_packet>(handler, descriptorSet, nullptr); +/// } +/// +///@endcode +/// +template +void DeviceInterface::registerDataCallback(C::mip_dispatch_handler& handler, void* userData, uint8_t descriptorSet) +{ + assert(descriptorSet != 0x00); + if(descriptorSet == 0x00) + return; + + assert(descriptorSet != 0xFF); // Descriptor set must be specified for shared data. + if(descriptorSet == 0xFF) + return; + + auto callback = [](void* context, const C::mip_field* field, Timestamp timestamp) + { + DataField data; + + bool ok = Field(*field).extract(data); + assert(ok); (void)ok; + + Callback(context, data, mip_field_descriptor_set(field), timestamp); + }; + + registerFieldCallback(handler, descriptorSet, DataField::FIELD_DESCRIPTOR, callback, userData); +} + //////////////////////////////////////////////////////////////////////////////// ///@brief Registers a data callback (member function version). /// @@ -575,6 +639,67 @@ void DeviceInterface::registerDataCallback(C::mip_dispatch_handler& handler, Obj registerFieldCallback(handler, descriptorSet, DataField::FIELD_DESCRIPTOR, callback, object); } +//////////////////////////////////////////////////////////////////////////////// +///@brief Registers a data callback (member function version). +/// +///@tparam Callback A pointer to the function to call. This must be a constant +/// member function pointer. +/// +///@param handler +/// This must exist as long as the hander remains registered. +/// +///@param object +/// A pointer to the object. The object must exist while the handler +/// remains registered. +/// +///@param descriptorSet +/// If specified, overrides the descriptor set. Intended to be used with +/// with shared data quantities. +/// +/// Example usage: +///@code{.cpp} +/// class MySystem +/// { +/// void handleAccel(const ScaledAccel& accel, uint8_t descriptorSet, Timestamp timestamp) +/// { +/// } +/// +/// void setup() +/// { +/// // setup device... +/// device.registerDataHandler(accelHandler, this); +/// } +/// +/// DeviceInterface device; +/// DispatchHandler accelHandler; +/// }; +///@endcode +/// +template +void DeviceInterface::registerDataCallback(C::mip_dispatch_handler& handler, Object* object, uint8_t descriptorSet) +{ + assert(descriptorSet != 0x00); + if(descriptorSet == 0x00) + return; + + assert(descriptorSet != 0xFF); // Descriptor set must be specified for shared data. + if(descriptorSet == 0xFF) + return; + + auto callback = [](void* pointer, const C::mip_field* field, Timestamp timestamp) + { + DataField data; + + bool ok = Field(*field).extract(data); + assert(ok); (void)ok; + + Object* obj = static_cast(pointer); + (obj->*Callback)(data, mip_field_descriptor_set(field), timestamp); + }; + + registerFieldCallback(handler, descriptorSet, DataField::FIELD_DESCRIPTOR, callback, object); +} + template void DeviceInterface::registerExtractor(C::mip_dispatch_handler& handler, DataField* field, uint8_t descriptorSet) From d4a6dc0406cd9bcfe0e0d3b36647240dafe0557e Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 22 Sep 2022 14:52:17 -0400 Subject: [PATCH 007/252] Generate MIP definitions from branches/dev@53572. --- src/mip/definitions/commands_3dm.c | 202 ++++++++++++++++++++++-- src/mip/definitions/commands_3dm.cpp | 191 ++++++++++++++++++++-- src/mip/definitions/commands_3dm.h | 146 +++++++++++------ src/mip/definitions/commands_3dm.hpp | 152 +++++++++++++----- src/mip/definitions/commands_base.h | 2 +- src/mip/definitions/commands_base.hpp | 2 +- src/mip/definitions/commands_filter.h | 15 +- src/mip/definitions/commands_filter.hpp | 15 +- src/mip/definitions/commands_gnss.c | 29 ---- src/mip/definitions/commands_gnss.cpp | 28 ---- src/mip/definitions/commands_gnss.h | 24 +-- src/mip/definitions/commands_gnss.hpp | 28 +--- src/mip/definitions/commands_rtk.c | 10 +- src/mip/definitions/commands_rtk.cpp | 10 +- src/mip/definitions/commands_rtk.h | 6 +- src/mip/definitions/commands_rtk.hpp | 6 +- src/mip/definitions/data_filter.h | 6 +- src/mip/definitions/data_filter.hpp | 6 +- src/mip/definitions/data_gnss.c | 163 +++++++++++++++++++ src/mip/definitions/data_gnss.cpp | 143 +++++++++++++++++ src/mip/definitions/data_gnss.h | 78 +++++++-- src/mip/definitions/data_gnss.hpp | 92 +++++++++-- src/mip/definitions/data_sensor.h | 8 +- src/mip/definitions/data_sensor.hpp | 8 +- 24 files changed, 1087 insertions(+), 283 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index 0748a27e2..c48418d79 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -1678,7 +1678,7 @@ mip_cmd_result mip_3dm_read_gnss_time_assistance(struct mip_interface* device, d } return result; } -void insert_mip_3dm_adv_lowpass_filter_command(mip_serializer* serializer, const mip_3dm_adv_lowpass_filter_command* self) +void insert_mip_3dm_imu_lowpass_filter_command(mip_serializer* serializer, const mip_3dm_imu_lowpass_filter_command* self) { insert_mip_function_selector(serializer, self->function); @@ -1696,7 +1696,7 @@ void insert_mip_3dm_adv_lowpass_filter_command(mip_serializer* serializer, const } } -void extract_mip_3dm_adv_lowpass_filter_command(mip_serializer* serializer, mip_3dm_adv_lowpass_filter_command* self) +void extract_mip_3dm_imu_lowpass_filter_command(mip_serializer* serializer, mip_3dm_imu_lowpass_filter_command* self) { extract_mip_function_selector(serializer, &self->function); @@ -1715,7 +1715,7 @@ void extract_mip_3dm_adv_lowpass_filter_command(mip_serializer* serializer, mip_ } } -void insert_mip_3dm_adv_lowpass_filter_response(mip_serializer* serializer, const mip_3dm_adv_lowpass_filter_response* self) +void insert_mip_3dm_imu_lowpass_filter_response(mip_serializer* serializer, const mip_3dm_imu_lowpass_filter_response* self) { insert_u8(serializer, self->target_descriptor); @@ -1728,7 +1728,7 @@ void insert_mip_3dm_adv_lowpass_filter_response(mip_serializer* serializer, cons insert_u8(serializer, self->reserved); } -void extract_mip_3dm_adv_lowpass_filter_response(mip_serializer* serializer, mip_3dm_adv_lowpass_filter_response* self) +void extract_mip_3dm_imu_lowpass_filter_response(mip_serializer* serializer, mip_3dm_imu_lowpass_filter_response* self) { extract_u8(serializer, &self->target_descriptor); @@ -1742,7 +1742,7 @@ void extract_mip_3dm_adv_lowpass_filter_response(mip_serializer* serializer, mip } -mip_cmd_result mip_3dm_write_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) +mip_cmd_result mip_3dm_write_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1762,9 +1762,9 @@ mip_cmd_result mip_3dm_write_adv_lowpass_filter(struct mip_interface* device, ui assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool* enable_out, bool* manual_out, uint16_t* frequency_out, uint8_t* reserved_out) +mip_cmd_result mip_3dm_read_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool* enable_out, bool* manual_out, uint16_t* frequency_out, uint8_t* reserved_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1777,7 +1777,7 @@ mip_cmd_result mip_3dm_read_adv_lowpass_filter(struct mip_interface* device, uin assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_ADVANCED_DATA_FILTER, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_ADVANCED_DATA_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1803,7 +1803,7 @@ mip_cmd_result mip_3dm_read_adv_lowpass_filter(struct mip_interface* device, uin } return result; } -mip_cmd_result mip_3dm_save_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor) +mip_cmd_result mip_3dm_save_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1815,9 +1815,9 @@ mip_cmd_result mip_3dm_save_adv_lowpass_filter(struct mip_interface* device, uin assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_load_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor) +mip_cmd_result mip_3dm_load_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1829,9 +1829,9 @@ mip_cmd_result mip_3dm_load_adv_lowpass_filter(struct mip_interface* device, uin assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_default_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor) +mip_cmd_result mip_3dm_default_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1843,7 +1843,7 @@ mip_cmd_result mip_3dm_default_adv_lowpass_filter(struct mip_interface* device, assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert_mip_3dm_pps_source_command(mip_serializer* serializer, const mip_3dm_pps_source_command* self) { @@ -4653,6 +4653,180 @@ mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mi } return result; } +void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + insert_u8(serializer, self->desc_set); + + insert_u8(serializer, self->field_desc); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_bool(serializer, self->enable); + + insert_bool(serializer, self->manual); + + insert_float(serializer, self->frequency); + + } +} +void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + extract_u8(serializer, &self->desc_set); + + extract_u8(serializer, &self->field_desc); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_bool(serializer, &self->enable); + + extract_bool(serializer, &self->manual); + + extract_float(serializer, &self->frequency); + + } +} + +void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_response* self) +{ + insert_u8(serializer, self->desc_set); + + insert_u8(serializer, self->field_desc); + + insert_bool(serializer, self->enable); + + insert_bool(serializer, self->manual); + + insert_float(serializer, self->frequency); + +} +void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_response* self) +{ + extract_u8(serializer, &self->desc_set); + + extract_u8(serializer, &self->field_desc); + + extract_bool(serializer, &self->enable); + + extract_bool(serializer, &self->manual); + + extract_float(serializer, &self->frequency); + +} + +mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, desc_set); + + insert_u8(&serializer, field_desc); + + insert_bool(&serializer, enable); + + insert_bool(&serializer, manual); + + insert_float(&serializer, frequency); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + insert_u8(&serializer, desc_set); + + insert_u8(&serializer, field_desc); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_LOWPASS_FILTER, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + extract_u8(&deserializer, &desc_set); + + extract_u8(&deserializer, &field_desc); + + assert(enable_out); + extract_bool(&deserializer, enable_out); + + assert(manual_out); + extract_bool(&deserializer, manual_out); + + assert(frequency_out); + extract_float(&deserializer, frequency_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + insert_u8(&serializer, desc_set); + + insert_u8(&serializer, field_desc); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + insert_u8(&serializer, desc_set); + + insert_u8(&serializer, field_desc); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_default_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + insert_u8(&serializer, desc_set); + + insert_u8(&serializer, field_desc); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index fb98d603c..423cb121f 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -1518,7 +1518,7 @@ CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint1 } return result; } -void insert(Serializer& serializer, const AdvLowpassFilter& self) +void insert(Serializer& serializer, const ImuLowpassFilter& self) { insert(serializer, self.function); @@ -1536,7 +1536,7 @@ void insert(Serializer& serializer, const AdvLowpassFilter& self) } } -void extract(Serializer& serializer, AdvLowpassFilter& self) +void extract(Serializer& serializer, ImuLowpassFilter& self) { extract(serializer, self.function); @@ -1555,7 +1555,7 @@ void extract(Serializer& serializer, AdvLowpassFilter& self) } } -void insert(Serializer& serializer, const AdvLowpassFilter::Response& self) +void insert(Serializer& serializer, const ImuLowpassFilter::Response& self) { insert(serializer, self.target_descriptor); @@ -1568,7 +1568,7 @@ void insert(Serializer& serializer, const AdvLowpassFilter::Response& self) insert(serializer, self.reserved); } -void extract(Serializer& serializer, AdvLowpassFilter::Response& self) +void extract(Serializer& serializer, ImuLowpassFilter::Response& self) { extract(serializer, self.target_descriptor); @@ -1582,7 +1582,7 @@ void extract(Serializer& serializer, AdvLowpassFilter::Response& self) } -CmdResult writeAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) +CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1600,9 +1600,9 @@ CmdResult writeAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescript assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) +CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1613,7 +1613,7 @@ CmdResult readAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescripto assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1638,7 +1638,7 @@ CmdResult readAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescripto } return result; } -CmdResult saveAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1648,9 +1648,9 @@ CmdResult saveAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescripto assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1660,9 +1660,9 @@ CmdResult loadAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescripto assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1672,7 +1672,7 @@ CmdResult defaultAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescri assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADVANCED_DATA_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert(Serializer& serializer, const PpsSource& self) { @@ -4130,6 +4130,169 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso } return result; } +void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self) +{ + insert(serializer, self.function); + + insert(serializer, self.desc_set); + + insert(serializer, self.field_desc); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.manual); + + insert(serializer, self.frequency); + + } +} +void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self) +{ + extract(serializer, self.function); + + extract(serializer, self.desc_set); + + extract(serializer, self.field_desc); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.manual); + + extract(serializer, self.frequency); + + } +} + +void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self) +{ + insert(serializer, self.desc_set); + + insert(serializer, self.field_desc); + + insert(serializer, self.enable); + + insert(serializer, self.manual); + + insert(serializer, self.frequency); + +} +void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self) +{ + extract(serializer, self.desc_set); + + extract(serializer, self.field_desc); + + extract(serializer, self.enable); + + extract(serializer, self.manual); + + extract(serializer, self.frequency); + +} + +CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, descSet); + + insert(serializer, fieldDesc); + + insert(serializer, enable); + + insert(serializer, manual); + + insert(serializer, frequency); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + insert(serializer, descSet); + + insert(serializer, fieldDesc); + + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LOWPASS_FILTER, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + extract(deserializer, descSet); + + extract(deserializer, fieldDesc); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(manualOut); + extract(deserializer, *manualOut); + + assert(frequencyOut); + extract(deserializer, *frequencyOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + insert(serializer, descSet); + + insert(serializer, fieldDesc); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + insert(serializer, descSet); + + insert(serializer, fieldDesc); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + insert(serializer, descSet); + + insert(serializer, fieldDesc); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); +} } // namespace commands_3dm } // namespace mip diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index afec6326c..b4da6c501 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -79,10 +79,11 @@ enum MIP_CMD_DESC_3DM_GPIO_CONFIG = 0x41, MIP_CMD_DESC_3DM_GPIO_STATE = 0x42, MIP_CMD_DESC_3DM_ODOMETER_CONFIG = 0x43, - MIP_CMD_DESC_3DM_ADVANCED_DATA_FILTER = 0x50, + MIP_CMD_DESC_3DM_IMU_LOWPASS_FILTER = 0x50, MIP_CMD_DESC_3DM_LEGACY_COMP_FILTER = 0x51, MIP_CMD_DESC_3DM_SENSOR_RANGE = 0x52, MIP_CMD_DESC_3DM_CALIBRATED_RANGES = 0x53, + MIP_CMD_DESC_3DM_LOWPASS_FILTER = 0x54, MIP_CMD_DESC_3DM_DATASTREAM_FORMAT = 0x60, MIP_CMD_DESC_3DM_DEVICE_POWER_STATE = 0x61, MIP_CMD_DESC_3DM_SAVE_RESTORE_GPS_SETTINGS = 0x62, @@ -136,6 +137,7 @@ enum MIP_REPLY_DESC_3DM_ODOMETER_CONFIG = 0xC3, MIP_REPLY_DESC_3DM_SENSOR_RANGE = 0xD2, MIP_REPLY_DESC_3DM_CALIBRATED_RANGES = 0xD3, + MIP_REPLY_DESC_3DM_LOWPASS_FILTER = 0xD4, }; //////////////////////////////////////////////////////////////////////////////// @@ -143,28 +145,29 @@ enum //////////////////////////////////////////////////////////////////////////////// typedef uint8_t mip_nmea_message_message_id; -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GGA = 1; ///< GPS System Fix Data -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GLL = 2; ///< Geographic Position Lat/Lon -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GSV = 3; ///< GNSS Satellites in View -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_RMC = 4; ///< Recommended Minimum Specific GNSS Data -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_VTG = 5; ///< Course over Ground -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_HDT = 6; ///< Heading, True -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_ZDA = 7; ///< Time & Date -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKA = 100; ///< Parker proprietary Euler angles -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKR = 101; ///< Parker proprietary Angular Rate/Acceleration +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GGA = 1; ///< GPS System Fix Data. Source can be the Filter or GNSS1/2 datasets. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GLL = 2; ///< Geographic Position Lat/Lon. Source can be the Filter or GNSS1/2 datasets. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GSV = 3; ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID is ignored (talker depends on the satellite). +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_RMC = 4; ///< Recommended Minimum Specific GNSS Data. Source can be the Filter or GNSS1/2 datasets. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_VTG = 5; ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_HDT = 6; ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_ZDA = 7; ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKA = 129; ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID is ignored. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKR = 130; ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID is ignored. typedef uint8_t mip_nmea_message_talker_id; -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GNSS = 1; ///< NMEA message will be produced with talker id "GN" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GPS = 2; ///< NMEA message will be produced with talker id "GP" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GALILEO = 3; ///< NMEA message will be produced with talker id "GA" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GLONASS = 4; ///< NMEA message will be produced with talker id "GL" +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_RESERVED = 0; ///< +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GNSS = 1; ///< NMEA message will be produced with talker id "GN" +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GPS = 2; ///< NMEA message will be produced with talker id "GP" +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GALILEO = 3; ///< NMEA message will be produced with talker id "GA" +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GLONASS = 4; ///< NMEA message will be produced with talker id "GL" struct mip_nmea_message { - mip_nmea_message_message_id message_id; ///< Message type (GGA, GLL, etc) - mip_nmea_message_talker_id talker_id; ///< Talker ID (GN, GP, etc) - uint8_t source_desc_set; ///< Data source descriptor set (Filter, GNSS, etc) - uint16_t decimation; ///< Decimation from the base rate of the source descriptor set. + mip_nmea_message_message_id message_id; ///< NMEA sentence type. + mip_nmea_message_talker_id talker_id; ///< NMEA talker ID. Ignored for proprietary sentences. + uint8_t source_desc_set; ///< Data descriptor set where the data will be sourced. Available options depend on the sentence. + uint16_t decimation; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. }; typedef struct mip_nmea_message mip_nmea_message; @@ -752,7 +755,7 @@ mip_cmd_result mip_3dm_default_datastream_control(struct mip_interface* device, typedef uint16_t mip_3dm_gnss_sbas_settings_command_sbasoptions; static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_NONE = 0x0000; -static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ENABLE_RANGING = 0x0001; ///< Use SBAS pseudoranges in position solution +static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ENABLE_RANGING = 0x0001; ///< Use SBAS pseudo-ranges in position solution static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ENABLE_CORRECTIONS = 0x0002; ///< Use SBAS differential corrections static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_APPLY_INTEGRITY = 0x0004; ///< Use SBAS integrity information. If enabled, only GPS satellites for which integrity information is available will be used. @@ -828,9 +831,11 @@ mip_cmd_result mip_3dm_read_gnss_time_assistance(struct mip_interface* device, d ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_3dm_adv_lowpass_filter (0x0C,0x50) Adv Lowpass Filter [C] +///@defgroup c_3dm_imu_lowpass_filter (0x0C,0x50) Imu Lowpass Filter [C] /// Advanced configuration for the IMU data quantity low-pass filters. /// +/// Deprecated, use the lowpass filter (0x0C,0x54) command instead. +/// /// The scaled data quantities are by default filtered through a single-pole IIR low-pass filter /// which is configured with a -3dB cutoff frequency of half the reporting frequency (set by /// decimation factor in the IMU Message Format command) to prevent aliasing on a per data @@ -839,14 +844,14 @@ mip_cmd_result mip_3dm_read_gnss_time_assistance(struct mip_interface* device, d /// complete bypass of the digital low-pass filter. /// /// Possible data descriptors: -/// 0x04 – Scaled accelerometer data -/// 0x05 – Scaled gyro data -/// 0x06 – Scaled magnetometer data (if applicable) -/// 0x17 – Scaled pressure data (if applicable) +/// 0x04 - Scaled accelerometer data +/// 0x05 - Scaled gyro data +/// 0x06 - Scaled magnetometer data (if applicable) +/// 0x17 - Scaled pressure data (if applicable) /// ///@{ -struct mip_3dm_adv_lowpass_filter_command +struct mip_3dm_imu_lowpass_filter_command { mip_function_selector function; uint8_t target_descriptor; ///< Field descriptor of filtered quantity within the Sensor data set. Supported values are accel (0x04), gyro (0x05), mag (0x06), and pressure (0x17), provided the data is supported by the device. Except with the READ function selector, this can be 0 to apply to all of the above quantities. @@ -856,11 +861,11 @@ struct mip_3dm_adv_lowpass_filter_command uint8_t reserved; ///< Reserved, set to 0x00. }; -typedef struct mip_3dm_adv_lowpass_filter_command mip_3dm_adv_lowpass_filter_command; -void insert_mip_3dm_adv_lowpass_filter_command(struct mip_serializer* serializer, const mip_3dm_adv_lowpass_filter_command* self); -void extract_mip_3dm_adv_lowpass_filter_command(struct mip_serializer* serializer, mip_3dm_adv_lowpass_filter_command* self); +typedef struct mip_3dm_imu_lowpass_filter_command mip_3dm_imu_lowpass_filter_command; +void insert_mip_3dm_imu_lowpass_filter_command(struct mip_serializer* serializer, const mip_3dm_imu_lowpass_filter_command* self); +void extract_mip_3dm_imu_lowpass_filter_command(struct mip_serializer* serializer, mip_3dm_imu_lowpass_filter_command* self); -struct mip_3dm_adv_lowpass_filter_response +struct mip_3dm_imu_lowpass_filter_response { uint8_t target_descriptor; bool enable; ///< True if the filter is currently enabled. @@ -869,15 +874,15 @@ struct mip_3dm_adv_lowpass_filter_response uint8_t reserved; ///< Reserved and must be ignored. }; -typedef struct mip_3dm_adv_lowpass_filter_response mip_3dm_adv_lowpass_filter_response; -void insert_mip_3dm_adv_lowpass_filter_response(struct mip_serializer* serializer, const mip_3dm_adv_lowpass_filter_response* self); -void extract_mip_3dm_adv_lowpass_filter_response(struct mip_serializer* serializer, mip_3dm_adv_lowpass_filter_response* self); +typedef struct mip_3dm_imu_lowpass_filter_response mip_3dm_imu_lowpass_filter_response; +void insert_mip_3dm_imu_lowpass_filter_response(struct mip_serializer* serializer, const mip_3dm_imu_lowpass_filter_response* self); +void extract_mip_3dm_imu_lowpass_filter_response(struct mip_serializer* serializer, mip_3dm_imu_lowpass_filter_response* self); -mip_cmd_result mip_3dm_write_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); -mip_cmd_result mip_3dm_read_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool* enable_out, bool* manual_out, uint16_t* frequency_out, uint8_t* reserved_out); -mip_cmd_result mip_3dm_save_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); -mip_cmd_result mip_3dm_load_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); -mip_cmd_result mip_3dm_default_adv_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); +mip_cmd_result mip_3dm_write_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); +mip_cmd_result mip_3dm_read_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor, bool* enable_out, bool* manual_out, uint16_t* frequency_out, uint8_t* reserved_out); +mip_cmd_result mip_3dm_save_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); +mip_cmd_result mip_3dm_load_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); +mip_cmd_result mip_3dm_default_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -968,9 +973,9 @@ static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BE typedef uint8_t mip_3dm_gpio_config_command_pin_mode; static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_NONE = 0x00; -static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_OPEN_DRAIN = 0x01; ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pullup resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. -static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLDOWN = 0x02; ///< The pin will have an internal pulldown resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pullup. See the device specifications for the resistance value. -static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLUP = 0x04; ///< The pin will have an internal pullup resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pulldown. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. +static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_OPEN_DRAIN = 0x01; ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pull-up resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. +static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLDOWN = 0x02; ///< The pin will have an internal pull-down resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pull-up. See the device specifications for the resistance value. +static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLUP = 0x04; ///< The pin will have an internal pull-up resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pull-down. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. struct mip_3dm_gpio_config_command { @@ -1029,7 +1034,7 @@ mip_cmd_result mip_3dm_default_gpio_config(struct mip_interface* device, uint8_t /// While the state of a pin can always be set, it will only have an observable effect if /// the pin is set to output mode. /// -/// This command does not support saving, loading, or reseting the state. Instead, use the +/// This command does not support saving, loading, or resetting the state. Instead, use the /// GPIO Configuration command, which allows the initial state to be configured. /// ///@{ @@ -1764,7 +1769,7 @@ mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_euler(struct mip_inter /// EQSTART p^{veh} = q^{-1} p^{sen} q EQEND
/// /// Where:
-/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion desrcribing the transformation.
+/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion describing the transformation.
/// EQSTART p^{sen} = (0, v^{sen}_x, v^{sen}_y, v^{sen}_z) EQEND and EQSTART v^{sen} EQEND is a 3-element vector expressed in the sensor body frame.
/// EQSTART p^{veh} = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) EQEND and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
/// @@ -1884,7 +1889,7 @@ mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_dcm(struct mip_interfa /// Configure the settings for the complementary filter which produces the following (0x80) descriptor set values: attitude matrix (0x80,09), quaternion (0x80,0A), and Euler angle (0x80,0C) outputs. /// /// The filter can be configured to correct for pitch and roll using the accelerometer (with the assumption that linear acceleration is minimal), -/// and to correct for heading using the magnetomer (with the assumption that the local magnetic field is dominated by the Earth's own magnetic field). +/// and to correct for heading using the magnetometer (with the assumption that the local magnetic field is dominated by the Earth's own magnetic field). /// Pitch/roll and heading corrections each have their own configurable time constants, with a valid range of 1-1000 seconds. The default time constant is 10 seconds. /// ///@{ @@ -1926,7 +1931,7 @@ mip_cmd_result mip_3dm_default_complementary_filter(struct mip_interface* device /// Changes the IMU sensor gain. /// /// This allows you to optimize the range to get the best accuracy and performance -/// while minimizing overrange events. +/// while minimizing over-range events. /// /// Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine /// the appropriate setting value for your application. Using values other than @@ -2004,6 +2009,59 @@ void extract_mip_3dm_calibrated_sensor_ranges_response(struct mip_serializer* se mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mip_sensor_range_type sensor, uint8_t* num_ranges_out, uint8_t num_ranges_out_max, mip_3dm_calibrated_sensor_ranges_command_entry* ranges_out); ///@} /// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_3dm_mip_cmd_3dm_lowpass_filter (0x0C,0x54) Mip Cmd 3Dm Lowpass Filter [C] +/// This command controls the low-pass anti-aliasing filter supported data quantities. +/// +/// See the device user manual for data quantities which support the anti-aliasing filter. +/// +/// If set to automatic mode, the frequency will track half of the transmission rate +/// of the target descriptor according to the configured message format (0x0C,0x0F). +/// For example, if scaled accel (0x80,0x04) is set to stream at 100 Hz, the filter would +/// be set to 50 Hz. Changing the message format to 200 Hz would automatically adjust the +/// filter to 100 Hz. +/// +/// For WRITE, SAVE, LOAD, and DEFAULT function selectors, the descriptor set and/or field descriptor +/// may be 0x00 to set, save, load, or reset the setting for all supported descriptors. The +/// field descriptor must be 0x00 if the descriptor set is 0x00. +/// +/// +///@{ + +struct mip_3dm_mip_cmd_3dm_lowpass_filter_command +{ + mip_function_selector function; + uint8_t desc_set; ///< Descriptor set of the quantity to be filtered. + uint8_t field_desc; ///< Field descriptor of the quantity to be filtered. + bool enable; ///< The filter will be enabled if this is true. + bool manual; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. + float frequency; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. + +}; +typedef struct mip_3dm_mip_cmd_3dm_lowpass_filter_command mip_3dm_mip_cmd_3dm_lowpass_filter_command; +void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(struct mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_command* self); +void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(struct mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_command* self); + +struct mip_3dm_mip_cmd_3dm_lowpass_filter_response +{ + uint8_t desc_set; ///< Descriptor set of the quantity to be filtered. + uint8_t field_desc; ///< Field descriptor of the quantity to be filtered. + bool enable; ///< The filter will be enabled if this is true. + bool manual; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. + float frequency; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. + +}; +typedef struct mip_3dm_mip_cmd_3dm_lowpass_filter_response mip_3dm_mip_cmd_3dm_lowpass_filter_response; +void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(struct mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_response* self); +void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(struct mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_response* self); + +mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency); +mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out); +mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_default_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +///@} +/// ///@} ///@} diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 9aea6c1fb..9b442f206 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -78,10 +78,11 @@ enum CMD_GPIO_CONFIG = 0x41, CMD_GPIO_STATE = 0x42, CMD_ODOMETER_CONFIG = 0x43, - CMD_ADVANCED_DATA_FILTER = 0x50, + CMD_IMU_LOWPASS_FILTER = 0x50, CMD_LEGACY_COMP_FILTER = 0x51, CMD_SENSOR_RANGE = 0x52, CMD_CALIBRATED_RANGES = 0x53, + CMD_LOWPASS_FILTER = 0x54, CMD_DATASTREAM_FORMAT = 0x60, CMD_DEVICE_POWER_STATE = 0x61, CMD_SAVE_RESTORE_GPS_SETTINGS = 0x62, @@ -135,6 +136,7 @@ enum REPLY_ODOMETER_CONFIG = 0xC3, REPLY_SENSOR_RANGE = 0xD2, REPLY_CALIBRATED_RANGES = 0xD3, + REPLY_LOWPASS_FILTER = 0xD4, }; //////////////////////////////////////////////////////////////////////////////// @@ -145,29 +147,30 @@ struct NmeaMessage { enum class MessageID : uint8_t { - GGA = 1, ///< GPS System Fix Data - GLL = 2, ///< Geographic Position Lat/Lon - GSV = 3, ///< GNSS Satellites in View - RMC = 4, ///< Recommended Minimum Specific GNSS Data - VTG = 5, ///< Course over Ground - HDT = 6, ///< Heading, True - ZDA = 7, ///< Time & Date - PRKA = 100, ///< Parker proprietary Euler angles - PRKR = 101, ///< Parker proprietary Angular Rate/Acceleration + GGA = 1, ///< GPS System Fix Data. Source can be the Filter or GNSS1/2 datasets. + GLL = 2, ///< Geographic Position Lat/Lon. Source can be the Filter or GNSS1/2 datasets. + GSV = 3, ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID is ignored (talker depends on the satellite). + RMC = 4, ///< Recommended Minimum Specific GNSS Data. Source can be the Filter or GNSS1/2 datasets. + VTG = 5, ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. + HDT = 6, ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. + ZDA = 7, ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. + PRKA = 129, ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID is ignored. + PRKR = 130, ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID is ignored. }; enum class TalkerID : uint8_t { - GNSS = 1, ///< NMEA message will be produced with talker id "GN" - GPS = 2, ///< NMEA message will be produced with talker id "GP" - GALILEO = 3, ///< NMEA message will be produced with talker id "GA" - GLONASS = 4, ///< NMEA message will be produced with talker id "GL" + RESERVED = 0, ///< + GNSS = 1, ///< NMEA message will be produced with talker id "GN" + GPS = 2, ///< NMEA message will be produced with talker id "GP" + GALILEO = 3, ///< NMEA message will be produced with talker id "GA" + GLONASS = 4, ///< NMEA message will be produced with talker id "GL" }; - MessageID message_id = static_cast(0); ///< Message type (GGA, GLL, etc) - TalkerID talker_id = static_cast(0); ///< Talker ID (GN, GP, etc) - uint8_t source_desc_set = 0; ///< Data source descriptor set (Filter, GNSS, etc) - uint16_t decimation = 0; ///< Decimation from the base rate of the source descriptor set. + MessageID message_id = static_cast(0); ///< NMEA sentence type. + TalkerID talker_id = static_cast(0); ///< NMEA talker ID. Ignored for proprietary sentences. + uint8_t source_desc_set = 0; ///< Data descriptor set where the data will be sourced. Available options depend on the sentence. + uint16_t decimation = 0; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. }; void insert(Serializer& serializer, const NmeaMessage& self); @@ -910,7 +913,7 @@ struct GnssSbasSettings enum _enumType : uint16_t { NONE = 0x0000, - ENABLE_RANGING = 0x0001, ///< Use SBAS pseudoranges in position solution + ENABLE_RANGING = 0x0001, ///< Use SBAS pseudo-ranges in position solution ENABLE_CORRECTIONS = 0x0002, ///< Use SBAS differential corrections APPLY_INTEGRITY = 0x0004, ///< Use SBAS integrity information. If enabled, only GPS satellites for which integrity information is available will be used. }; @@ -1003,9 +1006,11 @@ CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint1 ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_3dm_adv_lowpass_filter (0x0C,0x50) Adv Lowpass Filter [CPP] +///@defgroup cpp_3dm_imu_lowpass_filter (0x0C,0x50) Imu Lowpass Filter [CPP] /// Advanced configuration for the IMU data quantity low-pass filters. /// +/// Deprecated, use the lowpass filter (0x0C,0x54) command instead. +/// /// The scaled data quantities are by default filtered through a single-pole IIR low-pass filter /// which is configured with a -3dB cutoff frequency of half the reporting frequency (set by /// decimation factor in the IMU Message Format command) to prevent aliasing on a per data @@ -1014,17 +1019,17 @@ CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint1 /// complete bypass of the digital low-pass filter. /// /// Possible data descriptors: -/// 0x04 – Scaled accelerometer data -/// 0x05 – Scaled gyro data -/// 0x06 – Scaled magnetometer data (if applicable) -/// 0x17 – Scaled pressure data (if applicable) +/// 0x04 - Scaled accelerometer data +/// 0x05 - Scaled gyro data +/// 0x06 - Scaled magnetometer data (if applicable) +/// 0x17 - Scaled pressure data (if applicable) /// ///@{ -struct AdvLowpassFilter +struct ImuLowpassFilter { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ADVANCED_DATA_FILTER; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; static const bool HAS_WRITE_FUNCTION = true; static const bool HAS_READ_FUNCTION = true; @@ -1052,17 +1057,17 @@ struct AdvLowpassFilter }; }; -void insert(Serializer& serializer, const AdvLowpassFilter& self); -void extract(Serializer& serializer, AdvLowpassFilter& self); +void insert(Serializer& serializer, const ImuLowpassFilter& self); +void extract(Serializer& serializer, ImuLowpassFilter& self); -void insert(Serializer& serializer, const AdvLowpassFilter::Response& self); -void extract(Serializer& serializer, AdvLowpassFilter::Response& self); +void insert(Serializer& serializer, const ImuLowpassFilter::Response& self); +void extract(Serializer& serializer, ImuLowpassFilter::Response& self); -CmdResult writeAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); -CmdResult readAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); -CmdResult saveAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -CmdResult loadAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -CmdResult defaultAdvLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); +CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); +CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1180,9 +1185,9 @@ struct GpioConfig enum _enumType : uint8_t { NONE = 0x00, - OPEN_DRAIN = 0x01, ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pullup resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. - PULLDOWN = 0x02, ///< The pin will have an internal pulldown resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pullup. See the device specifications for the resistance value. - PULLUP = 0x04, ///< The pin will have an internal pullup resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pulldown. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. + OPEN_DRAIN = 0x01, ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pull-up resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. + PULLDOWN = 0x02, ///< The pin will have an internal pull-down resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pull-up. See the device specifications for the resistance value. + PULLUP = 0x04, ///< The pin will have an internal pull-up resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pull-down. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. }; uint8_t value = NONE; @@ -1242,7 +1247,7 @@ CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); /// While the state of a pin can always be set, it will only have an observable effect if /// the pin is set to output mode. /// -/// This command does not support saving, loading, or reseting the state. Instead, use the +/// This command does not support saving, loading, or resetting the state. Instead, use the /// GPIO Configuration command, which allows the initial state to be configured. /// ///@{ @@ -2096,7 +2101,7 @@ CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); /// EQSTART p^{veh} = q^{-1} p^{sen} q EQEND
/// /// Where:
-/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion desrcribing the transformation.
+/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion describing the transformation.
/// EQSTART p^{sen} = (0, v^{sen}_x, v^{sen}_y, v^{sen}_z) EQEND and EQSTART v^{sen} EQEND is a 3-element vector expressed in the sensor body frame.
/// EQSTART p^{veh} = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) EQEND and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
/// @@ -2236,7 +2241,7 @@ CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); /// Configure the settings for the complementary filter which produces the following (0x80) descriptor set values: attitude matrix (0x80,09), quaternion (0x80,0A), and Euler angle (0x80,0C) outputs. /// /// The filter can be configured to correct for pitch and roll using the accelerometer (with the assumption that linear acceleration is minimal), -/// and to correct for heading using the magnetomer (with the assumption that the local magnetic field is dominated by the Earth's own magnetic field). +/// and to correct for heading using the magnetometer (with the assumption that the local magnetic field is dominated by the Earth's own magnetic field). /// Pitch/roll and heading corrections each have their own configurable time constants, with a valid range of 1-1000 seconds. The default time constant is 10 seconds. /// ///@{ @@ -2288,7 +2293,7 @@ CmdResult defaultComplementaryFilter(C::mip_interface& device); /// Changes the IMU sensor gain. /// /// This allows you to optimize the range to get the best accuracy and performance -/// while minimizing overrange events. +/// while minimizing over-range events. /// /// Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine /// the appropriate setting value for your application. Using values other than @@ -2381,6 +2386,69 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Response& self); CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); ///@} /// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_3dm_mip_cmd_3dm_lowpass_filter (0x0C,0x54) Mip Cmd 3Dm Lowpass Filter [CPP] +/// This command controls the low-pass anti-aliasing filter supported data quantities. +/// +/// See the device user manual for data quantities which support the anti-aliasing filter. +/// +/// If set to automatic mode, the frequency will track half of the transmission rate +/// of the target descriptor according to the configured message format (0x0C,0x0F). +/// For example, if scaled accel (0x80,0x04) is set to stream at 100 Hz, the filter would +/// be set to 50 Hz. Changing the message format to 200 Hz would automatically adjust the +/// filter to 100 Hz. +/// +/// For WRITE, SAVE, LOAD, and DEFAULT function selectors, the descriptor set and/or field descriptor +/// may be 0x00 to set, save, load, or reset the setting for all supported descriptors. The +/// field descriptor must be 0x00 if the descriptor set is 0x00. +/// +/// +///@{ + +struct MipCmd3dmLowpassFilter +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + uint8_t desc_set = 0; ///< Descriptor set of the quantity to be filtered. + uint8_t field_desc = 0; ///< Field descriptor of the quantity to be filtered. + bool enable = 0; ///< The filter will be enabled if this is true. + bool manual = 0; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. + float frequency = 0; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; + + uint8_t desc_set = 0; ///< Descriptor set of the quantity to be filtered. + uint8_t field_desc = 0; ///< Field descriptor of the quantity to be filtered. + bool enable = 0; ///< The filter will be enabled if this is true. + bool manual = 0; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. + float frequency = 0; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. + + }; +}; +void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self); +void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self); + +void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self); +void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self); + +CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); +CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); +CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult defaultMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +///@} +/// ///@} ///@} diff --git a/src/mip/definitions/commands_base.h b/src/mip/definitions/commands_base.h index 030e04ccb..242e59ce6 100644 --- a/src/mip/definitions/commands_base.h +++ b/src/mip/definitions/commands_base.h @@ -242,7 +242,7 @@ mip_cmd_result mip_base_get_extended_descriptors(struct mip_interface* device, u /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_base_continuous_bit (0x01,0x08) Continuous Bit [C] -/// Report result of continous built-in test. +/// Report result of continuous built-in test. /// /// This test is non-disruptive but is not as thorough as the commanded BIT. /// diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 439a6beff..5d1da353c 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -342,7 +342,7 @@ CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptors /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_base_continuous_bit (0x01,0x08) Continuous Bit [CPP] -/// Report result of continous built-in test. +/// Report result of continuous built-in test. /// /// This test is non-disruptive but is not as thorough as the commanded BIT. /// diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index c680272d4..9169cc66f 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -736,7 +736,7 @@ mip_cmd_result mip_filter_default_gnss_source(struct mip_interface* device); typedef uint8_t mip_filter_heading_source_command_source; static const mip_filter_heading_source_command_source MIP_FILTER_HEADING_SOURCE_COMMAND_SOURCE_NONE = 0; ///< See note 3 static const mip_filter_heading_source_command_source MIP_FILTER_HEADING_SOURCE_COMMAND_SOURCE_MAG = 1; ///< -static const mip_filter_heading_source_command_source MIP_FILTER_HEADING_SOURCE_COMMAND_SOURCE_GNSS_VEL = 2; ///< Seen notes 1,2 +static const mip_filter_heading_source_command_source MIP_FILTER_HEADING_SOURCE_COMMAND_SOURCE_GNSS_VEL = 2; ///< See notes 1,2 static const mip_filter_heading_source_command_source MIP_FILTER_HEADING_SOURCE_COMMAND_SOURCE_EXTERNAL = 3; ///< static const mip_filter_heading_source_command_source MIP_FILTER_HEADING_SOURCE_COMMAND_SOURCE_GNSS_VEL_AND_MAG = 4; ///< static const mip_filter_heading_source_command_source MIP_FILTER_HEADING_SOURCE_COMMAND_SOURCE_GNSS_VEL_AND_EXTERNAL = 5; ///< @@ -968,7 +968,7 @@ struct mip_filter_aiding_measurement_enable_command { mip_function_selector function; mip_filter_aiding_measurement_enable_command_aiding_source aiding_source; ///< Aiding measurement source - bool enable; ///< Controls the aiding sorce + bool enable; ///< Controls the aiding source }; typedef struct mip_filter_aiding_measurement_enable_command mip_filter_aiding_measurement_enable_command; @@ -981,7 +981,7 @@ void extract_mip_filter_aiding_measurement_enable_command_aiding_source(struct m struct mip_filter_aiding_measurement_enable_response { mip_filter_aiding_measurement_enable_command_aiding_source aiding_source; ///< Aiding measurement source - bool enable; ///< Controls the aiding sorce + bool enable; ///< Controls the aiding source }; typedef struct mip_filter_aiding_measurement_enable_response mip_filter_aiding_measurement_enable_response; @@ -1153,6 +1153,7 @@ mip_cmd_result mip_filter_default_adaptive_filter_options(struct mip_interface* /// Set the antenna lever arm. /// /// This command works with devices that utilize multiple antennas. +///

Offset Limit: 10 m magnitude (default) /// ///@{ @@ -1226,8 +1227,10 @@ mip_cmd_result mip_filter_default_rel_pos_configuration(struct mip_interface* de /// This is used to change the location of the indicated point of reference, and will affect filter position and velocity outputs. /// Changing this setting from default will result in a global position offset that depends on vehicle attitude, /// and a velocity offset that depends on vehicle attitude and angular rate. -/// The lever arm is defined by a 3-element vector that points from the sensor to the desired reference point, with (x,y,z) components given in the vehicle's reference frame. -/// Note, if the reference point selector is set to VEH (1), this setting will affect the following data fields: (0x82, 0x01), (0x82, 0x02), (0x82, 0x40), (0x82, 0x41), and (0x82, 42) +///
The lever arm is defined by a 3-element vector that points from the sensor to the desired reference point, with (x,y,z) components given in the vehicle's reference frame. +///

Note, if the reference point selector is set to VEH (1), this setting will affect the following data fields: (0x82, 0x01), (0x82, 0x02), (0x82, 0x40), (0x82, 0x41), and (0x82, 42) +///

Offset Limits +///
Reference Point VEH (1): 10 m magnitude (default) /// ///@{ @@ -1336,7 +1339,7 @@ mip_cmd_result mip_filter_default_speed_lever_arm(struct mip_interface* device, /// By convention, the primary vehicle axis is the vehicle X-axis (note: the sensor may be physically installed in /// any orientation on the vehicle if the appropriate mounting transformation has been specified). /// This constraint will typically improve heading estimates for vehicles where the assumption is valid, such -/// as an automobile, particulary when GNSS coverage is intermittent. +/// as an automobile, particularly when GNSS coverage is intermittent. /// ///@{ diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 501e46b0a..c2409196f 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -874,7 +874,7 @@ struct HeadingSource { NONE = 0, ///< See note 3 MAG = 1, ///< - GNSS_VEL = 2, ///< Seen notes 1,2 + GNSS_VEL = 2, ///< See notes 1,2 EXTERNAL = 3, ///< GNSS_VEL_AND_MAG = 4, ///< GNSS_VEL_AND_EXTERNAL = 5, ///< @@ -1178,7 +1178,7 @@ struct AidingMeasurementEnable FunctionSelector function = static_cast(0); AidingSource aiding_source = static_cast(0); ///< Aiding measurement source - bool enable = 0; ///< Controls the aiding sorce + bool enable = 0; ///< Controls the aiding source struct Response { @@ -1186,7 +1186,7 @@ struct AidingMeasurementEnable static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; AidingSource aiding_source = static_cast(0); ///< Aiding measurement source - bool enable = 0; ///< Controls the aiding sorce + bool enable = 0; ///< Controls the aiding source }; }; @@ -1413,6 +1413,7 @@ CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device); /// Set the antenna lever arm. /// /// This command works with devices that utilize multiple antennas. +///

Offset Limit: 10 m magnitude (default) /// ///@{ @@ -1506,8 +1507,10 @@ CmdResult defaultRelPosConfiguration(C::mip_interface& device); /// This is used to change the location of the indicated point of reference, and will affect filter position and velocity outputs. /// Changing this setting from default will result in a global position offset that depends on vehicle attitude, /// and a velocity offset that depends on vehicle attitude and angular rate. -/// The lever arm is defined by a 3-element vector that points from the sensor to the desired reference point, with (x,y,z) components given in the vehicle's reference frame. -/// Note, if the reference point selector is set to VEH (1), this setting will affect the following data fields: (0x82, 0x01), (0x82, 0x02), (0x82, 0x40), (0x82, 0x41), and (0x82, 42) +///
The lever arm is defined by a 3-element vector that points from the sensor to the desired reference point, with (x,y,z) components given in the vehicle's reference frame. +///

Note, if the reference point selector is set to VEH (1), this setting will affect the following data fields: (0x82, 0x01), (0x82, 0x02), (0x82, 0x40), (0x82, 0x41), and (0x82, 42) +///

Offset Limits +///
Reference Point VEH (1): 10 m magnitude (default) /// ///@{ @@ -1639,7 +1642,7 @@ CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); /// By convention, the primary vehicle axis is the vehicle X-axis (note: the sensor may be physically installed in /// any orientation on the vehicle if the appropriate mounting transformation has been specified). /// This constraint will typically improve heading estimates for vehicles where the assumption is valid, such -/// as an automobile, particulary when GNSS coverage is intermittent. +/// as an automobile, particularly when GNSS coverage is intermittent. /// ///@{ diff --git a/src/mip/definitions/commands_gnss.c b/src/mip/definitions/commands_gnss.c index 2de3d6d6d..cb41c03a3 100644 --- a/src/mip/definitions/commands_gnss.c +++ b/src/mip/definitions/commands_gnss.c @@ -367,35 +367,6 @@ mip_cmd_result mip_gnss_default_rtk_dongle_configuration(struct mip_interface* d return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_gnss_receiver_safe_mode_command(mip_serializer* serializer, const mip_gnss_receiver_safe_mode_command* self) -{ - insert_u8(serializer, self->receiver_id); - - insert_u8(serializer, self->enable); - -} -void extract_mip_gnss_receiver_safe_mode_command(mip_serializer* serializer, mip_gnss_receiver_safe_mode_command* self) -{ - extract_u8(serializer, &self->receiver_id); - - extract_u8(serializer, &self->enable); - -} - -mip_cmd_result mip_gnss_receiver_safe_mode(struct mip_interface* device, uint8_t receiver_id, uint8_t enable) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_u8(&serializer, receiver_id); - - insert_u8(&serializer, enable); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_RECEIVER_SAFE_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/commands_gnss.cpp b/src/mip/definitions/commands_gnss.cpp index 28396592e..00bb9e59c 100644 --- a/src/mip/definitions/commands_gnss.cpp +++ b/src/mip/definitions/commands_gnss.cpp @@ -371,34 +371,6 @@ CmdResult defaultRtkDongleConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const ReceiverSafeMode& self) -{ - insert(serializer, self.receiver_id); - - insert(serializer, self.enable); - -} -void extract(Serializer& serializer, ReceiverSafeMode& self) -{ - extract(serializer, self.receiver_id); - - extract(serializer, self.enable); - -} - -CmdResult receiverSafeMode(C::mip_interface& device, uint8_t receiverId, uint8_t enable) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, receiverId); - - insert(serializer, enable); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RECEIVER_SAFE_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} } // namespace commands_gnss } // namespace mip diff --git a/src/mip/definitions/commands_gnss.h b/src/mip/definitions/commands_gnss.h index 9a18e418a..28959ca40 100644 --- a/src/mip/definitions/commands_gnss.h +++ b/src/mip/definitions/commands_gnss.h @@ -35,7 +35,6 @@ enum MIP_CMD_DESC_GNSS_LIST_RECEIVERS = 0x01, MIP_CMD_DESC_GNSS_SIGNAL_CONFIGURATION = 0x02, MIP_CMD_DESC_GNSS_RTK_DONGLE_CONFIGURATION = 0x10, - MIP_CMD_DESC_GNSS_RECEIVER_SAFE_MODE = 0x60, MIP_REPLY_DESC_GNSS_LIST_RECEIVERS = 0x81, MIP_REPLY_DESC_GNSS_SIGNAL_CONFIGURATION = 0x82, @@ -70,7 +69,7 @@ struct mip_gnss_receiver_info_command_info { uint8_t receiver_id; ///< Receiver id: e.g. 1, 2, etc. uint8_t mip_data_descriptor_set; ///< MIP descriptor set associated with this receiver - char description[32]; ///< Ascii description of receiver + char description[32]; ///< Ascii description of receiver. Contains the following info (comma-delimited):
Module name/model
Firmware version info }; typedef struct mip_gnss_receiver_info_command_info mip_gnss_receiver_info_command_info; @@ -166,27 +165,6 @@ mip_cmd_result mip_gnss_load_rtk_dongle_configuration(struct mip_interface* devi mip_cmd_result mip_gnss_default_rtk_dongle_configuration(struct mip_interface* device); ///@} /// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_gnss_receiver_safe_mode (0x0E,0x60) Receiver Safe Mode [C] -/// Enable/disable safe mode for the provided receiver ID. -/// Note: Receivers in safe mode will not output valid GNSS data. -/// -/// -///@{ - -struct mip_gnss_receiver_safe_mode_command -{ - uint8_t receiver_id; ///< Receiver id: e.g. 1, 2, etc. - uint8_t enable; ///< 0 - Disabled, 1- Enabled - -}; -typedef struct mip_gnss_receiver_safe_mode_command mip_gnss_receiver_safe_mode_command; -void insert_mip_gnss_receiver_safe_mode_command(struct mip_serializer* serializer, const mip_gnss_receiver_safe_mode_command* self); -void extract_mip_gnss_receiver_safe_mode_command(struct mip_serializer* serializer, mip_gnss_receiver_safe_mode_command* self); - -mip_cmd_result mip_gnss_receiver_safe_mode(struct mip_interface* device, uint8_t receiver_id, uint8_t enable); -///@} -/// ///@} ///@} diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 643039142..1ecefbd15 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -34,7 +34,6 @@ enum CMD_LIST_RECEIVERS = 0x01, CMD_SIGNAL_CONFIGURATION = 0x02, CMD_RTK_DONGLE_CONFIGURATION = 0x10, - CMD_RECEIVER_SAFE_MODE = 0x60, REPLY_LIST_RECEIVERS = 0x81, REPLY_SIGNAL_CONFIGURATION = 0x82, @@ -76,7 +75,7 @@ struct ReceiverInfo { uint8_t receiver_id = 0; ///< Receiver id: e.g. 1, 2, etc. uint8_t mip_data_descriptor_set = 0; ///< MIP descriptor set associated with this receiver - char description[32] = {0}; ///< Ascii description of receiver + char description[32] = {0}; ///< Ascii description of receiver. Contains the following info (comma-delimited):
Module name/model
Firmware version info }; @@ -198,31 +197,6 @@ CmdResult loadRtkDongleConfiguration(C::mip_interface& device); CmdResult defaultRtkDongleConfiguration(C::mip_interface& device); ///@} /// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_gnss_receiver_safe_mode (0x0E,0x60) Receiver Safe Mode [CPP] -/// Enable/disable safe mode for the provided receiver ID. -/// Note: Receivers in safe mode will not output valid GNSS data. -/// -/// -///@{ - -struct ReceiverSafeMode -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RECEIVER_SAFE_MODE; - - static const bool HAS_FUNCTION_SELECTOR = false; - - uint8_t receiver_id = 0; ///< Receiver id: e.g. 1, 2, etc. - uint8_t enable = 0; ///< 0 - Disabled, 1- Enabled - -}; -void insert(Serializer& serializer, const ReceiverSafeMode& self); -void extract(Serializer& serializer, ReceiverSafeMode& self); - -CmdResult receiverSafeMode(C::mip_interface& device, uint8_t receiverId, uint8_t enable); -///@} -/// ///@} ///@} diff --git a/src/mip/definitions/commands_rtk.c b/src/mip/definitions/commands_rtk.c index 3ff0bd274..d1d1c153d 100644 --- a/src/mip/definitions/commands_rtk.c +++ b/src/mip/definitions/commands_rtk.c @@ -360,7 +360,7 @@ void insert_mip_rtk_service_status_response(mip_serializer* serializer, const mi { insert_mip_rtk_service_status_command_service_flags(serializer, self->flags); - insert_u32(serializer, self->recievedBytes); + insert_u32(serializer, self->receivedBytes); insert_u32(serializer, self->lastBytes); @@ -371,7 +371,7 @@ void extract_mip_rtk_service_status_response(mip_serializer* serializer, mip_rtk { extract_mip_rtk_service_status_command_service_flags(serializer, &self->flags); - extract_u32(serializer, &self->recievedBytes); + extract_u32(serializer, &self->receivedBytes); extract_u32(serializer, &self->lastBytes); @@ -390,7 +390,7 @@ void extract_mip_rtk_service_status_command_service_flags(struct mip_serializer* *self = tmp; } -mip_cmd_result mip_rtk_service_status(struct mip_interface* device, uint32_t reserved1, uint32_t reserved2, mip_rtk_service_status_command_service_flags* flags_out, uint32_t* recieved_bytes_out, uint32_t* last_bytes_out, uint64_t* last_bytes_time_out) +mip_cmd_result mip_rtk_service_status(struct mip_interface* device, uint32_t reserved1, uint32_t reserved2, mip_rtk_service_status_command_service_flags* flags_out, uint32_t* received_bytes_out, uint32_t* last_bytes_out, uint64_t* last_bytes_time_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -413,8 +413,8 @@ mip_cmd_result mip_rtk_service_status(struct mip_interface* device, uint32_t res assert(flags_out); extract_mip_rtk_service_status_command_service_flags(&deserializer, flags_out); - assert(recieved_bytes_out); - extract_u32(&deserializer, recieved_bytes_out); + assert(received_bytes_out); + extract_u32(&deserializer, received_bytes_out); assert(last_bytes_out); extract_u32(&deserializer, last_bytes_out); diff --git a/src/mip/definitions/commands_rtk.cpp b/src/mip/definitions/commands_rtk.cpp index 65e75b575..b2997856e 100644 --- a/src/mip/definitions/commands_rtk.cpp +++ b/src/mip/definitions/commands_rtk.cpp @@ -461,7 +461,7 @@ void insert(Serializer& serializer, const ServiceStatus::Response& self) { insert(serializer, self.flags); - insert(serializer, self.recievedBytes); + insert(serializer, self.receivedBytes); insert(serializer, self.lastBytes); @@ -472,7 +472,7 @@ void extract(Serializer& serializer, ServiceStatus::Response& self) { extract(serializer, self.flags); - extract(serializer, self.recievedBytes); + extract(serializer, self.receivedBytes); extract(serializer, self.lastBytes); @@ -480,7 +480,7 @@ void extract(Serializer& serializer, ServiceStatus::Response& self) } -CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* recievedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) +CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -501,8 +501,8 @@ CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t r assert(flagsOut); extract(deserializer, *flagsOut); - assert(recievedbytesOut); - extract(deserializer, *recievedbytesOut); + assert(receivedbytesOut); + extract(deserializer, *receivedbytesOut); assert(lastbytesOut); extract(deserializer, *lastbytesOut); diff --git a/src/mip/definitions/commands_rtk.h b/src/mip/definitions/commands_rtk.h index 34f849d6e..028ca7c0f 100644 --- a/src/mip/definitions/commands_rtk.h +++ b/src/mip/definitions/commands_rtk.h @@ -303,7 +303,7 @@ void extract_mip_rtk_service_status_command_service_flags(struct mip_serializer* struct mip_rtk_service_status_response { mip_rtk_service_status_command_service_flags flags; - uint32_t recievedBytes; + uint32_t receivedBytes; uint32_t lastBytes; uint64_t lastBytesTime; @@ -312,12 +312,12 @@ typedef struct mip_rtk_service_status_response mip_rtk_service_status_response; void insert_mip_rtk_service_status_response(struct mip_serializer* serializer, const mip_rtk_service_status_response* self); void extract_mip_rtk_service_status_response(struct mip_serializer* serializer, mip_rtk_service_status_response* self); -mip_cmd_result mip_rtk_service_status(struct mip_interface* device, uint32_t reserved1, uint32_t reserved2, mip_rtk_service_status_command_service_flags* flags_out, uint32_t* recieved_bytes_out, uint32_t* last_bytes_out, uint64_t* last_bytes_time_out); +mip_cmd_result mip_rtk_service_status(struct mip_interface* device, uint32_t reserved1, uint32_t reserved2, mip_rtk_service_status_command_service_flags* flags_out, uint32_t* received_bytes_out, uint32_t* last_bytes_out, uint64_t* last_bytes_time_out); ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_rtk_prod_erase_storage (0x0F,0x20) Prod Erase Storage [C] -/// This command will erase the selected media to a raw and unitialized state. ALL DATA WILL BE LOST. +/// This command will erase the selected media to a raw and uninitialized state. ALL DATA WILL BE LOST. /// This command is only available in calibration mode. /// ///@{ diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index c91b4bbf3..43364dbf5 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -443,7 +443,7 @@ struct ServiceStatus static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; ServiceFlags flags; - uint32_t recievedBytes = 0; + uint32_t receivedBytes = 0; uint32_t lastBytes = 0; uint64_t lastBytesTime = 0; @@ -455,12 +455,12 @@ void extract(Serializer& serializer, ServiceStatus& self); void insert(Serializer& serializer, const ServiceStatus::Response& self); void extract(Serializer& serializer, ServiceStatus::Response& self); -CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* recievedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); +CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_rtk_prod_erase_storage (0x0F,0x20) Prod Erase Storage [CPP] -/// This command will erase the selected media to a raw and unitialized state. ALL DATA WILL BE LOST. +/// This command will erase the selected media to a raw and uninitialized state. ALL DATA WILL BE LOST. /// This command is only available in calibration mode. /// ///@{ diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index 8441cbe98..af7737875 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -255,7 +255,7 @@ bool extract_mip_filter_velocity_ned_data_from_field(const struct mip_field* fie /// EQSTART p^{veh} = q^{-1} p^{ned} q EQEND
/// /// Where:
-/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion desrcribing the rotation.
+/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion describing the rotation.
/// EQSTART p^ned = (0, v^{ned}_x, v^{ned}_y, v^{ned}_z) EQEND and EQSTART v^{ned} EQEND is a 3-element vector expressed in the NED frame.
/// EQSTART p^veh = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) EQEND and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
/// @@ -473,7 +473,7 @@ bool extract_mip_filter_accel_bias_uncertainty_data_from_field(const struct mip_ /// Upon recovering from a PPS outage, the user should expect a jump in the reported GPS time due to the accumulation of internal clock error. /// If synchronization to an external clock or onboard GNSS receiver (for products that have one) is disabled, this time is equivalent to internal system time. /// -/// Note: this data field may be deprecrated in the future. The more flexible shared data field (0x82, 0xD3) should be used instead. +/// Note: this data field may be deprecated in the future. The more flexible shared data field (0x82, 0xD3) should be used instead. /// ///@{ @@ -500,7 +500,7 @@ bool extract_mip_filter_timestamp_data_from_field(const struct mip_field* field, struct mip_filter_status_data { mip_filter_mode filter_state; ///< Device-specific filter state. Please consult the user manual for definition. - mip_filter_dynamics_mode dynamics_mode; ///< Device-specific dynamics mode. Please consult the user manual for definition. + mip_filter_dynamics_mode dynamics_mode; ///< Device-specific dynamics mode. Please consult the user manual for definition. mip_filter_status_flags status_flags; ///< Device-specific status flags. Please consult the user manual for definition. }; diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index f8962e5f2..bea60c5f6 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -290,7 +290,7 @@ void extract(Serializer& serializer, VelocityNed& self); /// EQSTART p^{veh} = q^{-1} p^{ned} q EQEND
/// /// Where:
-/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion desrcribing the rotation.
+/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion describing the rotation.
/// EQSTART p^ned = (0, v^{ned}_x, v^{ned}_y, v^{ned}_z) EQEND and EQSTART v^{ned} EQEND is a 3-element vector expressed in the NED frame.
/// EQSTART p^veh = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) EQEND and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
/// @@ -538,7 +538,7 @@ void extract(Serializer& serializer, AccelBiasUncertainty& self); /// Upon recovering from a PPS outage, the user should expect a jump in the reported GPS time due to the accumulation of internal clock error. /// If synchronization to an external clock or onboard GNSS receiver (for products that have one) is disabled, this time is equivalent to internal system time. /// -/// Note: this data field may be deprecrated in the future. The more flexible shared data field (0x82, 0xD3) should be used instead. +/// Note: this data field may be deprecated in the future. The more flexible shared data field (0x82, 0xD3) should be used instead. /// ///@{ @@ -573,7 +573,7 @@ struct Status static const bool HAS_FUNCTION_SELECTOR = false; FilterMode filter_state = static_cast(0); ///< Device-specific filter state. Please consult the user manual for definition. - FilterDynamicsMode dynamics_mode = static_cast(0); ///< Device-specific dynamics mode. Please consult the user manual for definition. + FilterDynamicsMode dynamics_mode = static_cast(0); ///< Device-specific dynamics mode. Please consult the user manual for definition. FilterStatusFlags status_flags; ///< Device-specific status flags. Please consult the user manual for definition. }; diff --git a/src/mip/definitions/data_gnss.c b/src/mip/definitions/data_gnss.c index 4a2b28c87..2213a4bd9 100644 --- a/src/mip/definitions/data_gnss.c +++ b/src/mip/definitions/data_gnss.c @@ -1544,6 +1544,169 @@ void extract_mip_gnss_gps_ephemeris_data_valid_flags(struct mip_serializer* seri *self = tmp; } +void insert_mip_gnss_galileo_ephemeris_data(mip_serializer* serializer, const mip_gnss_galileo_ephemeris_data* self) +{ + insert_u8(serializer, self->index); + + insert_u8(serializer, self->count); + + insert_double(serializer, self->time_of_week); + + insert_u16(serializer, self->week_number); + + insert_u8(serializer, self->satellite_id); + + insert_u8(serializer, self->health); + + insert_u8(serializer, self->iodc); + + insert_u8(serializer, self->iode); + + insert_double(serializer, self->t_oc); + + insert_double(serializer, self->af0); + + insert_double(serializer, self->af1); + + insert_double(serializer, self->af2); + + insert_double(serializer, self->t_gd); + + insert_double(serializer, self->ISC_L1CA); + + insert_double(serializer, self->ISC_L2C); + + insert_double(serializer, self->t_oe); + + insert_double(serializer, self->a); + + insert_double(serializer, self->a_dot); + + insert_double(serializer, self->mean_anomaly); + + insert_double(serializer, self->delta_mean_motion); + + insert_double(serializer, self->delta_mean_motion_dot); + + insert_double(serializer, self->eccentricity); + + insert_double(serializer, self->argument_of_perigee); + + insert_double(serializer, self->omega); + + insert_double(serializer, self->omega_dot); + + insert_double(serializer, self->inclination); + + insert_double(serializer, self->inclination_dot); + + insert_double(serializer, self->c_ic); + + insert_double(serializer, self->c_is); + + insert_double(serializer, self->c_uc); + + insert_double(serializer, self->c_us); + + insert_double(serializer, self->c_rc); + + insert_double(serializer, self->c_rs); + + insert_mip_gnss_galileo_ephemeris_data_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_gnss_galileo_ephemeris_data(mip_serializer* serializer, mip_gnss_galileo_ephemeris_data* self) +{ + extract_u8(serializer, &self->index); + + extract_u8(serializer, &self->count); + + extract_double(serializer, &self->time_of_week); + + extract_u16(serializer, &self->week_number); + + extract_u8(serializer, &self->satellite_id); + + extract_u8(serializer, &self->health); + + extract_u8(serializer, &self->iodc); + + extract_u8(serializer, &self->iode); + + extract_double(serializer, &self->t_oc); + + extract_double(serializer, &self->af0); + + extract_double(serializer, &self->af1); + + extract_double(serializer, &self->af2); + + extract_double(serializer, &self->t_gd); + + extract_double(serializer, &self->ISC_L1CA); + + extract_double(serializer, &self->ISC_L2C); + + extract_double(serializer, &self->t_oe); + + extract_double(serializer, &self->a); + + extract_double(serializer, &self->a_dot); + + extract_double(serializer, &self->mean_anomaly); + + extract_double(serializer, &self->delta_mean_motion); + + extract_double(serializer, &self->delta_mean_motion_dot); + + extract_double(serializer, &self->eccentricity); + + extract_double(serializer, &self->argument_of_perigee); + + extract_double(serializer, &self->omega); + + extract_double(serializer, &self->omega_dot); + + extract_double(serializer, &self->inclination); + + extract_double(serializer, &self->inclination_dot); + + extract_double(serializer, &self->c_ic); + + extract_double(serializer, &self->c_is); + + extract_double(serializer, &self->c_uc); + + extract_double(serializer, &self->c_us); + + extract_double(serializer, &self->c_rc); + + extract_double(serializer, &self->c_rs); + + extract_mip_gnss_galileo_ephemeris_data_valid_flags(serializer, &self->valid_flags); + +} +bool extract_mip_gnss_galileo_ephemeris_data_from_field(const mip_field* field, void* ptr) +{ + assert(ptr); + mip_gnss_galileo_ephemeris_data* self = ptr; + struct mip_serializer serializer; + mip_serializer_init_from_field(&serializer, field); + extract_mip_gnss_galileo_ephemeris_data(&serializer, self); + return mip_serializer_is_complete(&serializer); +} + +void insert_mip_gnss_galileo_ephemeris_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_galileo_ephemeris_data_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_gnss_galileo_ephemeris_data_valid_flags(struct mip_serializer* serializer, mip_gnss_galileo_ephemeris_data_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + void insert_mip_gnss_glo_ephemeris_data(mip_serializer* serializer, const mip_gnss_glo_ephemeris_data* self) { insert_u8(serializer, self->index); diff --git a/src/mip/definitions/data_gnss.cpp b/src/mip/definitions/data_gnss.cpp index e632168ab..2511644c5 100644 --- a/src/mip/definitions/data_gnss.cpp +++ b/src/mip/definitions/data_gnss.cpp @@ -910,6 +910,149 @@ void extract(Serializer& serializer, GpsEphemeris& self) } +void insert(Serializer& serializer, const GalileoEphemeris& self) +{ + insert(serializer, self.index); + + insert(serializer, self.count); + + insert(serializer, self.time_of_week); + + insert(serializer, self.week_number); + + insert(serializer, self.satellite_id); + + insert(serializer, self.health); + + insert(serializer, self.iodc); + + insert(serializer, self.iode); + + insert(serializer, self.t_oc); + + insert(serializer, self.af0); + + insert(serializer, self.af1); + + insert(serializer, self.af2); + + insert(serializer, self.t_gd); + + insert(serializer, self.ISC_L1CA); + + insert(serializer, self.ISC_L2C); + + insert(serializer, self.t_oe); + + insert(serializer, self.a); + + insert(serializer, self.a_dot); + + insert(serializer, self.mean_anomaly); + + insert(serializer, self.delta_mean_motion); + + insert(serializer, self.delta_mean_motion_dot); + + insert(serializer, self.eccentricity); + + insert(serializer, self.argument_of_perigee); + + insert(serializer, self.omega); + + insert(serializer, self.omega_dot); + + insert(serializer, self.inclination); + + insert(serializer, self.inclination_dot); + + insert(serializer, self.c_ic); + + insert(serializer, self.c_is); + + insert(serializer, self.c_uc); + + insert(serializer, self.c_us); + + insert(serializer, self.c_rc); + + insert(serializer, self.c_rs); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, GalileoEphemeris& self) +{ + extract(serializer, self.index); + + extract(serializer, self.count); + + extract(serializer, self.time_of_week); + + extract(serializer, self.week_number); + + extract(serializer, self.satellite_id); + + extract(serializer, self.health); + + extract(serializer, self.iodc); + + extract(serializer, self.iode); + + extract(serializer, self.t_oc); + + extract(serializer, self.af0); + + extract(serializer, self.af1); + + extract(serializer, self.af2); + + extract(serializer, self.t_gd); + + extract(serializer, self.ISC_L1CA); + + extract(serializer, self.ISC_L2C); + + extract(serializer, self.t_oe); + + extract(serializer, self.a); + + extract(serializer, self.a_dot); + + extract(serializer, self.mean_anomaly); + + extract(serializer, self.delta_mean_motion); + + extract(serializer, self.delta_mean_motion_dot); + + extract(serializer, self.eccentricity); + + extract(serializer, self.argument_of_perigee); + + extract(serializer, self.omega); + + extract(serializer, self.omega_dot); + + extract(serializer, self.inclination); + + extract(serializer, self.inclination_dot); + + extract(serializer, self.c_ic); + + extract(serializer, self.c_is); + + extract(serializer, self.c_uc); + + extract(serializer, self.c_us); + + extract(serializer, self.c_rc); + + extract(serializer, self.c_rs); + + extract(serializer, self.valid_flags); + +} + void insert(Serializer& serializer, const GloEphemeris& self) { insert(serializer, self.index); diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index 4e87526ad..a4121b879 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -453,7 +453,7 @@ static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX typedef uint16_t mip_gnss_fix_info_data_fix_flags; static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_NONE = 0x0000; static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_SBAS_USED = 0x0001; ///< -static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_DNGSS_USED = 0x0002; ///< +static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_DGNSS_USED = 0x0002; ///< typedef uint16_t mip_gnss_fix_info_data_valid_flags; static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -822,7 +822,7 @@ struct mip_gnss_sbas_correction_data mip_gnss_constellation_id gnss_id; ///< GNSS constellation id uint8_t sv_id; ///< GNSS satellite id within the constellation. uint8_t udrei; ///< [See above 0-13 usable, 14 not monitored, 15 - do not use] - float pseudorange_correction; ///< Pseudorange correction [meters]. + float pseudorange_correction; ///< Pseudo-range correction [meters]. float iono_correction; ///< Ionospheric correction [meters]. mip_gnss_sbas_correction_data_valid_flags valid_flags; @@ -1094,10 +1094,10 @@ struct mip_gnss_raw_data mip_gnss_signal_id signal_id; ///< Signal identifier for the satellite. float signal_strength; ///< Carrier to noise ratio [dBHz]. mip_gnss_raw_data_gnss_signal_quality quality; ///< Indicator of signal quality. - double pseudorange; ///< Pseudorange measurement [meters]. + double pseudorange; ///< Pseudo-range measurement [meters]. double carrier_phase; ///< Carrier phase measurement [Carrier periods]. float doppler; ///< Measured doppler shift [Hz]. - float range_uncert; ///< Uncertainty of the pseudorange measurement [m]. + float range_uncert; ///< Uncertainty of the pseudo-range measurement [m]. float phase_uncert; ///< Uncertainty of the phase measurement [Carrier periods]. float doppler_uncert; ///< Uncertainty of the measured doppler shift [Hz]. float lock_time; ///< DOC Minimum carrier phase lock time [s]. Note: the maximum value is dependent on the receiver. @@ -1119,7 +1119,7 @@ void extract_mip_gnss_raw_data_valid_flags(struct mip_serializer* serializer, mi /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_gnss_gps_ephemeris (0x81,0x61) Gps Ephemeris [C] -/// GPS/Galileo Ephemeris Data +/// GPS Ephemeris Data /// ///@{ @@ -1148,14 +1148,14 @@ struct mip_gnss_gps_ephemeris_data double ISC_L2C; double t_oe; ///< Reference time for ephemeris in [s]. double a; ///< Semi-major axis [m]. - double a_dot; ///< Semi-matjor axis rate [m/s]. + double a_dot; ///< Semi-major axis rate [m/s]. double mean_anomaly; ///< [rad]. double delta_mean_motion; ///< [rad]. double delta_mean_motion_dot; ///< [rad/s]. double eccentricity; double argument_of_perigee; ///< [rad]. double omega; ///< Longitude of Ascending Node [rad]. - double omega_dot; ///< Rate of Right Ascention [rad/s]. + double omega_dot; ///< Rate of Right Ascension [rad/s]. double inclination; ///< Inclination angle [rad]. double inclination_dot; ///< Inclination angle rate of change [rad/s]. double c_ic; ///< Harmonic Correction Term. @@ -1175,6 +1175,66 @@ bool extract_mip_gnss_gps_ephemeris_data_from_field(const struct mip_field* fiel void insert_mip_gnss_gps_ephemeris_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_gps_ephemeris_data_valid_flags self); void extract_mip_gnss_gps_ephemeris_data_valid_flags(struct mip_serializer* serializer, mip_gnss_gps_ephemeris_data_valid_flags* self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_gnss_galileo_ephemeris (0x81,0x63) Galileo Ephemeris [C] +/// Galileo Ephemeris Data +/// +///@{ + +typedef uint16_t mip_gnss_galileo_ephemeris_data_valid_flags; +static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_NONE = 0x0000; +static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_EPHEMERIS = 0x0001; ///< +static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_MODERN_DATA = 0x0002; ///< +static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< + +struct mip_gnss_galileo_ephemeris_data +{ + uint8_t index; ///< Index of this field in this epoch. + uint8_t count; ///< Total number of fields in this epoch. + double time_of_week; ///< GPS Time of week [seconds] + uint16_t week_number; ///< GPS Week since 1980 [weeks] + uint8_t satellite_id; ///< GNSS satellite id within the constellation. + uint8_t health; ///< Satellite and signal health + uint8_t iodc; ///< Issue of Data Clock. This increments each time the data changes and rolls over at 4. It is used to make sure various raw data elements from different sources line up correctly. + uint8_t iode; ///< Issue of Data Ephemeris. + double t_oc; ///< Reference time for clock data. + double af0; ///< Clock bias in [s]. + double af1; ///< Clock drift in [s/s]. + double af2; ///< Clock drift rate in [s/s^2]. + double t_gd; ///< T Group Delay [s]. + double ISC_L1CA; + double ISC_L2C; + double t_oe; ///< Reference time for ephemeris in [s]. + double a; ///< Semi-major axis [m]. + double a_dot; ///< Semi-major axis rate [m/s]. + double mean_anomaly; ///< [rad]. + double delta_mean_motion; ///< [rad]. + double delta_mean_motion_dot; ///< [rad/s]. + double eccentricity; + double argument_of_perigee; ///< [rad]. + double omega; ///< Longitude of Ascending Node [rad]. + double omega_dot; ///< Rate of Right Ascension [rad/s]. + double inclination; ///< Inclination angle [rad]. + double inclination_dot; ///< Inclination angle rate of change [rad/s]. + double c_ic; ///< Harmonic Correction Term. + double c_is; ///< Harmonic Correction Term. + double c_uc; ///< Harmonic Correction Term. + double c_us; ///< Harmonic Correction Term. + double c_rc; ///< Harmonic Correction Term. + double c_rs; ///< Harmonic Correction Term. + mip_gnss_galileo_ephemeris_data_valid_flags valid_flags; + +}; +typedef struct mip_gnss_galileo_ephemeris_data mip_gnss_galileo_ephemeris_data; +void insert_mip_gnss_galileo_ephemeris_data(struct mip_serializer* serializer, const mip_gnss_galileo_ephemeris_data* self); +void extract_mip_gnss_galileo_ephemeris_data(struct mip_serializer* serializer, mip_gnss_galileo_ephemeris_data* self); +bool extract_mip_gnss_galileo_ephemeris_data_from_field(const struct mip_field* field, void* ptr); + +void insert_mip_gnss_galileo_ephemeris_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_galileo_ephemeris_data_valid_flags self); +void extract_mip_gnss_galileo_ephemeris_data_valid_flags(struct mip_serializer* serializer, mip_gnss_galileo_ephemeris_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1199,11 +1259,11 @@ struct mip_gnss_glo_ephemeris_data uint32_t tk; ///< Frame start time within current day [seconds] uint32_t tb; ///< Ephemeris reference time [seconds] uint8_t sat_type; ///< Type of satellite (M) GLONASS = 0, GLONASS-M = 1 - double gamma; ///< Relative deviation of carrier frequency from nominal [dimesnionless] + double gamma; ///< Relative deviation of carrier frequency from nominal [dimensionless] double tau_n; ///< Time correction relative to GLONASS Time [seconds] double x[3]; ///< Satellite PE-90 position [m] float v[3]; ///< Satellite PE-90 velocity [m/s] - float a[3]; ///< Satellite PE-90 acceleration due to pertubations [m/s^2] + float a[3]; ///< Satellite PE-90 acceleration due to perturbations [m/s^2] uint8_t health; ///< Satellite Health (Bn), Non-zero indicates satellite malfunction uint8_t P; ///< Satellite operation mode (See GLONASS ICD) uint8_t NT; ///< Day number within a 4 year period. diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 199554912..010702097 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -573,7 +573,7 @@ struct FixInfo { NONE = 0x0000, SBAS_USED = 0x0001, ///< - DNGSS_USED = 0x0002, ///< + DGNSS_USED = 0x0002, ///< }; uint16_t value = NONE; @@ -1090,7 +1090,7 @@ struct SbasCorrection GnssConstellationId gnss_id = static_cast(0); ///< GNSS constellation id uint8_t sv_id = 0; ///< GNSS satellite id within the constellation. uint8_t udrei = 0; ///< [See above 0-13 usable, 14 not monitored, 15 - do not use] - float pseudorange_correction = 0; ///< Pseudorange correction [meters]. + float pseudorange_correction = 0; ///< Pseudo-range correction [meters]. float iono_correction = 0; ///< Ionospheric correction [meters]. ValidFlags valid_flags; @@ -1453,10 +1453,10 @@ struct Raw GnssSignalId signal_id = static_cast(0); ///< Signal identifier for the satellite. float signal_strength = 0; ///< Carrier to noise ratio [dBHz]. GnssSignalQuality quality = static_cast(0); ///< Indicator of signal quality. - double pseudorange = 0; ///< Pseudorange measurement [meters]. + double pseudorange = 0; ///< Pseudo-range measurement [meters]. double carrier_phase = 0; ///< Carrier phase measurement [Carrier periods]. float doppler = 0; ///< Measured doppler shift [Hz]. - float range_uncert = 0; ///< Uncertainty of the pseudorange measurement [m]. + float range_uncert = 0; ///< Uncertainty of the pseudo-range measurement [m]. float phase_uncert = 0; ///< Uncertainty of the phase measurement [Carrier periods]. float doppler_uncert = 0; ///< Uncertainty of the measured doppler shift [Hz]. float lock_time = 0; ///< DOC Minimum carrier phase lock time [s]. Note: the maximum value is dependent on the receiver. @@ -1470,7 +1470,7 @@ void extract(Serializer& serializer, Raw& self); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_gnss_gps_ephemeris (0x81,0x61) Gps Ephemeris [CPP] -/// GPS/Galileo Ephemeris Data +/// GPS Ephemeris Data /// ///@{ @@ -1518,14 +1518,14 @@ struct GpsEphemeris double ISC_L2C = 0; double t_oe = 0; ///< Reference time for ephemeris in [s]. double a = 0; ///< Semi-major axis [m]. - double a_dot = 0; ///< Semi-matjor axis rate [m/s]. + double a_dot = 0; ///< Semi-major axis rate [m/s]. double mean_anomaly = 0; ///< [rad]. double delta_mean_motion = 0; ///< [rad]. double delta_mean_motion_dot = 0; ///< [rad/s]. double eccentricity = 0; double argument_of_perigee = 0; ///< [rad]. double omega = 0; ///< Longitude of Ascending Node [rad]. - double omega_dot = 0; ///< Rate of Right Ascention [rad/s]. + double omega_dot = 0; ///< Rate of Right Ascension [rad/s]. double inclination = 0; ///< Inclination angle [rad]. double inclination_dot = 0; ///< Inclination angle rate of change [rad/s]. double c_ic = 0; ///< Harmonic Correction Term. @@ -1540,6 +1540,80 @@ struct GpsEphemeris void insert(Serializer& serializer, const GpsEphemeris& self); void extract(Serializer& serializer, GpsEphemeris& self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_gnss_galileo_ephemeris (0x81,0x63) Galileo Ephemeris [CPP] +/// Galileo Ephemeris Data +/// +///@{ + +struct GalileoEphemeris +{ + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; + + static const bool HAS_FUNCTION_SELECTOR = false; + + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + EPHEMERIS = 0x0001, ///< + MODERN_DATA = 0x0002, ///< + FLAGS = 0x0003, ///< + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + }; + + uint8_t index = 0; ///< Index of this field in this epoch. + uint8_t count = 0; ///< Total number of fields in this epoch. + double time_of_week = 0; ///< GPS Time of week [seconds] + uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] + uint8_t satellite_id = 0; ///< GNSS satellite id within the constellation. + uint8_t health = 0; ///< Satellite and signal health + uint8_t iodc = 0; ///< Issue of Data Clock. This increments each time the data changes and rolls over at 4. It is used to make sure various raw data elements from different sources line up correctly. + uint8_t iode = 0; ///< Issue of Data Ephemeris. + double t_oc = 0; ///< Reference time for clock data. + double af0 = 0; ///< Clock bias in [s]. + double af1 = 0; ///< Clock drift in [s/s]. + double af2 = 0; ///< Clock drift rate in [s/s^2]. + double t_gd = 0; ///< T Group Delay [s]. + double ISC_L1CA = 0; + double ISC_L2C = 0; + double t_oe = 0; ///< Reference time for ephemeris in [s]. + double a = 0; ///< Semi-major axis [m]. + double a_dot = 0; ///< Semi-major axis rate [m/s]. + double mean_anomaly = 0; ///< [rad]. + double delta_mean_motion = 0; ///< [rad]. + double delta_mean_motion_dot = 0; ///< [rad/s]. + double eccentricity = 0; + double argument_of_perigee = 0; ///< [rad]. + double omega = 0; ///< Longitude of Ascending Node [rad]. + double omega_dot = 0; ///< Rate of Right Ascension [rad/s]. + double inclination = 0; ///< Inclination angle [rad]. + double inclination_dot = 0; ///< Inclination angle rate of change [rad/s]. + double c_ic = 0; ///< Harmonic Correction Term. + double c_is = 0; ///< Harmonic Correction Term. + double c_uc = 0; ///< Harmonic Correction Term. + double c_us = 0; ///< Harmonic Correction Term. + double c_rc = 0; ///< Harmonic Correction Term. + double c_rs = 0; ///< Harmonic Correction Term. + ValidFlags valid_flags; + +}; +void insert(Serializer& serializer, const GalileoEphemeris& self); +void extract(Serializer& serializer, GalileoEphemeris& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1583,11 +1657,11 @@ struct GloEphemeris uint32_t tk = 0; ///< Frame start time within current day [seconds] uint32_t tb = 0; ///< Ephemeris reference time [seconds] uint8_t sat_type = 0; ///< Type of satellite (M) GLONASS = 0, GLONASS-M = 1 - double gamma = 0; ///< Relative deviation of carrier frequency from nominal [dimesnionless] + double gamma = 0; ///< Relative deviation of carrier frequency from nominal [dimensionless] double tau_n = 0; ///< Time correction relative to GLONASS Time [seconds] double x[3] = {0}; ///< Satellite PE-90 position [m] float v[3] = {0}; ///< Satellite PE-90 velocity [m/s] - float a[3] = {0}; ///< Satellite PE-90 acceleration due to pertubations [m/s^2] + float a[3] = {0}; ///< Satellite PE-90 acceleration due to perturbations [m/s^2] uint8_t health = 0; ///< Satellite Health (Bn), Non-zero indicates satellite malfunction uint8_t P = 0; ///< Satellite operation mode (See GLONASS ICD) uint8_t NT = 0; ///< Day number within a 4 year period. diff --git a/src/mip/definitions/data_sensor.h b/src/mip/definitions/data_sensor.h index 0ca5dc02b..ae2cee70a 100644 --- a/src/mip/definitions/data_sensor.h +++ b/src/mip/definitions/data_sensor.h @@ -295,9 +295,9 @@ bool extract_mip_sensor_comp_orientation_matrix_data_from_field(const struct mip /// EQSTART p^{veh} = q^{-1} p^{ned} q EQEND
/// /// Where:
-/// EQSTART q = (q_w, q_x, q_y, q_z) ENDEQ is the quaternion desrcribing the rotation.
-/// EQSTART p^ned = (0, v^{ned}_x, v^{ned}_y, v^{ned}_z) ENDEQ and EQSTART v^{ned} EQEND is a 3-element vector expressed in the NED frame.
-/// EQSTART p^veh = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) ENDEQ and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
+/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion describing the rotation.
+/// EQSTART p^ned = (0, v^{ned}_x, v^{ned}_y, v^{ned}_z) EQEND and EQSTART v^{ned} EQEND is a 3-element vector expressed in the NED frame.
+/// EQSTART p^veh = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) EQEND and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
/// ///@{ @@ -415,7 +415,7 @@ bool extract_mip_sensor_pps_timestamp_data_from_field(const struct mip_field* fi /// Upon recovering from a PPS outage, the user should expect a jump in the reported GPS time due to the accumulation of internal clock error. /// If synchronization to an external clock or onboard GNSS receiver (for products that have one) is disabled, this time is equivalent to internal system time. /// -/// Note: this data field may be deprecrated in the future. The more flexible shared data field (0x80, 0xD3) should be used instead. +/// Note: this data field may be deprecated in the future. The more flexible shared data field (0x80, 0xD3) should be used instead. /// ///@{ diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 34062130c..9794957de 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -327,9 +327,9 @@ void extract(Serializer& serializer, CompOrientationMatrix& self); /// EQSTART p^{veh} = q^{-1} p^{ned} q EQEND
/// /// Where:
-/// EQSTART q = (q_w, q_x, q_y, q_z) ENDEQ is the quaternion desrcribing the rotation.
-/// EQSTART p^ned = (0, v^{ned}_x, v^{ned}_y, v^{ned}_z) ENDEQ and EQSTART v^{ned} EQEND is a 3-element vector expressed in the NED frame.
-/// EQSTART p^veh = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) ENDEQ and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
+/// EQSTART q = (q_w, q_x, q_y, q_z) EQEND is the quaternion describing the rotation.
+/// EQSTART p^ned = (0, v^{ned}_x, v^{ned}_y, v^{ned}_z) EQEND and EQSTART v^{ned} EQEND is a 3-element vector expressed in the NED frame.
+/// EQSTART p^veh = (0, v^{veh}_x, v^{veh}_y, v^{veh}_z) EQEND and EQSTART v^{veh} EQEND is a 3-element vector expressed in the vehicle frame.
/// ///@{ @@ -465,7 +465,7 @@ void extract(Serializer& serializer, PpsTimestamp& self); /// Upon recovering from a PPS outage, the user should expect a jump in the reported GPS time due to the accumulation of internal clock error. /// If synchronization to an external clock or onboard GNSS receiver (for products that have one) is disabled, this time is equivalent to internal system time. /// -/// Note: this data field may be deprecrated in the future. The more flexible shared data field (0x80, 0xD3) should be used instead. +/// Note: this data field may be deprecated in the future. The more flexible shared data field (0x80, 0xD3) should be used instead. /// ///@{ From fcb66d663cf9c4c41c8557de2b0536437f5cbd6e Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 26 Sep 2022 11:28:34 -0400 Subject: [PATCH 008/252] Generate MIP definitions from branches/dev@53581. --- src/mip/definitions/data_gnss.h | 16 ++++++++-------- src/mip/definitions/data_gnss.hpp | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index a4121b879..d62f57310 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -595,14 +595,14 @@ void extract_mip_gnss_hw_status_data_valid_flags(struct mip_serializer* serializ /// GNSS reported DGNSS status /// ///
Possible Base Station Status Values:
-///
  0 – UDRE Scale Factor = 1.0
-///
  1 – UDRE Scale Factor = 0.75
-///
  2 – UDRE Scale Factor = 0.5
-///
  3 – UDRE Scale Factor = 0.3
-///
  4 – UDRE Scale Factor = 0.2
-///
  5 – UDRE Scale Factor = 0.1
-///
  6 – Reference Station Transmission Not Monitored
-///
  7 – Reference Station Not Working
+///
  0 - UDRE Scale Factor = 1.0
+///
  1 - UDRE Scale Factor = 0.75
+///
  2 - UDRE Scale Factor = 0.5
+///
  3 - UDRE Scale Factor = 0.3
+///
  4 - UDRE Scale Factor = 0.2
+///
  5 - UDRE Scale Factor = 0.1
+///
  6 - Reference Station Transmission Not Monitored
+///
  7 - Reference Station Not Working
/// /// (UDRE = User Differential Range Error) /// diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 010702097..5a279e844 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -763,14 +763,14 @@ void extract(Serializer& serializer, HwStatus& self); /// GNSS reported DGNSS status /// ///
Possible Base Station Status Values:
-///
  0 – UDRE Scale Factor = 1.0
-///
  1 – UDRE Scale Factor = 0.75
-///
  2 – UDRE Scale Factor = 0.5
-///
  3 – UDRE Scale Factor = 0.3
-///
  4 – UDRE Scale Factor = 0.2
-///
  5 – UDRE Scale Factor = 0.1
-///
  6 – Reference Station Transmission Not Monitored
-///
  7 – Reference Station Not Working
+///
  0 - UDRE Scale Factor = 1.0
+///
  1 - UDRE Scale Factor = 0.75
+///
  2 - UDRE Scale Factor = 0.5
+///
  3 - UDRE Scale Factor = 0.3
+///
  4 - UDRE Scale Factor = 0.2
+///
  5 - UDRE Scale Factor = 0.1
+///
  6 - Reference Station Transmission Not Monitored
+///
  7 - Reference Station Not Working
/// /// (UDRE = User Differential Range Error) /// From 9a736a2f9d00f8e1108ed48dcf1f7703b352c9b6 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Tue, 27 Sep 2022 12:54:59 -0400 Subject: [PATCH 009/252] Recording connection (#24) * Adds first pass at recording connection * Adds documentation to some functions * Updates docs for connection objects, and updates cmake file to not compile C examples if serial support is not enabled * Reorders recording connection wrapper to fix compiler error * Fix swapped send/recv in documentation block. * Updates based on PR review * Updates readme * Updates forgotten references to WITH flags Co-authored-by: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> --- CMakeLists.txt | 40 ++++++++++------ README.md | 11 +++-- examples/CMakeLists.txt | 31 ++++++++----- examples/example_utils.cpp | 40 ++++++++++++++-- examples/example_utils.hpp | 11 ++++- src/mip/extras/recording_connection.cpp | 37 +++++++++++++++ src/mip/extras/recording_connection.hpp | 62 +++++++++++++++++++++++++ src/mip/mip_device.hpp | 11 +++++ src/mip/platform/serial_connection.cpp | 11 ++++- src/mip/platform/serial_connection.hpp | 14 +++++- src/mip/platform/tcp_connection.cpp | 11 ++++- src/mip/platform/tcp_connection.hpp | 10 ++++ src/mip/utils/serial_port.h | 2 +- src/mip/utils/tcp_socket.h | 2 +- 14 files changed, 249 insertions(+), 44 deletions(-) create mode 100644 src/mip/extras/recording_connection.cpp create mode 100644 src/mip/extras/recording_connection.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fcde1f1af..5ff3b16e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,8 +70,9 @@ configure_file(${VERSION_IN_FILE} ${VERSION_OUT_FILE}) # Build options # -option(WITH_SERIAL "Build serial connection support into the library and examples" ON) -option(WITH_TCP "Build TCP connection support into the library and exampels" ON) +option(MIP_USE_SERIAL "Build serial connection support into the library and examples" ON) +option(MIP_USE_TCP "Build TCP connection support into the library and exampels" ON) +option(MIP_USE_EXTRAS "Build extra support into the library including some things that might work at a higher level and use dynamically allocated memory" ON) set(MIP_TIMESTAMP_TYPE "uint64_t" CACHE STRING "Override the type used for received data timestamps and timeouts (must be unsigned or at least 64 bits).") @@ -152,7 +153,7 @@ set(MIPDEF_SOURCES string(REPLACE ".h" ".hpp" MIPDEF_HPP_SOURCES "${MIPDEF_SOURCES}") string(REPLACE ".c" ".cpp" MIPDEF_CPP_SOURCES "${MIPDEF_HPP_SOURCES}") -if(WITH_SERIAL) +if(MIP_USE_SERIAL) list(APPEND UTILS_SOURCES "${UTILS_DIR}/serial_port.c" "${UTILS_DIR}/serial_port.h" @@ -162,7 +163,7 @@ if(WITH_SERIAL) "${MIP_DIR}/platform/serial_connection.cpp" ) endif() -if(WITH_TCP) +if(MIP_USE_TCP) list(APPEND UTILS_SOURCES "${UTILS_DIR}/tcp_socket.c" "${UTILS_DIR}/tcp_socket.h" @@ -172,6 +173,13 @@ if(WITH_TCP) "${MIP_DIR}/platform/tcp_connection.cpp" ) endif() +if(MIP_USE_EXTRAS) + set(MIP_EXTRAS_DIR "${MIP_DIR}/extras") + set(MIP_EXTRA_SOURCES + "${MIP_EXTRAS_DIR}/recording_connection.hpp" + "${MIP_EXTRAS_DIR}/recording_connection.cpp" + ) +endif() set(ALL_MIP_SOURCES ${MIPDEF_SOURCES} @@ -181,18 +189,16 @@ set(ALL_MIP_SOURCES ${UTILS_SOURCES} ${MIP_CPP_HEADERS} ${MIP_INTERFACE_SOURCES} + ${MIP_EXTRA_SOURCES} ) - option(MIP_DISABLE_CPP "Excludes all C++ files from the project." OFF) if(MIP_DISABLE_CPP) list(FILTER ALL_MIP_SOURCES EXCLUDE REGEX "[c|h]pp$") endif() - add_library(mip ${ALL_MIP_SOURCES}) - if(${MIP_TIMESTAMP_TYPE}) add_compile_definitions("MIP_TIMESTAMP_TYPE=${MIP_TIMESTAMP_TYPE}") endif() @@ -310,12 +316,20 @@ write_basic_package_version_file( # Installation # -install(DIRECTORY - "${SRC_DIR}/mip" - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" - COMPONENT mip - FILES_MATCHING PATTERN "*.h*" -) +# Only install headers that we build the source files for +set(ALL_MIP_HEADERS "${ALL_MIP_SOURCES}") +list(FILTER ALL_MIP_HEADERS INCLUDE REGEX "^.*\.(h|hpp)$") +foreach(MIP_HEADER ${ALL_MIP_HEADERS}) + string(REPLACE "${SRC_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER}") + set(MIP_HEADER_DESTINATION_FULL "${CMAKE_INSTALL_INCLUDEDIR}/${MIP_HEADER_RELATIVE}") + get_filename_component(MIP_HEADER_DESTINATION "${MIP_HEADER_DESTINATION_FULL}" DIRECTORY) + install( + FILES "${MIP_HEADER}" + DESTINATION "${MIP_HEADER_DESTINATION}" + COMPONENT mip + ) +endforeach() + install(TARGETS mip EXPORT mip-targets diff --git a/README.md b/README.md index c35ed8858..a94297823 100644 --- a/README.md +++ b/README.md @@ -57,7 +57,7 @@ A basic serial port interface is provided in C and C++ for Linux and Windows. Th The serial port connection will be used in most cases, when the MIP device is connected via a serial or USB cable (the USB connection acts like a virtual serial port). -Enable it in the CMake configuration with `-DWITH_SERIAL=1`. +Enable it in the CMake configuration with `-DMIP_USE_SERIAL=1`. ### TCP Client @@ -65,7 +65,7 @@ The TCP client connection allows you to connect to a MIP device remotely. The MI via the normal serial or USB cable to a commputer system running a TCP server which forwards data between the serial port and TCP clients. -Enable it in the CMake configuration with `-DWITH_TCP=1`. +Enable it in the CMake configuration with `-DMIP_USE_TCP=1`. How to Build @@ -85,8 +85,9 @@ How to Build ### Build configuration The following options may be specified when configuring the build with CMake (e.g. `cmake .. -DOPTION=VALUE`): -* WITH_SERIAL - Builds the included serial port library (default enabled). -* WITH_TCP - Builds the included socket library (default enabled). +* MIP_USE_SERIAL - Builds the included serial port library (default enabled). +* MIP_USE_TCP - Builds the included socket library (default enabled). +* MIP_USE_EXTRAS - Builds some higher level utility classes and functions that may use dynamic memory. * BUILD_EXAMPLES - If enabled (`-DBUILD_EXAMPLES=ON`), the example projects will be built (default disabled). * BUILD_TESTING - If enabled (`-DBUILD_TESTING=ON`), the test programs in the /test directory will be compiled and linked. Run the tests with `ctest`. * BUILD_DOCUMENTATION - If enabled, the documentation will be built with doxygen. You must have doxygen installed. @@ -100,7 +101,7 @@ The following options may be specified when configuring the build with CMake (e. 1. Create the build directory (e.g. `mkdir build`). 2. In the build directory, run `cmake .. ` - * Replace `` with your configuration options, such as `-DWITH_SERIAL=1`. + * Replace `` with your configuration options, such as `-DMIP_USE_SERIAL=1`. * You can use `cmake-gui ..` instead if you'd prefer to use the GUI tool (and have it installed). * An alternative generator may be used, such as ninja, code blocks, etc. by specifying `-G ` 3. Invoke `cmake --build .` in the build directory diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 033973966..7a29e5ec9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -7,46 +7,56 @@ set(DEVICE_SOURCES "${EXAMPLE_DIR}/example_utils.hpp" ) -if(WITH_SERIAL) +if(MIP_USE_SERIAL) set(SERIAL_DEFS "MIP_USE_SERIAL") endif() -if(WITH_TCP) +if(MIP_USE_TCP) set(TCP_DEFS "MIP_USE_TCP") endif() +if(MIP_USE_EXTRAS) + set(EXTRAS_DEFS "MIP_USE_EXTRAS") +endif() + +set(MIP_EXAMPLE_DEFS ${SERIAL_DEFS} ${TCP_DEFS} ${EXTRAS_DEFS}) -if(WITH_SERIAL OR WITH_TCP) +# C++ examples need either serial or TCP support +if(MIP_USE_SERIAL OR MIP_USE_TCP) if(NOT MIP_DISABLE_CPP) add_executable(DeviceInfo "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/device_info.cpp" ${DEVICE_SOURCES}) target_link_libraries(DeviceInfo mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(DeviceInfo PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_compile_definitions(DeviceInfo PUBLIC "${MIP_EXAMPLE_DEFS}") add_executable(WatchImu "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/watch_imu.cpp" ${DEVICE_SOURCES}) target_link_libraries(WatchImu mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(WatchImu PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_compile_definitions(WatchImu PUBLIC "${MIP_EXAMPLE_DEFS}") find_package(Threads REQUIRED) add_executable(ThreadingDemo "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/threading.cpp" ${DEVICE_SOURCES}) target_link_libraries(ThreadingDemo mip "${SERIAL_LIB}" "${SOCKET_LIB}" "${CMAKE_THREAD_LIBS_INIT}") - target_compile_definitions(ThreadingDemo PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_compile_definitions(ThreadingDemo PUBLIC "${MIP_EXAMPLE_DEFS}") add_executable(GQ7_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GQ7/GQ7_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(GQ7_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(GQ7_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_compile_definitions(GQ7_Example PUBLIC "${MIP_EXAMPLE_DEFS}") add_executable(CV7_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7/CV7_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(CV7_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(CV7_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_compile_definitions(CV7_Example PUBLIC "${MIP_EXAMPLE_DEFS}") add_executable(GX5_45_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GX5_45/GX5_45_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(GX5_45_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_compile_definitions(GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") endif() +endif() + +# C examples need serial support +if(MIP_USE_SERIAL) add_executable(WatchImuC "${EXAMPLE_DIR}/watch_imu.c") target_link_libraries(WatchImuC mip) @@ -58,5 +68,4 @@ if(WITH_SERIAL OR WITH_TCP) add_executable(GX5_45_ExampleC "${EXAMPLE_DIR}/GX5_45/GX5_45_example.c") target_link_libraries(GX5_45_ExampleC mip) - -endif() +endif() \ No newline at end of file diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index 8009adfc4..53f5771de 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -17,9 +17,21 @@ mip::Timestamp getCurrentTimestamp() } -std::unique_ptr openFromArgs(const std::string& port_or_hostname, const std::string& baud_or_port) +std::unique_ptr openFromArgs(const std::string& port_or_hostname, const std::string& baud_or_port, const std::string& binary_file_path) { auto example_utils = std::unique_ptr(new ExampleUtils()); + + if( !binary_file_path.empty() ) + { +#ifdef MIP_USE_EXTRAS + example_utils->recordedFile = std::unique_ptr(new std::ofstream(binary_file_path)); + if( !example_utils->recordedFile->is_open() ) + throw std::runtime_error("Unable to open binary file"); +#else // MIP_USE_EXTRAS + throw std::runtime_error("The program was compiled without binary file recording support. Recompile with -DMIP_USE_EXTRAS=ON"); +#endif // MIP_USE_EXTRAS + } + if(port_or_hostname.find(PORT_KEY) == std::string::npos) // Not a serial port { @@ -28,7 +40,14 @@ std::unique_ptr openFromArgs(const std::string& port_or_hostname, if( port < 1024 || port > 65535 ) throw std::runtime_error("Invalid TCP port (must be between 1024 and 65535."); - example_utils->connection = std::unique_ptr(new mip::platform::TcpConnection(port_or_hostname, port)); +#ifdef MIP_USE_EXTRAS + using RecordingTcpConnection = mip::extras::RecordingConnectionWrapper; + example_utils->connection = std::unique_ptr(new RecordingTcpConnection(example_utils->recordedFile.get(), example_utils->recordedFile.get(), port_or_hostname, port)); +#else // MIP_USE_EXTRAS + using TcpConnection = mip::platform::TcpConnection; + example_utils->connection = std::unique_ptr(new TcpConnection(port_or_hostname, port)); +#endif // MIP_USE_EXTRAS + example_utils->device = std::unique_ptr(new mip::DeviceInterface(example_utils->connection.get(), example_utils->buffer, sizeof(example_utils->buffer), 1000, 2000)); #else // MIP_USE_TCP throw std::runtime_error("This program was compiled without socket support. Recompile with -DMIP_USE_TCP=1"); @@ -43,7 +62,13 @@ std::unique_ptr openFromArgs(const std::string& port_or_hostname, if( baud == 0 ) throw std::runtime_error("Serial baud rate must be a decimal integer greater than 0."); - example_utils->connection = std::unique_ptr(new mip::platform::SerialConnection(port_or_hostname, baud)); +#ifdef MIP_USE_EXTRAS + using RecordingSerialConnection = mip::extras::RecordingConnectionWrapper; + example_utils->connection = std::unique_ptr(new RecordingSerialConnection(example_utils->recordedFile.get(), example_utils->recordedFile.get(), port_or_hostname, baud)); +#else // MIP_USE_EXTRAS + using SerialConnection = mip::platform::SerialConnection; + example_utils->connection = std::unique_ptr(new SerialConnection(port_or_hostname, baud)); +#endif // MIP_USE_EXTRAS example_utils->device = std::unique_ptr(new mip::DeviceInterface(example_utils->connection.get(), example_utils->buffer, sizeof(example_utils->buffer), mip::C::mip_timeout_from_baudrate(baud), 500)); #else // MIP_USE_SERIAL throw std::runtime_error("This program was compiled without serial support. Recompile with -DMIP_USE_SERIAL=1.\n"); @@ -59,12 +84,17 @@ std::unique_ptr handleCommonArgs(int argc, const char* argv[], int throw std::underflow_error("Usage error"); } - return openFromArgs(argv[1], argv[2]); + // If we were passed a file name, record the data in that file + std::string binary_file_path = ""; + if (argc >= 4) + binary_file_path = argv[3]; + + return openFromArgs(argv[1], argv[2], binary_file_path); } int printCommonUsage(const char* argv[]) { - fprintf(stderr, "Usage: %s \nUsage: %s \n", argv[0], argv[0]); + fprintf(stderr, "Usage: %s \nUsage: %s \n", argv[0], argv[0]); return 1; } diff --git a/examples/example_utils.hpp b/examples/example_utils.hpp index fe29d4dcf..a95376728 100644 --- a/examples/example_utils.hpp +++ b/examples/example_utils.hpp @@ -7,23 +7,30 @@ #ifdef MIP_USE_TCP #include "mip/platform/tcp_connection.hpp" #endif +#ifdef MIP_USE_EXTRAS + #include "mip/extras/recording_connection.hpp" +#endif + #include #include #include +#include +#include #include struct ExampleUtils { std::unique_ptr connection; std::unique_ptr device; + std::unique_ptr recordedFile; uint8_t buffer[1024]; }; mip::Timestamp getCurrentTimestamp(); -std::unique_ptr openFromArgs(const std::string& port_or_hostname, const std::string& baud_or_port); +std::unique_ptr openFromArgs(const std::string& port_or_hostname, const std::string& baud_or_port, const std::string& binary_file_path); -std::unique_ptr handleCommonArgs(int argc, const char* argv[], int maxArgs=3); +std::unique_ptr handleCommonArgs(int argc, const char* argv[], int maxArgs=4); int printCommonUsage(const char* argv[]); diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp new file mode 100644 index 000000000..82bd86208 --- /dev/null +++ b/src/mip/extras/recording_connection.cpp @@ -0,0 +1,37 @@ +#include "recording_connection.hpp" + +namespace mip +{ +namespace extras +{ + +///@brief Creates a RecordingConnection that will write received bytes to recvStream, and sent bytes to sendStream +/// +///@param connection Connection object that will actually communicate with the device +///@param recvStream The stream to write to when bytes are received. Null if received bytes should not be written to a stream +///@param sendStream The stream to write to when bytes are sent. Null if sent bytes should not be written to a stream +RecordingConnection::RecordingConnection(Connection* connection, std::ostream* recvStream, std::ostream* sendStream) : + mConnection(connection), mRecvFile(recvStream), mSendFile(sendStream) +{ +} + +///@copydoc mip::Connection::sendToDevice +bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) +{ + const bool ok = mConnection->sendToDevice(data, length); + if( ok && mSendFile != nullptr ) + mSendFile->write(reinterpret_cast(data), length); + return ok; +} + +///@copydoc mip::Connection::recvFromDevice +bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* count_out, Timestamp* timestamp_out) +{ + const bool ok = mConnection->recvFromDevice(buffer, max_length, count_out, timestamp_out); + if( ok && mRecvFile != nullptr ) + mRecvFile->write(reinterpret_cast(buffer), *count_out); + return ok; +} + +} // namespace extras +} // namespace mip \ No newline at end of file diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp new file mode 100644 index 000000000..d05ea8069 --- /dev/null +++ b/src/mip/extras/recording_connection.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include +#include +#include + +namespace mip +{ +namespace extras +{ + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_extras Extra utilities +///@{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief Can be used with another connection to communicate with a device, and record the data at the same time +/// +class RecordingConnection : public Connection +{ +public: + RecordingConnection(Connection* connection, std::ostream* recvStream=nullptr, std::ostream* sendStream=nullptr); + + bool sendToDevice(const uint8_t* data, size_t length) final; + bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* count_out, Timestamp* timestamp_out) final; + +protected: + Connection* mConnection; + + // Files may be NULL to not record one direction or the other + std::ostream* mRecvFile; + std::ostream* mSendFile; +}; + +//////////////////////////////////////////////////////////////////////////////// +///@brief Template wrapper for a recording connection. +/// +///@param ConnectionType The type of connection used to actually communicate. +/// +template +class RecordingConnectionWrapper : public RecordingConnection +{ +public: + ///@brief Creates a RecordingConnectionWrapper that will write received bytes to recvStream, sent bytes to sendStream, and construct a connection object from args + /// + ///@param recvStream The stream to write to when bytes are received. Null if received bytes should not be written to a stream + ///@param sendStream The stream to write to when bytes are sent. Null if sent bytes should not be written to a stream + ///@param args Arguments required to construct the ConnectionType + template + RecordingConnectionWrapper(std::ostream* recvStream, std::ostream* sendStream, Args&&... args) : RecordingConnection(new ConnectionType(std::forward(args)...), recvStream, sendStream) {} + + ///@brief Deconstructs the RecordingConnectionWrapper as well as the underlying connection object made in the constructor + ~RecordingConnectionWrapper() { delete mConnection; } +}; + +///@} +//////////////////////////////////////////////////////////////////////////////// + +} // namespace extras +} // namespace mip diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index a972abf80..28b51fb07 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -135,7 +135,18 @@ template bool startCommand(C::mip_interface& device, C::mip_pending_c class Connection { public: + ///@brief Sends bytes to the device + /// + ///@param data The data to send to the device + ///@param length Length of data in bytes virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; // Must be implemented by a derived class. + + ///@brief Receives bytes from the device + /// + ///@param buffer Buffer to store the received data in + ///@param max_length Max number of bytes that can be read. Should be at most the length of buffer. + ///@param length_out Number of bytes actually read. + ///@param timestamp Timestamp of when the data was received virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, Timestamp* timestamp) = 0; // Must be implemented by a derived class. }; diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index e3d759b36..f31722e06 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -9,28 +9,35 @@ namespace mip namespace platform { +///@brief Creates a SerialConnection that will communicate with a device over serial +/// +///@param portName Path to the port to connect to. On Windows, this usually looks like "COM", on linux, "/dev/tty" +///@param baudrate Baud rate to open the device at. Note that the device needs to be configured to SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrate) { if (!serial_port_open(&mPort, portName.c_str(), baudrate)) throw std::runtime_error("Unable to open serial port"); } +///@brief Closes the underlying serial port SerialConnection::~SerialConnection() { serial_port_close(&mPort); } +///@copydoc mip::Connection::recvFromDevice bool SerialConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) { *timestamp = getCurrentTimestamp(); return serial_port_read(&mPort, buffer, max_length, length_out); } +///@copydoc mip::Connection::sendToDevice bool SerialConnection::sendToDevice(const uint8_t* data, size_t length) { size_t length_out; return serial_port_write(&mPort, data, length, &length_out); } -}; // namespace platform -}; // namespace mip \ No newline at end of file +} // namespace platform +} // namespace mip diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index 6aed1a30a..1e0e9f510 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -13,6 +13,13 @@ namespace mip namespace platform { +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_platform +///@{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief Can be used on Windows, OSX, or linux to communicate with a MIP device over serial +/// class SerialConnection : public mip::Connection { public: @@ -27,5 +34,8 @@ class SerialConnection : public mip::Connection serial_port mPort; }; -}; // namespace platform -}; // namespace mip \ No newline at end of file +///@} +//////////////////////////////////////////////////////////////////////////////// + +} // namespace platform +} // namespace mip \ No newline at end of file diff --git a/src/mip/platform/tcp_connection.cpp b/src/mip/platform/tcp_connection.cpp index a3fb21bf4..9d63a51fb 100644 --- a/src/mip/platform/tcp_connection.cpp +++ b/src/mip/platform/tcp_connection.cpp @@ -10,28 +10,35 @@ namespace mip namespace platform { +///@brief Creates a TcpConnection that will communicate with a device over TCP +/// +///@param hostName Host name or IP address to connect to +///@param port Port on hostName to connect to TcpConnection::TcpConnection(const std::string& hostname, uint16_t port) { if (!tcp_socket_open(&mSocket, hostname.c_str(), port, 3000)) throw std::runtime_error("Unable to open TCP socket"); } +///@brief Closes the underlying TCP socket TcpConnection::~TcpConnection() { tcp_socket_close(&mSocket); } +///@copydoc mip::Connection::sendToDevice bool TcpConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) { *timestamp = getCurrentTimestamp(); return tcp_socket_recv(&mSocket, buffer, max_length, length_out); } +///@copydoc mip::Connection::recvFromDevice bool TcpConnection::sendToDevice(const uint8_t* data, size_t length) { size_t length_out; return tcp_socket_send(&mSocket, data, length, &length_out); } -}; // namespace platform -}; // namespace mip \ No newline at end of file +} // namespace platform +} // namespace mip \ No newline at end of file diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index 9c44d384b..426cb40a4 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -14,6 +14,13 @@ namespace mip namespace platform { +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_platform +///@{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief Can be used on Windows, OSX, or linux to communicate with a MIP device over TCP +/// class TcpConnection : public mip::Connection { public: @@ -28,5 +35,8 @@ class TcpConnection : public mip::Connection tcp_socket mSocket; }; +///@} +//////////////////////////////////////////////////////////////////////////////// + }; // namespace platform }; // namespace mip \ No newline at end of file diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index a19cf4135..a61a65ab9 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -21,7 +21,7 @@ extern "C" { #endif //////////////////////////////////////////////////////////////////////////////// -///@addtogroup mip_extras Extra utilities +///@addtogroup mip_platform Platform specific utilities /// ///@{ diff --git a/src/mip/utils/tcp_socket.h b/src/mip/utils/tcp_socket.h index 38a582384..f2dc86f5c 100644 --- a/src/mip/utils/tcp_socket.h +++ b/src/mip/utils/tcp_socket.h @@ -9,7 +9,7 @@ extern "C" { #endif //////////////////////////////////////////////////////////////////////////////// -///@addtogroup mip_extras +///@addtogroup mip_platform /// ///@{ From 90f3a960d819721907d5f15139db3e51ba08c6ae Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 6 Oct 2022 13:44:36 -0400 Subject: [PATCH 010/252] Generate MIP definitions from branches/field-index@53647. (#28) --- src/mip/definitions/commands_base.h | 4 +- src/mip/definitions/commands_base.hpp | 4 +- src/mip/definitions/data_filter.hpp | 275 ++++++++++++++++++++++++++ src/mip/definitions/data_gnss.hpp | 135 +++++++++++++ src/mip/definitions/data_sensor.hpp | 115 +++++++++++ src/mip/definitions/data_shared.hpp | 45 +++++ src/mip/definitions/data_system.hpp | 20 ++ 7 files changed, 594 insertions(+), 4 deletions(-) diff --git a/src/mip/definitions/commands_base.h b/src/mip/definitions/commands_base.h index 242e59ce6..f94559e66 100644 --- a/src/mip/definitions/commands_base.h +++ b/src/mip/definitions/commands_base.h @@ -173,8 +173,8 @@ mip_cmd_result mip_base_get_device_info(struct mip_interface* device, mip_base_d struct mip_base_get_device_descriptors_response { - uint8_t descriptors_count; uint16_t* descriptors; + uint8_t descriptors_count; }; typedef struct mip_base_get_device_descriptors_response mip_base_get_device_descriptors_response; @@ -229,8 +229,8 @@ mip_cmd_result mip_base_resume(struct mip_interface* device); struct mip_base_get_extended_descriptors_response { - uint8_t descriptors_count; uint16_t* descriptors; + uint8_t descriptors_count; }; typedef struct mip_base_get_extended_descriptors_response mip_base_get_extended_descriptors_response; diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 5d1da353c..42a9128c2 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -230,8 +230,8 @@ struct GetDeviceDescriptors static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; - uint8_t descriptors_count = 0; uint16_t* descriptors = {nullptr}; + uint8_t descriptors_count = 0; }; }; @@ -326,8 +326,8 @@ struct GetExtendedDescriptors static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; - uint8_t descriptors_count = 0; uint16_t* descriptors = {nullptr}; + uint8_t descriptors_count = 0; }; }; diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index bea60c5f6..c878a52e8 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -247,6 +247,11 @@ struct PositionLlh static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(latitude,longitude,ellipsoid_height,valid_flags); + } + double latitude = 0; ///< [degrees] double longitude = 0; ///< [degrees] double ellipsoid_height = 0; ///< [meters] @@ -271,6 +276,11 @@ struct VelocityNed static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(north,east,down,valid_flags); + } + float north = 0; ///< [meters/second] float east = 0; ///< [meters/second] float down = 0; ///< [meters/second] @@ -303,6 +313,11 @@ struct AttitudeQuaternion static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(q,valid_flags); + } + float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -335,6 +350,11 @@ struct AttitudeDcm static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(dcm,valid_flags); + } + float dcm[9] = {0}; ///< Matrix elements in row-major order. uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -358,6 +378,11 @@ struct EulerAngles static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(roll,pitch,yaw,valid_flags); + } + float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] @@ -382,6 +407,11 @@ struct GyroBias static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias,valid_flags); + } + float bias[3] = {0}; ///< (x, y, z) [radians/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -404,6 +434,11 @@ struct AccelBias static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias,valid_flags); + } + float bias[3] = {0}; ///< (x, y, z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -426,6 +461,11 @@ struct PositionLlhUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(north,east,down,valid_flags); + } + float north = 0; ///< [meters] float east = 0; ///< [meters] float down = 0; ///< [meters] @@ -450,6 +490,11 @@ struct VelocityNedUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(north,east,down,valid_flags); + } + float north = 0; ///< [meters/second] float east = 0; ///< [meters/second] float down = 0; ///< [meters/second] @@ -475,6 +520,11 @@ struct EulerAnglesUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(roll,pitch,yaw,valid_flags); + } + float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] @@ -499,6 +549,11 @@ struct GyroBiasUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias_uncert,valid_flags); + } + float bias_uncert[3] = {0}; ///< (x,y,z) [radians/sec] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -521,6 +576,11 @@ struct AccelBiasUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias_uncert,valid_flags); + } + float bias_uncert[3] = {0}; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -549,6 +609,11 @@ struct Timestamp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(tow,week_number,valid_flags); + } + double tow = 0; ///< GPS Time of Week [seconds] uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -572,6 +637,11 @@ struct Status static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(filter_state,dynamics_mode,status_flags); + } + FilterMode filter_state = static_cast(0); ///< Device-specific filter state. Please consult the user manual for definition. FilterDynamicsMode dynamics_mode = static_cast(0); ///< Device-specific dynamics mode. Please consult the user manual for definition. FilterStatusFlags status_flags; ///< Device-specific status flags. Please consult the user manual for definition. @@ -596,6 +666,11 @@ struct LinearAccel static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(accel,valid_flags); + } + float accel[3] = {0}; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -618,6 +693,11 @@ struct GravityVector static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(gravity,valid_flags); + } + float gravity[3] = {0}; ///< (x, y, z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -640,6 +720,11 @@ struct CompAccel static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(accel,valid_flags); + } + float accel[3] = {0}; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -662,6 +747,11 @@ struct CompAngularRate static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(gyro,valid_flags); + } + float gyro[3] = {0}; ///< (x, y, z) [radians/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -684,6 +774,11 @@ struct QuaternionAttitudeUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(q,valid_flags); + } + float q[4] = {0}; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -706,6 +801,11 @@ struct Wgs84GravityMag static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(magnitude,valid_flags); + } + float magnitude = 0; ///< [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -731,6 +831,11 @@ struct HeadingUpdateState static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(heading,heading_1sigma,source,valid_flags); + } + enum class HeadingSource : uint16_t { NONE = 0, ///< @@ -765,6 +870,11 @@ struct MagneticModel static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(intensity_north,intensity_east,intensity_down,inclination,declination,valid_flags); + } + float intensity_north = 0; ///< [Gauss] float intensity_east = 0; ///< [Gauss] float intensity_down = 0; ///< [Gauss] @@ -791,6 +901,11 @@ struct AccelScaleFactor static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scale_factor,valid_flags); + } + float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -813,6 +928,11 @@ struct AccelScaleFactorUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scale_factor_uncert,valid_flags); + } + float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -835,6 +955,11 @@ struct GyroScaleFactor static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scale_factor,valid_flags); + } + float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -857,6 +982,11 @@ struct GyroScaleFactorUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scale_factor_uncert,valid_flags); + } + float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -879,6 +1009,11 @@ struct MagBias static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias,valid_flags); + } + float bias[3] = {0}; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -901,6 +1036,11 @@ struct MagBiasUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias_uncert,valid_flags); + } + float bias_uncert[3] = {0}; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -925,6 +1065,11 @@ struct StandardAtmosphere static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(geometric_altitude,geopotential_altitude,standard_temperature,standard_pressure,standard_density,valid_flags); + } + float geometric_altitude = 0; ///< Input into calculation [meters] float geopotential_altitude = 0; ///< [meters] float standard_temperature = 0; ///< [degC] @@ -955,6 +1100,11 @@ struct PressureAltitude static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(pressure_altitude,valid_flags); + } + float pressure_altitude = 0; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -976,6 +1126,11 @@ struct DensityAltitude static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(density_altitude,valid_flags); + } + float density_altitude = 0; ///< m uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1000,6 +1155,11 @@ struct AntennaOffsetCorrection static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(offset,valid_flags); + } + float offset[3] = {0}; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1022,6 +1182,11 @@ struct AntennaOffsetCorrectionUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(offset_uncert,valid_flags); + } + float offset_uncert[3] = {0}; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1046,6 +1211,11 @@ struct MultiAntennaOffsetCorrection static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(receiver_id,offset,valid_flags); + } + uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached float offset[3] = {0}; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1069,6 +1239,11 @@ struct MultiAntennaOffsetCorrectionUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(receiver_id,offset_uncert,valid_flags); + } + uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached float offset_uncert[3] = {0}; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1094,6 +1269,11 @@ struct MagnetometerOffset static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(hard_iron,valid_flags); + } + float hard_iron[3] = {0}; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1118,6 +1298,11 @@ struct MagnetometerMatrix static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(soft_iron,valid_flags); + } + float soft_iron[9] = {0}; ///< Row-major [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1140,6 +1325,11 @@ struct MagnetometerOffsetUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(hard_iron_uncertainty,valid_flags); + } + float hard_iron_uncertainty[3] = {0}; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1162,6 +1352,11 @@ struct MagnetometerMatrixUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(soft_iron_uncertainty,valid_flags); + } + float soft_iron_uncertainty[9] = {0}; ///< Row-major [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1183,6 +1378,11 @@ struct MagnetometerCovarianceMatrix static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(covariance,valid_flags); + } + float covariance[9] = {0}; uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1205,6 +1405,11 @@ struct MagnetometerResidualVector static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(residual,valid_flags); + } + float residual[3] = {0}; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1227,6 +1432,11 @@ struct ClockCorrection static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(receiver_id,bias,bias_drift,valid_flags); + } + uint8_t receiver_id = 0; ///< 1, 2, etc. float bias = 0; ///< [seconds] float bias_drift = 0; ///< [seconds/second] @@ -1251,6 +1461,11 @@ struct ClockCorrectionUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(receiver_id,bias_uncertainty,bias_drift_uncertainty,valid_flags); + } + uint8_t receiver_id = 0; ///< 1, 2, etc. float bias_uncertainty = 0; ///< [seconds] float bias_drift_uncertainty = 0; ///< [seconds/second] @@ -1275,6 +1490,11 @@ struct GnssPosAidStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(receiver_id,time_of_week,status,reserved); + } + uint8_t receiver_id = 0; float time_of_week = 0; ///< Last GNSS aiding measurement time of week [seconds] GnssAidStatusFlags status; ///< Aiding measurement status bitfield @@ -1299,6 +1519,11 @@ struct GnssAttAidStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,status,reserved); + } + float time_of_week = 0; ///< Last valid aiding measurement time of week [seconds] [processed instead of measured?] GnssAidStatusFlags status; ///< Last valid aiding measurement status bitfield uint8_t reserved[8] = {0}; @@ -1322,6 +1547,11 @@ struct HeadAidStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,type,reserved); + } + enum class HeadingAidType : uint8_t { DUAL_ANTENNA = 1, ///< @@ -1351,6 +1581,11 @@ struct RelPosNed static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(relative_position,valid_flags); + } + double relative_position[3] = {0}; ///< [meters, NED] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1373,6 +1608,11 @@ struct EcefPos static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(position_ecef,valid_flags); + } + double position_ecef[3] = {0}; ///< [meters, ECEF] uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid @@ -1395,6 +1635,11 @@ struct EcefVel static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(velocity_ecef,valid_flags); + } + float velocity_ecef[3] = {0}; ///< [meters/second, ECEF] uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid @@ -1417,6 +1662,11 @@ struct EcefPosUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(pos_uncertainty,valid_flags); + } + float pos_uncertainty[3] = {0}; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1439,6 +1689,11 @@ struct EcefVelUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(vel_uncertainty,valid_flags); + } + float vel_uncertainty[3] = {0}; ///< [meters/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1461,6 +1716,11 @@ struct AidingMeasurementSummary static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,source,type,indicator); + } + float time_of_week = 0; ///< [seconds] uint8_t source = 0; FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) @@ -1485,6 +1745,11 @@ struct OdometerScaleFactorError static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scale_factor_error,valid_flags); + } + float scale_factor_error = 0; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1507,6 +1772,11 @@ struct OdometerScaleFactorErrorUncertainty static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scale_factor_error_uncertainty,valid_flags); + } + float scale_factor_error_uncertainty = 0; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid @@ -1529,6 +1799,11 @@ struct GnssDualAntennaStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,heading,heading_unc,fix_type,status_flags,valid_flags); + } + enum class FixType : uint8_t { FIX_NONE = 0, ///< diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 5a279e844..33cca17a3 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -180,6 +180,11 @@ struct PosLlh static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(latitude,longitude,ellipsoid_height,msl_height,horizontal_accuracy,vertical_accuracy,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -230,6 +235,11 @@ struct PosEcef static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(x,x_accuracy,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -273,6 +283,11 @@ struct VelNed static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(v,speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -324,6 +339,11 @@ struct VelEcef static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(v,v_accuracy,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -367,6 +387,11 @@ struct Dop static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(gdop,pdop,hdop,vdop,tdop,ndop,edop,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -420,6 +445,11 @@ struct UtcTime static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(year,month,day,hour,min,sec,msec,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -468,6 +498,11 @@ struct GpsTime static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(tow,week_number,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -511,6 +546,11 @@ struct ClockInfo static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias,drift,accuracy_estimate,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -556,6 +596,11 @@ struct FixInfo static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(fix_type,num_sv,fix_flags,valid_flags); + } + enum class FixType : uint8_t { FIX_3D = 0, ///< @@ -633,6 +678,11 @@ struct SvInfo static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(channel,sv_id,carrier_noise_ratio,azimuth,elevation,sv_flags,valid_flags); + } + struct SVFlags : Bitfield { enum _enumType : uint16_t @@ -703,6 +753,11 @@ struct HwStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(receiver_state,antenna_state,antenna_power,valid_flags); + } + enum class ReceiverState : uint8_t { OFF = 0, ///< @@ -783,6 +838,11 @@ struct DgpsInfo static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -832,6 +892,11 @@ struct DgpsChannel static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -881,6 +946,11 @@ struct ClockInfo2 static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(bias,drift,bias_accuracy_estimate,drift_accuracy_estimate,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -928,6 +998,11 @@ struct GpsLeapSeconds static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(leap_seconds,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -968,6 +1043,11 @@ struct SbasInfo static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,sbas_system,sbas_id,count,sbas_status,valid_flags); + } + struct SbasStatus : Bitfield { enum _enumType : uint8_t @@ -1062,6 +1142,11 @@ struct SbasCorrection static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,gnss_id,sv_id,udrei,pseudorange_correction,iono_correction,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1113,6 +1198,11 @@ struct RfErrorDetection static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(rf_band,jamming_state,spoofing_state,reserved,valid_flags); + } + enum class RFBand : uint8_t { UNKNOWN = 0, ///< @@ -1185,6 +1275,11 @@ struct BaseStationInfo static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,ecef_pos,height,station_id,indicators,valid_flags); + } + struct IndicatorFlags : Bitfield { enum _enumType : uint16_t @@ -1261,6 +1356,11 @@ struct RtkCorrectionsStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1343,6 +1443,11 @@ struct SatelliteStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,gnss_id,satellite_id,elevation,azimuth,health,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1398,6 +1503,11 @@ struct Raw static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,receiver_id,tracking_channel,gnss_id,satellite_id,signal_id,signal_strength,quality,pseudorange,carrier_phase,doppler,range_uncert,phase_uncert,doppler_uncert,lock_time,valid_flags); + } + enum class GnssSignalQuality : uint8_t { NONE = 0, ///< @@ -1481,6 +1591,11 @@ struct GpsEphemeris static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1555,6 +1670,11 @@ struct GalileoEphemeris static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1629,6 +1749,11 @@ struct GloEphemeris static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x,v,a,health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1693,6 +1818,11 @@ struct GpsIonoCorr static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,alpha,beta,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1740,6 +1870,11 @@ struct GalileoIonoCorr static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,alpha,disturbance_flags,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 9794957de..9ef2aed3a 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -83,6 +83,11 @@ struct RawAccel static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(raw_accel); + } + float raw_accel[3] = {0}; ///< Native sensor counts }; @@ -105,6 +110,11 @@ struct RawGyro static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(raw_gyro); + } + float raw_gyro[3] = {0}; ///< Native sensor counts }; @@ -127,6 +137,11 @@ struct RawMag static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(raw_mag); + } + float raw_mag[3] = {0}; ///< Native sensor counts }; @@ -149,6 +164,11 @@ struct RawPressure static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(raw_pressure); + } + float raw_pressure = 0; ///< Native sensor counts }; @@ -171,6 +191,11 @@ struct ScaledAccel static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scaled_accel); + } + float scaled_accel[3] = {0}; ///< (x, y, z)[g] }; @@ -193,6 +218,11 @@ struct ScaledGyro static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scaled_gyro); + } + float scaled_gyro[3] = {0}; ///< (x, y, z) [radians/second] }; @@ -215,6 +245,11 @@ struct ScaledMag static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scaled_mag); + } + float scaled_mag[3] = {0}; ///< (x, y, z) [Gauss] }; @@ -236,6 +271,11 @@ struct ScaledPressure static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(scaled_pressure); + } + float scaled_pressure = 0; ///< [mBar] }; @@ -258,6 +298,11 @@ struct DeltaTheta static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(delta_theta); + } + float delta_theta[3] = {0}; ///< (x, y, z) [radians] }; @@ -280,6 +325,11 @@ struct DeltaVelocity static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(delta_velocity); + } + float delta_velocity[3] = {0}; ///< (x, y, z) [g*sec] }; @@ -311,6 +361,11 @@ struct CompOrientationMatrix static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(m); + } + float m[9] = {0}; ///< Matrix elements in row-major order. }; @@ -340,6 +395,11 @@ struct CompQuaternion static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(q); + } + float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND }; @@ -362,6 +422,11 @@ struct CompEulerAngles static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(roll,pitch,yaw); + } + float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] @@ -385,6 +450,11 @@ struct CompOrientationUpdateMatrix static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(m); + } + float m[9] = {0}; }; @@ -406,6 +476,11 @@ struct OrientationRawTemp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(raw_temp); + } + uint16_t raw_temp[4] = {0}; }; @@ -427,6 +502,11 @@ struct InternalTimestamp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(counts); + } + uint32_t counts = 0; }; @@ -448,6 +528,11 @@ struct PpsTimestamp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(seconds,useconds); + } + uint32_t seconds = 0; uint32_t useconds = 0; @@ -476,6 +561,11 @@ struct GpsTimestamp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(tow,week_number,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -525,6 +615,11 @@ struct TemperatureAbs static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(min_temp,max_temp,mean_temp); + } + float min_temp = 0; ///< [degC] float max_temp = 0; ///< [degC] float mean_temp = 0; ///< [degC] @@ -554,6 +649,11 @@ struct UpVector static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(up); + } + float up[3] = {0}; ///< [Gs] }; @@ -578,6 +678,11 @@ struct NorthVector static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(north); + } + float north[3] = {0}; ///< [Gauss] }; @@ -598,6 +703,11 @@ struct OverrangeStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(status); + } + struct Status : Bitfield { enum _enumType : uint16_t @@ -645,6 +755,11 @@ struct OdometerData static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(speed,uncertainty,valid_flags); + } + float speed = 0; ///< Average speed over the time interval [m/s]. Can be negative for quadrature encoders. float uncertainty = 0; ///< Uncertainty of velocity [m/s]. uint16_t valid_flags = 0; ///< If odometer is configured, bit 0 will be set to 1. diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index d1a9bb377..c210f2790 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -70,6 +70,11 @@ struct EventSource static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(trigger_id); + } + uint8_t trigger_id = 0; ///< Trigger ID number. If 0, this message was emitted due to being scheduled in the 3DM Message Format Command (0x0C,0x0F). }; @@ -94,6 +99,11 @@ struct Ticks static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(ticks); + } + uint32_t ticks = 0; ///< Ticks since powerup. }; @@ -119,6 +129,11 @@ struct DeltaTicks static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(ticks); + } + uint32_t ticks = 0; ///< Ticks since last output. }; @@ -143,6 +158,11 @@ struct GpsTimestamp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(tow,week_number,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -195,6 +215,11 @@ struct DeltaTime static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(seconds); + } + double seconds = 0; ///< Seconds since last output. }; @@ -223,6 +248,11 @@ struct ReferenceTimestamp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(nanoseconds); + } + uint64_t nanoseconds = 0; ///< Nanoseconds since initialization. }; @@ -253,6 +283,11 @@ struct ReferenceTimeDelta static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(dt_nanos); + } + uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. }; @@ -282,6 +317,11 @@ struct ExternalTimestamp static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(nanoseconds,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -334,6 +374,11 @@ struct ExternalTimeDelta static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(dt_nanos,valid_flags); + } + struct ValidFlags : Bitfield { enum _enumType : uint16_t diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 405310418..84e42294b 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -78,6 +78,11 @@ struct BuiltInTest static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(result); + } + uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. }; @@ -99,6 +104,11 @@ struct TimeSyncStatus static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(time_sync,last_pps_rcvd); + } + bool time_sync = 0; ///< True if sync with the PPS signal is currently valid. False if PPS feature is disabled or a PPS signal is not detected. uint8_t last_pps_rcvd = 0; ///< Elapsed time in seconds since last PPS was received, with a maximum value of 255. @@ -139,6 +149,11 @@ struct GpioState static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(states); + } + uint8_t states = 0; ///< Bitfield containing the states for each GPIO pin.
Bit 0 (0x01): pin 1
Bit 1 (0x02): pin 2
Bit 2 (0x04): pin 3
Bit 3 (0x08): pin 4
Bits for pins that don't exist will read as 0. }; @@ -161,6 +176,11 @@ struct GpioAnalogValue static const bool HAS_FUNCTION_SELECTOR = false; + auto as_tuple() const + { + return std::make_tuple(gpio_id,value); + } + uint8_t gpio_id = 0; ///< GPIO pin number starting with 1. float value = 0; ///< Value of the GPIO line in scaled volts. From 9e6e4f6b021f8e12e2ab11abed879539546a9dc8 Mon Sep 17 00:00:00 2001 From: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Date: Mon, 10 Oct 2022 15:19:19 -0400 Subject: [PATCH 011/252] Added git attributes for line endings --- .gitattributes | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .gitattributes diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 000000000..1537c1938 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,13 @@ +# Set the default behavior, in case people don't have core.autocrlf set. +* text=auto + +# Declare files that will always have LF line endings on checkout. +*.c* text eol=LF +*.h* text eol=LF +*.md text eol=LF +CMakeLists.txt text eol=LF +LICENSE.txt text eol=LF + +# Denote all files that are truly binary and should not be modified. +*.bin binary +*.svg binary \ No newline at end of file From a3f06049ee23f9421c448b073b5083f3342b2a45 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 1 Nov 2022 15:39:48 -0400 Subject: [PATCH 012/252] Generate MIP definitions from branches/dev@53813. --- src/mip/definitions/commands_3dm.c | 108 ++ src/mip/definitions/commands_3dm.cpp | 97 ++ src/mip/definitions/commands_3dm.h | 32 + src/mip/definitions/commands_3dm.hpp | 42 + src/mip/definitions/commands_filter.c | 1725 +++++++++++++++++++++-- src/mip/definitions/commands_filter.cpp | 1625 +++++++++++++++++++-- src/mip/definitions/commands_filter.h | 470 ++++++ src/mip/definitions/commands_filter.hpp | 568 ++++++++ 8 files changed, 4483 insertions(+), 184 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index c48418d79..caef68235 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -3933,6 +3933,114 @@ mip_cmd_result mip_3dm_default_mag_soft_iron_matrix(struct mip_interface* device return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_3dm_coning_sculling_enable_command(mip_serializer* serializer, const mip_3dm_coning_sculling_enable_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_bool(serializer, self->enable); + + } +} +void extract_mip_3dm_coning_sculling_enable_command(mip_serializer* serializer, mip_3dm_coning_sculling_enable_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_bool(serializer, &self->enable); + + } +} + +void insert_mip_3dm_coning_sculling_enable_response(mip_serializer* serializer, const mip_3dm_coning_sculling_enable_response* self) +{ + insert_bool(serializer, self->enable); + +} +void extract_mip_3dm_coning_sculling_enable_response(mip_serializer* serializer, mip_3dm_coning_sculling_enable_response* self) +{ + extract_bool(serializer, &self->enable); + +} + +mip_cmd_result mip_3dm_write_coning_sculling_enable(struct mip_interface* device, bool enable) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_bool(&serializer, enable); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_read_coning_sculling_enable(struct mip_interface* device, bool* enable_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_bool(&deserializer, enable_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_3dm_save_coning_sculling_enable(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_load_coning_sculling_enable(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_default_coning_sculling_enable(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_3dm_sensor_2_vehicle_transform_euler_command(mip_serializer* serializer, const mip_3dm_sensor_2_vehicle_transform_euler_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index 423cb121f..fd5471d53 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -3472,6 +3472,103 @@ CmdResult defaultMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const ConingScullingEnable& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + } +} +void extract(Serializer& serializer, ConingScullingEnable& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + } +} + +void insert(Serializer& serializer, const ConingScullingEnable::Response& self) +{ + insert(serializer, self.enable); + +} +void extract(Serializer& serializer, ConingScullingEnable::Response& self) +{ + extract(serializer, self.enable); + +} + +CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveConingScullingEnable(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadConingScullingEnable(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultConingScullingEnable(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const Sensor2VehicleTransformEuler& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index b4da6c501..3d96ab5ed 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -1699,6 +1699,38 @@ mip_cmd_result mip_3dm_default_mag_soft_iron_matrix(struct mip_interface* device ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_3dm_coning_sculling_enable (0x0C,0x3E) Coning Sculling Enable [C] +/// Controls the Coning and Sculling Compenstation setting. +/// +///@{ + +struct mip_3dm_coning_sculling_enable_command +{ + mip_function_selector function; + bool enable; ///< If true, coning and sculling compensation is enabled. + +}; +typedef struct mip_3dm_coning_sculling_enable_command mip_3dm_coning_sculling_enable_command; +void insert_mip_3dm_coning_sculling_enable_command(struct mip_serializer* serializer, const mip_3dm_coning_sculling_enable_command* self); +void extract_mip_3dm_coning_sculling_enable_command(struct mip_serializer* serializer, mip_3dm_coning_sculling_enable_command* self); + +struct mip_3dm_coning_sculling_enable_response +{ + bool enable; ///< If true, coning and sculling compensation is enabled. + +}; +typedef struct mip_3dm_coning_sculling_enable_response mip_3dm_coning_sculling_enable_response; +void insert_mip_3dm_coning_sculling_enable_response(struct mip_serializer* serializer, const mip_3dm_coning_sculling_enable_response* self); +void extract_mip_3dm_coning_sculling_enable_response(struct mip_serializer* serializer, mip_3dm_coning_sculling_enable_response* self); + +mip_cmd_result mip_3dm_write_coning_sculling_enable(struct mip_interface* device, bool enable); +mip_cmd_result mip_3dm_read_coning_sculling_enable(struct mip_interface* device, bool* enable_out); +mip_cmd_result mip_3dm_save_coning_sculling_enable(struct mip_interface* device); +mip_cmd_result mip_3dm_load_coning_sculling_enable(struct mip_interface* device); +mip_cmd_result mip_3dm_default_coning_sculling_enable(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_3dm_sensor_2_vehicle_transform_euler (0x0C,0x31) Sensor 2 Vehicle Transform Euler [C] /// Sets the sensor-to-vehicle frame transformation using Yaw, Pitch, Roll Euler angles. /// These are the Yaw, Pitch, and Roll mounting angles of the sensor with respect to vehicle frame of reference, diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 9b442f206..ffe5f045f 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -2021,6 +2021,48 @@ CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_3dm_coning_sculling_enable (0x0C,0x3E) Coning Sculling Enable [CPP] +/// Controls the Coning and Sculling Compenstation setting. +/// +///@{ + +struct ConingScullingEnable +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + bool enable = 0; ///< If true, coning and sculling compensation is enabled. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; + + bool enable = 0; ///< If true, coning and sculling compensation is enabled. + + }; +}; +void insert(Serializer& serializer, const ConingScullingEnable& self); +void extract(Serializer& serializer, ConingScullingEnable& self); + +void insert(Serializer& serializer, const ConingScullingEnable::Response& self); +void extract(Serializer& serializer, ConingScullingEnable::Response& self); + +CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable); +CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); +CmdResult saveConingScullingEnable(C::mip_interface& device); +CmdResult loadConingScullingEnable(C::mip_interface& device); +CmdResult defaultConingScullingEnable(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_3dm_sensor_2_vehicle_transform_euler (0x0C,0x31) Sensor 2 Vehicle Transform Euler [CPP] /// Sets the sensor-to-vehicle frame transformation using Yaw, Pitch, Roll Euler angles. /// These are the Yaw, Pitch, and Roll mounting angles of the sensor with respect to vehicle frame of reference, diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index 4edc45da0..ccbca3b69 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -44,6 +44,17 @@ void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer *self = tmp; } +void insert_mip_filter_adaptive_measurement(struct mip_serializer* serializer, const mip_filter_adaptive_measurement self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_adaptive_measurement(struct mip_serializer* serializer, mip_filter_adaptive_measurement* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -485,6 +496,125 @@ mip_cmd_result mip_filter_default_tare_orientation(struct mip_interface* device) return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_filter_vehicle_dynamics_mode_command(mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, self->mode); + + } +} +void extract_mip_filter_vehicle_dynamics_mode_command(mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, &self->mode); + + } +} + +void insert_mip_filter_vehicle_dynamics_mode_response(mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_response* self) +{ + insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, self->mode); + +} +void extract_mip_filter_vehicle_dynamics_mode_response(mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_response* self) +{ + extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, &self->mode); + +} + +void insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command_dynamics_mode self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_filter_write_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(&serializer, mode); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* mode_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(mode_out); + extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(&deserializer, mode_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_vehicle_dynamics_mode(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_vehicle_dynamics_mode(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_vehicle_dynamics_mode(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_filter_sensor_to_vehicle_rotation_euler_command(mip_serializer* serializer, const mip_filter_sensor_to_vehicle_rotation_euler_command* self) { insert_mip_function_selector(serializer, self->function); @@ -1425,39 +1555,1206 @@ mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_altitude_aiding_command(mip_serializer* serializer, const mip_filter_altitude_aiding_command* self) +void insert_mip_filter_accel_noise_command(mip_serializer* serializer, const mip_filter_accel_noise_command* self) { insert_mip_function_selector(serializer, self->function); if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->aiding_selector); + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); } } -void extract_mip_filter_altitude_aiding_command(mip_serializer* serializer, mip_filter_altitude_aiding_command* self) +void extract_mip_filter_accel_noise_command(mip_serializer* serializer, mip_filter_accel_noise_command* self) { extract_mip_function_selector(serializer, &self->function); if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->aiding_selector); + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); } } -void insert_mip_filter_altitude_aiding_response(mip_serializer* serializer, const mip_filter_altitude_aiding_response* self) +void insert_mip_filter_accel_noise_response(mip_serializer* serializer, const mip_filter_accel_noise_response* self) +{ + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_accel_noise_response(mip_serializer* serializer, mip_filter_accel_noise_response* self) +{ + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ACCEL_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_gyro_noise_command(mip_serializer* serializer, const mip_filter_gyro_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_gyro_noise_command(mip_serializer* serializer, mip_filter_gyro_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_gyro_noise_response(mip_serializer* serializer, const mip_filter_gyro_noise_response* self) +{ + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_gyro_noise_response(mip_serializer* serializer, mip_filter_gyro_noise_response* self) +{ + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_GYRO_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_accel_bias_model_command(mip_serializer* serializer, const mip_filter_accel_bias_model_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_accel_bias_model_command(mip_serializer* serializer, mip_filter_accel_bias_model_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_accel_bias_model_response(mip_serializer* serializer, const mip_filter_accel_bias_model_response* self) +{ + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_accel_bias_model_response(mip_serializer* serializer, mip_filter_accel_bias_model_response* self) +{ + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x_beta); + + insert_float(&serializer, y_beta); + + insert_float(&serializer, z_beta); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_beta_out); + extract_float(&deserializer, x_beta_out); + + assert(y_beta_out); + extract_float(&deserializer, y_beta_out); + + assert(z_beta_out); + extract_float(&deserializer, z_beta_out); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_gyro_bias_model_command(mip_serializer* serializer, const mip_filter_gyro_bias_model_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_gyro_bias_model_command(mip_serializer* serializer, mip_filter_gyro_bias_model_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_gyro_bias_model_response(mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self) +{ + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_gyro_bias_model_response(mip_serializer* serializer, mip_filter_gyro_bias_model_response* self) +{ + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x_beta); + + insert_float(&serializer, y_beta); + + insert_float(&serializer, z_beta); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_GYRO_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_beta_out); + extract_float(&deserializer, x_beta_out); + + assert(y_beta_out); + extract_float(&deserializer, y_beta_out); + + assert(z_beta_out); + extract_float(&deserializer, z_beta_out); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_altitude_aiding_command(mip_serializer* serializer, const mip_filter_altitude_aiding_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->aiding_selector); + + } +} +void extract_mip_filter_altitude_aiding_command(mip_serializer* serializer, mip_filter_altitude_aiding_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->aiding_selector); + + } +} + +void insert_mip_filter_altitude_aiding_response(mip_serializer* serializer, const mip_filter_altitude_aiding_response* self) +{ + insert_u8(serializer, self->aiding_selector); + +} +void extract_mip_filter_altitude_aiding_response(mip_serializer* serializer, mip_filter_altitude_aiding_response* self) +{ + extract_u8(serializer, &self->aiding_selector); + +} + +mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, aiding_selector); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(aiding_selector_out); + extract_u8(&deserializer, aiding_selector_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_auto_zupt_command(mip_serializer* serializer, const mip_filter_auto_zupt_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + + } +} +void extract_mip_filter_auto_zupt_command(mip_serializer* serializer, mip_filter_auto_zupt_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + + } +} + +void insert_mip_filter_auto_zupt_response(mip_serializer* serializer, const mip_filter_auto_zupt_response* self) +{ + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + +} +void extract_mip_filter_auto_zupt_response(mip_serializer* serializer, mip_filter_auto_zupt_response* self) +{ + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + +} + +mip_cmd_result mip_filter_write_auto_zupt(struct mip_interface* device, uint8_t enable, float threshold) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, enable); + + insert_float(&serializer, threshold); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_auto_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_u8(&deserializer, enable_out); + + assert(threshold_out); + extract_float(&deserializer, threshold_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_auto_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_auto_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_auto_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, const mip_filter_auto_angular_zupt_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + + } +} +void extract_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, mip_filter_auto_angular_zupt_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + + } +} + +void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +{ + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + +} +void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +{ + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + +} + +mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, enable); + + insert_float(&serializer, threshold); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_u8(&deserializer, enable_out); + + assert(threshold_out); + extract_float(&deserializer, threshold_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); +} +mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert_mip_filter_reference_position_command(mip_serializer* serializer, const mip_filter_reference_position_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_bool(serializer, self->enable); + + insert_double(serializer, self->latitude); + + insert_double(serializer, self->longitude); + + insert_double(serializer, self->altitude); + + } +} +void extract_mip_filter_reference_position_command(mip_serializer* serializer, mip_filter_reference_position_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_bool(serializer, &self->enable); + + extract_double(serializer, &self->latitude); + + extract_double(serializer, &self->longitude); + + extract_double(serializer, &self->altitude); + + } +} + +void insert_mip_filter_reference_position_response(mip_serializer* serializer, const mip_filter_reference_position_response* self) +{ + insert_bool(serializer, self->enable); + + insert_double(serializer, self->latitude); + + insert_double(serializer, self->longitude); + + insert_double(serializer, self->altitude); + +} +void extract_mip_filter_reference_position_response(mip_serializer* serializer, mip_filter_reference_position_response* self) +{ + extract_bool(serializer, &self->enable); + + extract_double(serializer, &self->latitude); + + extract_double(serializer, &self->longitude); + + extract_double(serializer, &self->altitude); + +} + +mip_cmd_result mip_filter_write_reference_position(struct mip_interface* device, bool enable, double latitude, double longitude, double altitude) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_bool(&serializer, enable); + + insert_double(&serializer, latitude); + + insert_double(&serializer, longitude); + + insert_double(&serializer, altitude); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_reference_position(struct mip_interface* device, bool* enable_out, double* latitude_out, double* longitude_out, double* altitude_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_REFERENCE_POSITION, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_bool(&deserializer, enable_out); + + assert(latitude_out); + extract_double(&deserializer, latitude_out); + + assert(longitude_out); + extract_double(&deserializer, longitude_out); + + assert(altitude_out); + extract_double(&deserializer, altitude_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_reference_position(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_reference_position(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_reference_position(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); + + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); + + } +} +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); + + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); + + } +} + +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_response* self) { - insert_u8(serializer, self->aiding_selector); + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); + + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } -void extract_mip_filter_altitude_aiding_response(mip_serializer* serializer, mip_filter_altitude_aiding_response* self) +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_response* self) { - extract_u8(serializer, &self->aiding_selector); + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); + + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } -mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector) +mip_cmd_result mip_filter_write_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1465,13 +2762,25 @@ mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, ui insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, aiding_selector); + insert_mip_filter_adaptive_measurement(&serializer, adaptive_measurement); + + insert_float(&serializer, frequency); + + insert_float(&serializer, low_limit); + + insert_float(&serializer, high_limit); + + insert_float(&serializer, low_limit_uncertainty); + + insert_float(&serializer, high_limit_uncertainty); + + insert_float(&serializer, minimum_uncertainty); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out) +mip_cmd_result mip_filter_read_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1482,22 +2791,40 @@ mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uin assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(aiding_selector_out); - extract_u8(&deserializer, aiding_selector_out); + assert(adaptive_measurement_out); + extract_mip_filter_adaptive_measurement(&deserializer, adaptive_measurement_out); + + assert(frequency_out); + extract_float(&deserializer, frequency_out); + + assert(low_limit_out); + extract_float(&deserializer, low_limit_out); + + assert(high_limit_out); + extract_float(&deserializer, high_limit_out); + + assert(low_limit_uncertainty_out); + extract_float(&deserializer, low_limit_uncertainty_out); + + assert(high_limit_uncertainty_out); + extract_float(&deserializer, high_limit_uncertainty_out); + + assert(minimum_uncertainty_out); + extract_float(&deserializer, minimum_uncertainty_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device) +mip_cmd_result mip_filter_save_accel_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1507,9 +2834,9 @@ mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device) +mip_cmd_result mip_filter_load_accel_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1519,9 +2846,9 @@ mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) +mip_cmd_result mip_filter_default_accel_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1531,49 +2858,89 @@ mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_auto_zupt_command(mip_serializer* serializer, const mip_filter_auto_zupt_command* self) +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_command* self) { insert_mip_function_selector(serializer, self->function); if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->enable); + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } } -void extract_mip_filter_auto_zupt_command(mip_serializer* serializer, mip_filter_auto_zupt_command* self) +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_command* self) { extract_mip_function_selector(serializer, &self->function); if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->enable); + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } } -void insert_mip_filter_auto_zupt_response(mip_serializer* serializer, const mip_filter_auto_zupt_response* self) +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_response* self) { - insert_u8(serializer, self->enable); + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } -void extract_mip_filter_auto_zupt_response(mip_serializer* serializer, mip_filter_auto_zupt_response* self) +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_response* self) { - extract_u8(serializer, &self->enable); + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } -mip_cmd_result mip_filter_write_auto_zupt(struct mip_interface* device, uint8_t enable, float threshold) +mip_cmd_result mip_filter_write_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1581,15 +2948,25 @@ mip_cmd_result mip_filter_write_auto_zupt(struct mip_interface* device, uint8_t insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, enable); + insert_mip_filter_adaptive_measurement(&serializer, adaptive_measurement); - insert_float(&serializer, threshold); + insert_float(&serializer, frequency); + + insert_float(&serializer, low_limit); + + insert_float(&serializer, high_limit); + + insert_float(&serializer, low_limit_uncertainty); + + insert_float(&serializer, high_limit_uncertainty); + + insert_float(&serializer, minimum_uncertainty); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_auto_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +mip_cmd_result mip_filter_read_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1600,25 +2977,40 @@ mip_cmd_result mip_filter_read_auto_zupt(struct mip_interface* device, uint8_t* assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ZUPT_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(enable_out); - extract_u8(&deserializer, enable_out); + assert(adaptive_measurement_out); + extract_mip_filter_adaptive_measurement(&deserializer, adaptive_measurement_out); - assert(threshold_out); - extract_float(&deserializer, threshold_out); + assert(frequency_out); + extract_float(&deserializer, frequency_out); + + assert(low_limit_out); + extract_float(&deserializer, low_limit_out); + + assert(high_limit_out); + extract_float(&deserializer, high_limit_out); + + assert(low_limit_uncertainty_out); + extract_float(&deserializer, low_limit_uncertainty_out); + + assert(high_limit_uncertainty_out); + extract_float(&deserializer, high_limit_uncertainty_out); + + assert(minimum_uncertainty_out); + extract_float(&deserializer, minimum_uncertainty_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_auto_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_save_mag_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1628,9 +3020,9 @@ mip_cmd_result mip_filter_save_auto_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_auto_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_load_mag_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1640,9 +3032,9 @@ mip_cmd_result mip_filter_load_auto_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_auto_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_default_mag_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1652,49 +3044,73 @@ mip_cmd_result mip_filter_default_auto_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, const mip_filter_auto_angular_zupt_command* self) +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_command(mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_command* self) { insert_mip_function_selector(serializer, self->function); if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->enable); + insert_bool(serializer, self->enable); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } } -void extract_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, mip_filter_auto_angular_zupt_command* self) +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_command(mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_command* self) { extract_mip_function_selector(serializer, &self->function); if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->enable); + extract_bool(serializer, &self->enable); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } } -void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_response(mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_response* self) { - insert_u8(serializer, self->enable); + insert_bool(serializer, self->enable); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } -void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_response(mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_response* self) { - extract_u8(serializer, &self->enable); + extract_bool(serializer, &self->enable); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } -mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +mip_cmd_result mip_filter_write_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool enable, float frequency, float high_limit, float high_limit_uncertainty, float minimum_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1702,15 +3118,21 @@ mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, enable); + insert_bool(&serializer, enable); - insert_float(&serializer, threshold); + insert_float(&serializer, frequency); + + insert_float(&serializer, high_limit); + + insert_float(&serializer, high_limit_uncertainty); + + insert_float(&serializer, minimum_uncertainty); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +mip_cmd_result mip_filter_read_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool* enable_out, float* frequency_out, float* high_limit_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1721,7 +3143,7 @@ mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, u assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1729,17 +3151,26 @@ mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, u mip_serializer_init_insertion(&deserializer, buffer, responseLength); assert(enable_out); - extract_u8(&deserializer, enable_out); + extract_bool(&deserializer, enable_out); - assert(threshold_out); - extract_float(&deserializer, threshold_out); + assert(frequency_out); + extract_float(&deserializer, frequency_out); + + assert(high_limit_out); + extract_float(&deserializer, high_limit_out); + + assert(high_limit_uncertainty_out); + extract_float(&deserializer, high_limit_uncertainty_out); + + assert(minimum_uncertainty_out); + extract_float(&deserializer, minimum_uncertainty_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_save_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1749,9 +3180,9 @@ mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_load_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1761,9 +3192,9 @@ mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_default_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1773,15 +3204,7 @@ mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); -} -mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert_mip_filter_aiding_measurement_enable_command(mip_serializer* serializer, const mip_filter_aiding_measurement_enable_command* self) { @@ -3357,6 +4780,140 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_GNSS_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self) +{ + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self) +{ + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 12e176284..7048f63d0 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -428,6 +428,103 @@ CmdResult defaultTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const VehicleDynamicsMode& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.mode); + + } +} +void extract(Serializer& serializer, VehicleDynamicsMode& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.mode); + + } +} + +void insert(Serializer& serializer, const VehicleDynamicsMode::Response& self) +{ + insert(serializer, self.mode); + +} +void extract(Serializer& serializer, VehicleDynamicsMode::Response& self) +{ + extract(serializer, self.mode); + +} + +CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, mode); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(modeOut); + extract(deserializer, *modeOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveVehicleDynamicsMode(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadVehicleDynamicsMode(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultVehicleDynamicsMode(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const SensorToVehicleRotationEuler& self) { insert(serializer, self.function); @@ -1258,51 +1355,1164 @@ CmdResult defaultAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const AltitudeAiding& self) +void insert(Serializer& serializer, const AccelNoise& self) { insert(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.aiding_selector); + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); } } -void extract(Serializer& serializer, AltitudeAiding& self) +void extract(Serializer& serializer, AccelNoise& self) { extract(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.aiding_selector); + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const AccelNoise::Response& self) +{ + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, AccelNoise::Response& self) +{ + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAccelNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAccelNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAccelNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const GyroNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, GyroNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const GyroNoise::Response& self) +{ + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, GyroNoise::Response& self) +{ + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGyroNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGyroNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGyroNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AccelBiasModel& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, AccelBiasModel& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const AccelBiasModel::Response& self) +{ + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, AccelBiasModel::Response& self) +{ + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, xBeta); + + insert(serializer, yBeta); + + insert(serializer, zBeta); + + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xBetaOut); + extract(deserializer, *xBetaOut); + + assert(yBetaOut); + extract(deserializer, *yBetaOut); + + assert(zBetaOut); + extract(deserializer, *zBetaOut); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAccelBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAccelBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAccelBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const GyroBiasModel& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, GyroBiasModel& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const GyroBiasModel::Response& self) +{ + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, GyroBiasModel::Response& self) +{ + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, xBeta); + + insert(serializer, yBeta); + + insert(serializer, zBeta); + + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xBetaOut); + extract(deserializer, *xBetaOut); + + assert(yBetaOut); + extract(deserializer, *yBetaOut); + + assert(zBetaOut); + extract(deserializer, *zBetaOut); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGyroBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGyroBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGyroBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AltitudeAiding& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.aiding_selector); + + } +} +void extract(Serializer& serializer, AltitudeAiding& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.aiding_selector); + + } +} + +void insert(Serializer& serializer, const AltitudeAiding::Response& self) +{ + insert(serializer, self.aiding_selector); + +} +void extract(Serializer& serializer, AltitudeAiding::Response& self) +{ + extract(serializer, self.aiding_selector); + +} + +CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, aidingSelector); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(aidingSelectorOut); + extract(deserializer, *aidingSelectorOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAltitudeAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAltitudeAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAltitudeAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AutoZupt& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.threshold); + + } +} +void extract(Serializer& serializer, AutoZupt& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.threshold); + + } +} + +void insert(Serializer& serializer, const AutoZupt::Response& self) +{ + insert(serializer, self.enable); + + insert(serializer, self.threshold); + +} +void extract(Serializer& serializer, AutoZupt::Response& self) +{ + extract(serializer, self.enable); + + extract(serializer, self.threshold); + +} + +CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + insert(serializer, threshold); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(thresholdOut); + extract(deserializer, *thresholdOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAutoZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAutoZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAutoZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AutoAngularZupt& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.threshold); + + } +} +void extract(Serializer& serializer, AutoAngularZupt& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.threshold); + + } +} + +void insert(Serializer& serializer, const AutoAngularZupt::Response& self) +{ + insert(serializer, self.enable); + + insert(serializer, self.threshold); + +} +void extract(Serializer& serializer, AutoAngularZupt::Response& self) +{ + extract(serializer, self.enable); + + extract(serializer, self.threshold); + +} + +CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + insert(serializer, threshold); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(thresholdOut); + extract(deserializer, *thresholdOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const CommandedZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedAngularZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const ReferencePosition& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.latitude); + + insert(serializer, self.longitude); + + insert(serializer, self.altitude); + + } +} +void extract(Serializer& serializer, ReferencePosition& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.latitude); + + extract(serializer, self.longitude); + + extract(serializer, self.altitude); + + } +} + +void insert(Serializer& serializer, const ReferencePosition::Response& self) +{ + insert(serializer, self.enable); + + insert(serializer, self.latitude); + + insert(serializer, self.longitude); + + insert(serializer, self.altitude); + +} +void extract(Serializer& serializer, ReferencePosition::Response& self) +{ + extract(serializer, self.enable); + + extract(serializer, self.latitude); + + extract(serializer, self.longitude); + + extract(serializer, self.altitude); + +} + +CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + insert(serializer, latitude); + + insert(serializer, longitude); + + insert(serializer, altitude); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(latitudeOut); + extract(deserializer, *latitudeOut); + + assert(longitudeOut); + extract(deserializer, *longitudeOut); + + assert(altitudeOut); + extract(deserializer, *altitudeOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveReferencePosition(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadReferencePosition(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultReferencePosition(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.adaptive_measurement); + + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); + + } +} +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.adaptive_measurement); + + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } } -void insert(Serializer& serializer, const AltitudeAiding::Response& self) +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement::Response& self) { - insert(serializer, self.aiding_selector); + insert(serializer, self.adaptive_measurement); + + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } -void extract(Serializer& serializer, AltitudeAiding::Response& self) +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Response& self) { - extract(serializer, self.aiding_selector); + extract(serializer, self.adaptive_measurement); + + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } -CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector) +CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, aidingSelector); + insert(serializer, adaptiveMeasurement); + + insert(serializer, frequency); + + insert(serializer, lowLimit); + + insert(serializer, highLimit); + + insert(serializer, lowLimitUncertainty); + + insert(serializer, highLimitUncertainty); + + insert(serializer, minimumUncertainty); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut) +CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1311,21 +2521,39 @@ CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(aidingSelectorOut); - extract(deserializer, *aidingSelectorOut); + assert(adaptiveMeasurementOut); + extract(deserializer, *adaptiveMeasurementOut); + + assert(frequencyOut); + extract(deserializer, *frequencyOut); + + assert(lowLimitOut); + extract(deserializer, *lowLimitOut); + + assert(highLimitOut); + extract(deserializer, *highLimitOut); + + assert(lowLimitUncertaintyOut); + extract(deserializer, *lowLimitUncertaintyOut); + + assert(highLimitUncertaintyOut); + extract(deserializer, *highLimitUncertaintyOut); + + assert(minimumUncertaintyOut); + extract(deserializer, *minimumUncertaintyOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAltitudeAiding(C::mip_interface& device) +CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1333,9 +2561,9 @@ CmdResult saveAltitudeAiding(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAltitudeAiding(C::mip_interface& device) +CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1343,9 +2571,9 @@ CmdResult loadAltitudeAiding(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAltitudeAiding(C::mip_interface& device) +CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1353,63 +2581,113 @@ CmdResult defaultAltitudeAiding(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const AutoZupt& self) +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement& self) { insert(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.enable); + insert(serializer, self.adaptive_measurement); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } } -void extract(Serializer& serializer, AutoZupt& self) +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement& self) { extract(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.enable); + extract(serializer, self.adaptive_measurement); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } } -void insert(Serializer& serializer, const AutoZupt::Response& self) +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement::Response& self) { - insert(serializer, self.enable); + insert(serializer, self.adaptive_measurement); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } -void extract(Serializer& serializer, AutoZupt::Response& self) +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Response& self) { - extract(serializer, self.enable); + extract(serializer, self.adaptive_measurement); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } -CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) +CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, enable); + insert(serializer, adaptiveMeasurement); - insert(serializer, threshold); + insert(serializer, frequency); + + insert(serializer, lowLimit); + + insert(serializer, highLimit); + + insert(serializer, lowLimitUncertainty); + + insert(serializer, highLimitUncertainty); + + insert(serializer, minimumUncertainty); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1418,24 +2696,39 @@ CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thre assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(enableOut); - extract(deserializer, *enableOut); + assert(adaptiveMeasurementOut); + extract(deserializer, *adaptiveMeasurementOut); - assert(thresholdOut); - extract(deserializer, *thresholdOut); + assert(frequencyOut); + extract(deserializer, *frequencyOut); + + assert(lowLimitOut); + extract(deserializer, *lowLimitOut); + + assert(highLimitOut); + extract(deserializer, *highLimitOut); + + assert(lowLimitUncertaintyOut); + extract(deserializer, *lowLimitUncertaintyOut); + + assert(highLimitUncertaintyOut); + extract(deserializer, *highLimitUncertaintyOut); + + assert(minimumUncertaintyOut); + extract(deserializer, *minimumUncertaintyOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAutoZupt(C::mip_interface& device) +CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1443,9 +2736,9 @@ CmdResult saveAutoZupt(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoZupt(C::mip_interface& device) +CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1453,9 +2746,9 @@ CmdResult loadAutoZupt(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoZupt(C::mip_interface& device) +CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1463,9 +2756,9 @@ CmdResult defaultAutoZupt(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const AutoAngularZupt& self) +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement& self) { insert(serializer, self.function); @@ -1473,11 +2766,17 @@ void insert(Serializer& serializer, const AutoAngularZupt& self) { insert(serializer, self.enable); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.high_limit); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } } -void extract(Serializer& serializer, AutoAngularZupt& self) +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement& self) { extract(serializer, self.function); @@ -1485,27 +2784,45 @@ void extract(Serializer& serializer, AutoAngularZupt& self) { extract(serializer, self.enable); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.high_limit); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } } -void insert(Serializer& serializer, const AutoAngularZupt::Response& self) +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement::Response& self) { insert(serializer, self.enable); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.high_limit); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } -void extract(Serializer& serializer, AutoAngularZupt::Response& self) +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Response& self) { extract(serializer, self.enable); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.high_limit); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } -CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) +CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1513,13 +2830,19 @@ CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float t insert(serializer, FunctionSelector::WRITE); insert(serializer, enable); - insert(serializer, threshold); + insert(serializer, frequency); + + insert(serializer, highLimit); + + insert(serializer, highLimitUncertainty); + + insert(serializer, minimumUncertainty); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1528,7 +2851,7 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1537,15 +2860,24 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(enableOut); extract(deserializer, *enableOut); - assert(thresholdOut); - extract(deserializer, *thresholdOut); + assert(frequencyOut); + extract(deserializer, *frequencyOut); + + assert(highLimitOut); + extract(deserializer, *highLimitOut); + + assert(highLimitUncertaintyOut); + extract(deserializer, *highLimitUncertaintyOut); + + assert(minimumUncertaintyOut); + extract(deserializer, *minimumUncertaintyOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAutoAngularZupt(C::mip_interface& device) +CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1553,9 +2885,9 @@ CmdResult saveAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoAngularZupt(C::mip_interface& device) +CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1563,9 +2895,9 @@ CmdResult loadAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoAngularZupt(C::mip_interface& device) +CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1573,37 +2905,7 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert(Serializer& serializer, const CommandedZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); -} -void insert(Serializer& serializer, const CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedAngularZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert(Serializer& serializer, const AidingMeasurementEnable& self) { @@ -3024,6 +4326,129 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const HardIronOffsetNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, HardIronOffsetNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self) +{ + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) +{ + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const MagneticDeclinationSource& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 9169cc66f..3b52d260f 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -170,6 +170,14 @@ static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE void insert_mip_filter_mag_declination_source(struct mip_serializer* serializer, const mip_filter_mag_declination_source self); void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer, mip_filter_mag_declination_source* self); +typedef uint8_t mip_filter_adaptive_measurement; +static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_DISABLED = 0; ///< No adaptive measurement +static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_FIXED = 1; ///< Enable fixed adaptive measurement (use specified limits) +static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_AUTO = 2; ///< Enable auto adaptive measurement + +void insert_mip_filter_adaptive_measurement(struct mip_serializer* serializer, const mip_filter_adaptive_measurement self); +void extract_mip_filter_adaptive_measurement(struct mip_serializer* serializer, mip_filter_adaptive_measurement* self); + //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -418,6 +426,47 @@ mip_cmd_result mip_filter_default_tare_orientation(struct mip_interface* device) ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_vehicle_dynamics_mode (0x0D,0x10) Vehicle Dynamics Mode [C] +/// Controls the vehicle dynamics mode. +/// +///@{ + +typedef uint8_t mip_filter_vehicle_dynamics_mode_command_dynamics_mode; +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_PORTABLE = 1; ///< +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_AUTOMOTIVE = 2; ///< +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_AIRBORNE = 3; ///< +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_AIRBORNE_HIGH_G = 4; ///< + +struct mip_filter_vehicle_dynamics_mode_command +{ + mip_function_selector function; + mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode; + +}; +typedef struct mip_filter_vehicle_dynamics_mode_command mip_filter_vehicle_dynamics_mode_command; +void insert_mip_filter_vehicle_dynamics_mode_command(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command* self); +void extract_mip_filter_vehicle_dynamics_mode_command(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command* self); + +void insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command_dynamics_mode self); +void extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* self); + +struct mip_filter_vehicle_dynamics_mode_response +{ + mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode; + +}; +typedef struct mip_filter_vehicle_dynamics_mode_response mip_filter_vehicle_dynamics_mode_response; +void insert_mip_filter_vehicle_dynamics_mode_response(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_response* self); +void extract_mip_filter_vehicle_dynamics_mode_response(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_response* self); + +mip_cmd_result mip_filter_write_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode); +mip_cmd_result mip_filter_read_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* mode_out); +mip_cmd_result mip_filter_save_vehicle_dynamics_mode(struct mip_interface* device); +mip_cmd_result mip_filter_load_vehicle_dynamics_mode(struct mip_interface* device); +mip_cmd_result mip_filter_default_vehicle_dynamics_mode(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_sensor_to_vehicle_rotation_euler (0x0D,0x11) Sensor To Vehicle Rotation Euler [C] /// Set the sensor to vehicle frame rotation using Yaw, Pitch, Roll Euler angles. /// @@ -813,6 +862,182 @@ mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_accel_noise (0x0D,0x1A) Accel Noise [C] +/// Accelerometer Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0. +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct mip_filter_accel_noise_command +{ + mip_function_selector function; + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_noise_command mip_filter_accel_noise_command; +void insert_mip_filter_accel_noise_command(struct mip_serializer* serializer, const mip_filter_accel_noise_command* self); +void extract_mip_filter_accel_noise_command(struct mip_serializer* serializer, mip_filter_accel_noise_command* self); + +struct mip_filter_accel_noise_response +{ + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_noise_response mip_filter_accel_noise_response; +void insert_mip_filter_accel_noise_response(struct mip_serializer* serializer, const mip_filter_accel_noise_response* self); +void extract_mip_filter_accel_noise_response(struct mip_serializer* serializer, mip_filter_accel_noise_response* self); + +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z); +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_gyro_noise (0x0D,0x1B) Gyro Noise [C] +/// Gyroscope Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct mip_filter_gyro_noise_command +{ + mip_function_selector function; + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_noise_command mip_filter_gyro_noise_command; +void insert_mip_filter_gyro_noise_command(struct mip_serializer* serializer, const mip_filter_gyro_noise_command* self); +void extract_mip_filter_gyro_noise_command(struct mip_serializer* serializer, mip_filter_gyro_noise_command* self); + +struct mip_filter_gyro_noise_response +{ + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_noise_response mip_filter_gyro_noise_response; +void insert_mip_filter_gyro_noise_response(struct mip_serializer* serializer, const mip_filter_gyro_noise_response* self); +void extract_mip_filter_gyro_noise_response(struct mip_serializer* serializer, mip_filter_gyro_noise_response* self); + +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z); +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [C] +/// Accelerometer Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct mip_filter_accel_bias_model_command +{ + mip_function_selector function; + float x_beta; ///< Accel Bias Beta [1/second] + float y_beta; ///< Accel Bias Beta [1/second] + float z_beta; ///< Accel Bias Beta [1/second] + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_bias_model_command mip_filter_accel_bias_model_command; +void insert_mip_filter_accel_bias_model_command(struct mip_serializer* serializer, const mip_filter_accel_bias_model_command* self); +void extract_mip_filter_accel_bias_model_command(struct mip_serializer* serializer, mip_filter_accel_bias_model_command* self); + +struct mip_filter_accel_bias_model_response +{ + float x_beta; ///< Accel Bias Beta [1/second] + float y_beta; ///< Accel Bias Beta [1/second] + float z_beta; ///< Accel Bias Beta [1/second] + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_bias_model_response mip_filter_accel_bias_model_response; +void insert_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, const mip_filter_accel_bias_model_response* self); +void extract_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, mip_filter_accel_bias_model_response* self); + +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [C] +/// Gyroscope Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct mip_filter_gyro_bias_model_command +{ + mip_function_selector function; + float x_beta; ///< Gyro Bias Beta [1/second] + float y_beta; ///< Gyro Bias Beta [1/second] + float z_beta; ///< Gyro Bias Beta [1/second] + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_bias_model_command mip_filter_gyro_bias_model_command; +void insert_mip_filter_gyro_bias_model_command(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_command* self); +void extract_mip_filter_gyro_bias_model_command(struct mip_serializer* serializer, mip_filter_gyro_bias_model_command* self); + +struct mip_filter_gyro_bias_model_response +{ + float x_beta; ///< Gyro Bias Beta [1/second] + float y_beta; ///< Gyro Bias Beta [1/second] + float z_beta; ///< Gyro Bias Beta [1/second] + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_bias_model_response mip_filter_gyro_bias_model_response; +void insert_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self); +void extract_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, mip_filter_gyro_bias_model_response* self); + +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [C] /// Altitude Aiding Control /// @@ -948,6 +1173,206 @@ mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_reference_position (0x0D,0x26) Reference Position [C] +/// Set the Lat/Long/Alt reference position for the sensor. +/// +/// This position is used by the sensor to calculate the WGS84 gravity and WMM2015 magnetic field parameters. +/// +/// +///@{ + +struct mip_filter_reference_position_command +{ + mip_function_selector function; + bool enable; ///< enable/disable + double latitude; ///< [degrees] + double longitude; ///< [degrees] + double altitude; ///< [meters] + +}; +typedef struct mip_filter_reference_position_command mip_filter_reference_position_command; +void insert_mip_filter_reference_position_command(struct mip_serializer* serializer, const mip_filter_reference_position_command* self); +void extract_mip_filter_reference_position_command(struct mip_serializer* serializer, mip_filter_reference_position_command* self); + +struct mip_filter_reference_position_response +{ + bool enable; ///< enable/disable + double latitude; ///< [degrees] + double longitude; ///< [degrees] + double altitude; ///< [meters] + +}; +typedef struct mip_filter_reference_position_response mip_filter_reference_position_response; +void insert_mip_filter_reference_position_response(struct mip_serializer* serializer, const mip_filter_reference_position_response* self); +void extract_mip_filter_reference_position_response(struct mip_serializer* serializer, mip_filter_reference_position_response* self); + +mip_cmd_result mip_filter_write_reference_position(struct mip_interface* device, bool enable, double latitude, double longitude, double altitude); +mip_cmd_result mip_filter_read_reference_position(struct mip_interface* device, bool* enable_out, double* latitude_out, double* longitude_out, double* altitude_out); +mip_cmd_result mip_filter_save_reference_position(struct mip_interface* device); +mip_cmd_result mip_filter_load_reference_position(struct mip_interface* device); +mip_cmd_result mip_filter_default_reference_position(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_accel_magnitude_error_adaptive_measurement (0x0D,0x44) Accel Magnitude Error Adaptive Measurement [C] +/// Enable or disable the gravity magnitude error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// Adaptive measurements can be enabled/disabled without the need for providing the additional parameters. +/// In this case, only the function selector and enable value are required; all other parameters will remain at their previous values. +/// When “auto-adaptive” is selected, the filter and limit parameters are ignored. +/// Instead, aiding measurements which rely on the gravity vector will be automatically reweighted by the Kalman filter according to the perceived measurement quality. +/// +/// +///@{ + +struct mip_filter_accel_magnitude_error_adaptive_measurement_command +{ + mip_function_selector function; + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_magnitude_error_adaptive_measurement_command mip_filter_accel_magnitude_error_adaptive_measurement_command; +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_command* self); +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_command* self); + +struct mip_filter_accel_magnitude_error_adaptive_measurement_response +{ + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_magnitude_error_adaptive_measurement_response mip_filter_accel_magnitude_error_adaptive_measurement_response; +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_response* self); +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_response* self); + +mip_cmd_result mip_filter_write_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty); +mip_cmd_result mip_filter_read_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out); +mip_cmd_result mip_filter_save_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_load_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_default_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_magnitude_error_adaptive_measurement (0x0D,0x45) Mag Magnitude Error Adaptive Measurement [C] +/// Enable or disable the magnetometer magnitude error adaptive measurement. +/// This feature will reject magnetometer readings that are out of range of the thresholds specified (fixed adaptive) or calculated internally (auto-adaptive). +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// +///@{ + +struct mip_filter_mag_magnitude_error_adaptive_measurement_command +{ + mip_function_selector function; + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_magnitude_error_adaptive_measurement_command mip_filter_mag_magnitude_error_adaptive_measurement_command; +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_command* self); +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_command* self); + +struct mip_filter_mag_magnitude_error_adaptive_measurement_response +{ + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_magnitude_error_adaptive_measurement_response mip_filter_mag_magnitude_error_adaptive_measurement_response; +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_response* self); +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_response* self); + +mip_cmd_result mip_filter_write_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty); +mip_cmd_result mip_filter_read_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out); +mip_cmd_result mip_filter_save_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_dip_angle_error_adaptive_measurement (0x0D,0x46) Mag Dip Angle Error Adaptive Measurement [C] +/// Enable or disable the magnetometer dip angle error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// The magnetometer dip angle adaptive measurement is ignored if the auto-adaptive magnetometer magnitude or auto-adaptive accel magnitude options are selected. +/// +/// +///@{ + +struct mip_filter_mag_dip_angle_error_adaptive_measurement_command +{ + mip_function_selector function; + bool enable; ///< Enable/Disable + float frequency; ///< Low-pass filter curoff frequency [hertz] + float high_limit; ///< [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_dip_angle_error_adaptive_measurement_command mip_filter_mag_dip_angle_error_adaptive_measurement_command; +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_command(struct mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_command* self); +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_command(struct mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_command* self); + +struct mip_filter_mag_dip_angle_error_adaptive_measurement_response +{ + bool enable; ///< Enable/Disable + float frequency; ///< Low-pass filter curoff frequency [hertz] + float high_limit; ///< [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_dip_angle_error_adaptive_measurement_response mip_filter_mag_dip_angle_error_adaptive_measurement_response; +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_response(struct mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_response* self); +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_response(struct mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_response* self); + +mip_cmd_result mip_filter_write_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool enable, float frequency, float high_limit, float high_limit_uncertainty, float minimum_uncertainty); +mip_cmd_result mip_filter_read_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool* enable_out, float* frequency_out, float* high_limit_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out); +mip_cmd_result mip_filter_save_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_aiding_measurement_enable (0x0D,0x50) Aiding Measurement Enable [C] /// Enables / disables the specified aiding measurement source. /// @@ -1442,6 +1867,51 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [C] +/// Set the expected hard iron offset noise 1-sigma values +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct mip_filter_hard_iron_offset_noise_command +{ + mip_function_selector function; + float x; ///< HI Offset Noise 1-sima [gauss] + float y; ///< HI Offset Noise 1-sima [gauss] + float z; ///< HI Offset Noise 1-sima [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_command mip_filter_hard_iron_offset_noise_command; +void insert_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self); +void extract_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self); + +struct mip_filter_hard_iron_offset_noise_response +{ + float x; ///< HI Offset Noise 1-sima [gauss] + float y; ///< HI Offset Noise 1-sima [gauss] + float z; ///< HI Offset Noise 1-sima [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_offset_noise_response; +void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); +void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z); +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [C] /// Source for magnetic declination angle, and user supplied value for manual selection. /// diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index c2409196f..956db55e0 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -167,6 +167,13 @@ enum class FilterMagDeclinationSource : uint8_t MANUAL = 3, ///< Magnetic field is assumed to have the declination angle specified by the user. }; +enum class FilterAdaptiveMeasurement : uint8_t +{ + DISABLED = 0, ///< No adaptive measurement + FIXED = 1, ///< Enable fixed adaptive measurement (use specified limits) + AUTO = 2, ///< Enable auto adaptive measurement +}; + //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -485,6 +492,56 @@ CmdResult defaultTareOrientation(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_vehicle_dynamics_mode (0x0D,0x10) Vehicle Dynamics Mode [CPP] +/// Controls the vehicle dynamics mode. +/// +///@{ + +struct VehicleDynamicsMode +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class DynamicsMode : uint8_t + { + PORTABLE = 1, ///< + AUTOMOTIVE = 2, ///< + AIRBORNE = 3, ///< + AIRBORNE_HIGH_G = 4, ///< + }; + + FunctionSelector function = static_cast(0); + DynamicsMode mode = static_cast(0); + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; + + DynamicsMode mode = static_cast(0); + + }; +}; +void insert(Serializer& serializer, const VehicleDynamicsMode& self); +void extract(Serializer& serializer, VehicleDynamicsMode& self); + +void insert(Serializer& serializer, const VehicleDynamicsMode::Response& self); +void extract(Serializer& serializer, VehicleDynamicsMode::Response& self); + +CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); +CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); +CmdResult saveVehicleDynamicsMode(C::mip_interface& device); +CmdResult loadVehicleDynamicsMode(C::mip_interface& device); +CmdResult defaultVehicleDynamicsMode(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_sensor_to_vehicle_rotation_euler (0x0D,0x11) Sensor To Vehicle Rotation Euler [CPP] /// Set the sensor to vehicle frame rotation using Yaw, Pitch, Roll Euler angles. /// @@ -958,6 +1015,222 @@ CmdResult defaultAutoInitControl(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_accel_noise (0x0D,0x1A) Accel Noise [CPP] +/// Accelerometer Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0. +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct AccelNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; + + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const AccelNoise& self); +void extract(Serializer& serializer, AccelNoise& self); + +void insert(Serializer& serializer, const AccelNoise::Response& self); +void extract(Serializer& serializer, AccelNoise::Response& self); + +CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z); +CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult saveAccelNoise(C::mip_interface& device); +CmdResult loadAccelNoise(C::mip_interface& device); +CmdResult defaultAccelNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_gyro_noise (0x0D,0x1B) Gyro Noise [CPP] +/// Gyroscope Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct GyroNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; + + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const GyroNoise& self); +void extract(Serializer& serializer, GyroNoise& self); + +void insert(Serializer& serializer, const GyroNoise::Response& self); +void extract(Serializer& serializer, GyroNoise::Response& self); + +CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z); +CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult saveGyroNoise(C::mip_interface& device); +CmdResult loadGyroNoise(C::mip_interface& device); +CmdResult defaultGyroNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [CPP] +/// Accelerometer Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct AccelBiasModel +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x_beta = 0; ///< Accel Bias Beta [1/second] + float y_beta = 0; ///< Accel Bias Beta [1/second] + float z_beta = 0; ///< Accel Bias Beta [1/second] + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; + + float x_beta = 0; ///< Accel Bias Beta [1/second] + float y_beta = 0; ///< Accel Bias Beta [1/second] + float z_beta = 0; ///< Accel Bias Beta [1/second] + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const AccelBiasModel& self); +void extract(Serializer& serializer, AccelBiasModel& self); + +void insert(Serializer& serializer, const AccelBiasModel::Response& self); +void extract(Serializer& serializer, AccelBiasModel::Response& self); + +CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); +CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult saveAccelBiasModel(C::mip_interface& device); +CmdResult loadAccelBiasModel(C::mip_interface& device); +CmdResult defaultAccelBiasModel(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [CPP] +/// Gyroscope Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct GyroBiasModel +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x_beta = 0; ///< Gyro Bias Beta [1/second] + float y_beta = 0; ///< Gyro Bias Beta [1/second] + float z_beta = 0; ///< Gyro Bias Beta [1/second] + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; + + float x_beta = 0; ///< Gyro Bias Beta [1/second] + float y_beta = 0; ///< Gyro Bias Beta [1/second] + float z_beta = 0; ///< Gyro Bias Beta [1/second] + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const GyroBiasModel& self); +void extract(Serializer& serializer, GyroBiasModel& self); + +void insert(Serializer& serializer, const GyroBiasModel::Response& self); +void extract(Serializer& serializer, GyroBiasModel::Response& self); + +CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); +CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult saveGyroBiasModel(C::mip_interface& device); +CmdResult loadGyroBiasModel(C::mip_interface& device); +CmdResult defaultGyroBiasModel(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [CPP] /// Altitude Aiding Control /// @@ -1147,6 +1420,246 @@ CmdResult commandedAngularZupt(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_reference_position (0x0D,0x26) Reference Position [CPP] +/// Set the Lat/Long/Alt reference position for the sensor. +/// +/// This position is used by the sensor to calculate the WGS84 gravity and WMM2015 magnetic field parameters. +/// +/// +///@{ + +struct ReferencePosition +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + bool enable = 0; ///< enable/disable + double latitude = 0; ///< [degrees] + double longitude = 0; ///< [degrees] + double altitude = 0; ///< [meters] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; + + bool enable = 0; ///< enable/disable + double latitude = 0; ///< [degrees] + double longitude = 0; ///< [degrees] + double altitude = 0; ///< [meters] + + }; +}; +void insert(Serializer& serializer, const ReferencePosition& self); +void extract(Serializer& serializer, ReferencePosition& self); + +void insert(Serializer& serializer, const ReferencePosition::Response& self); +void extract(Serializer& serializer, ReferencePosition::Response& self); + +CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); +CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); +CmdResult saveReferencePosition(C::mip_interface& device); +CmdResult loadReferencePosition(C::mip_interface& device); +CmdResult defaultReferencePosition(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_accel_magnitude_error_adaptive_measurement (0x0D,0x44) Accel Magnitude Error Adaptive Measurement [CPP] +/// Enable or disable the gravity magnitude error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// Adaptive measurements can be enabled/disabled without the need for providing the additional parameters. +/// In this case, only the function selector and enable value are required; all other parameters will remain at their previous values. +/// When “auto-adaptive” is selected, the filter and limit parameters are ignored. +/// Instead, aiding measurements which rely on the gravity vector will be automatically reweighted by the Kalman filter according to the perceived measurement quality. +/// +/// +///@{ + +struct AccelMagnitudeErrorAdaptiveMeasurement +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement& self); +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement& self); + +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement::Response& self); +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Response& self); + +CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_magnitude_error_adaptive_measurement (0x0D,0x45) Mag Magnitude Error Adaptive Measurement [CPP] +/// Enable or disable the magnetometer magnitude error adaptive measurement. +/// This feature will reject magnetometer readings that are out of range of the thresholds specified (fixed adaptive) or calculated internally (auto-adaptive). +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// +///@{ + +struct MagMagnitudeErrorAdaptiveMeasurement +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement& self); +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement& self); + +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement::Response& self); +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Response& self); + +CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_dip_angle_error_adaptive_measurement (0x0D,0x46) Mag Dip Angle Error Adaptive Measurement [CPP] +/// Enable or disable the magnetometer dip angle error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// The magnetometer dip angle adaptive measurement is ignored if the auto-adaptive magnetometer magnitude or auto-adaptive accel magnitude options are selected. +/// +/// +///@{ + +struct MagDipAngleErrorAdaptiveMeasurement +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + bool enable = 0; ///< Enable/Disable + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float high_limit = 0; ///< [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + bool enable = 0; ///< Enable/Disable + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float high_limit = 0; ///< [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement& self); +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement& self); + +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement::Response& self); +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Response& self); + +CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); +CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_aiding_measurement_enable (0x0D,0x50) Aiding Measurement Enable [CPP] /// Enables / disables the specified aiding measurement source. /// @@ -1775,6 +2288,61 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [CPP] +/// Set the expected hard iron offset noise 1-sigma values +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct HardIronOffsetNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x = 0; ///< HI Offset Noise 1-sima [gauss] + float y = 0; ///< HI Offset Noise 1-sima [gauss] + float z = 0; ///< HI Offset Noise 1-sima [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + + float x = 0; ///< HI Offset Noise 1-sima [gauss] + float y = 0; ///< HI Offset Noise 1-sima [gauss] + float z = 0; ///< HI Offset Noise 1-sima [gauss] + + }; +}; +void insert(Serializer& serializer, const HardIronOffsetNoise& self); +void extract(Serializer& serializer, HardIronOffsetNoise& self); + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z); +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult saveHardIronOffsetNoise(C::mip_interface& device); +CmdResult loadHardIronOffsetNoise(C::mip_interface& device); +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [CPP] /// Source for magnetic declination angle, and user supplied value for manual selection. /// From d36b454ff96e427bc65af6b76968e5044605a983 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 1 Nov 2022 16:56:20 -0400 Subject: [PATCH 013/252] Fix bug in mip_interface_wait_for_reply when update function fails. (#35) --- examples/threading.cpp | 2 ++ src/mip/mip_device.hpp | 2 +- src/mip/mip_interface.c | 17 ++++++++++++++++- src/mip/mip_interface.h | 2 +- 4 files changed, 20 insertions(+), 3 deletions(-) diff --git a/examples/threading.cpp b/examples/threading.cpp index dd58dc340..46cf29662 100644 --- a/examples/threading.cpp +++ b/examples/threading.cpp @@ -69,6 +69,8 @@ bool update_device(mip::DeviceInterface& device, bool blocking) // Displaying it here makes it update more frequently. //display_progress(); + // Avoid failing the update function as long as the other thread is running. + // Doing so may cause a race condition (see comments in mip_interface_wait_for_reply). std::this_thread::sleep_for(std::chrono::milliseconds(5)); return true; } diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 28b51fb07..bd8717ec7 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -211,7 +211,7 @@ class DeviceInterface : public C::mip_interface void processUnparsedPackets() { C::mip_interface_process_unparsed_packets(this); } - CmdResult waitForReply(const C::mip_pending_cmd& cmd) { return C::mip_interface_wait_for_reply(this, &cmd); } + CmdResult waitForReply(C::mip_pending_cmd& cmd) { return C::mip_interface_wait_for_reply(this, &cmd); } bool defaultUpdate(bool blocking=false) { return C::mip_interface_default_update(this, blocking); } diff --git a/src/mip/mip_interface.c b/src/mip/mip_interface.c index 03ca1f7dd..98b8a8e3c 100644 --- a/src/mip/mip_interface.c +++ b/src/mip/mip_interface.c @@ -297,13 +297,28 @@ mip_cmd_queue* mip_interface_cmd_queue(mip_interface* device) /// ///@returns The final status of the command. /// -enum mip_cmd_result mip_interface_wait_for_reply(mip_interface* device, const mip_pending_cmd* cmd) +enum mip_cmd_result mip_interface_wait_for_reply(mip_interface* device, mip_pending_cmd* cmd) { enum mip_cmd_result status; while( !mip_cmd_result_is_finished(status = mip_pending_cmd_status(cmd)) ) { if( !mip_interface_update(device, true) ) + { + // When this function returns the pending command may be deallocated and the + // queue will have a dangling pointer. Therefore, the command must be manually + // errored out and de-queued. + // + // Note: This fix can still cause a race condition in multithreaded apps if the + // update thread happens to run right before the cmd is dequeued. The user is + // advised to not fail the update callback when another thread is handling + // reception, unless that thread is not running. Generally such updates shouldn't + // fail as long as the other thread is working normally anyway. + + mip_cmd_queue_dequeue(mip_interface_cmd_queue(device), cmd); + cmd->_status = MIP_STATUS_ERROR; + return MIP_STATUS_ERROR; + } } return status; } diff --git a/src/mip/mip_interface.h b/src/mip/mip_interface.h index dc9e10ea4..fafdb5f37 100644 --- a/src/mip/mip_interface.h +++ b/src/mip/mip_interface.h @@ -78,7 +78,7 @@ void mip_interface_receive_packet(mip_interface* device, const mip_packet* packe // Commands // -enum mip_cmd_result mip_interface_wait_for_reply(mip_interface* device, const mip_pending_cmd* cmd); +enum mip_cmd_result mip_interface_wait_for_reply(mip_interface* device, mip_pending_cmd* cmd); enum mip_cmd_result mip_interface_run_command(mip_interface* device, uint8_t descriptor_set, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length); enum mip_cmd_result mip_interface_run_command_with_response(mip_interface* device, uint8_t descriptor_set, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length, uint8_t response_descriptor, uint8_t* response_data, uint8_t* response_length_inout); enum mip_cmd_result mip_interface_run_command_packet(mip_interface* device, const mip_packet* packet, mip_pending_cmd* cmd); From c5f48339e6e842a65dc7e185d35a812b2845e70e Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 1 Nov 2022 17:23:42 -0400 Subject: [PATCH 014/252] Generate MIP definitions from branches/bitfield_accessors@53746. (#33) --- src/mip/definitions/commands_3dm.hpp | 21 ++ src/mip/definitions/commands_base.hpp | 55 ++++ src/mip/definitions/commands_filter.hpp | 29 ++ src/mip/definitions/commands_rtk.hpp | 55 ++++ src/mip/definitions/data_filter.hpp | 110 +++++++ src/mip/definitions/data_gnss.hpp | 368 ++++++++++++++++++++++++ src/mip/definitions/data_sensor.hpp | 32 +++ src/mip/definitions/data_shared.hpp | 13 + src/mip/definitions/descriptors.c | 342 +++++++++++----------- 9 files changed, 854 insertions(+), 171 deletions(-) diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 9b442f206..621dd12dd 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -926,6 +926,13 @@ struct GnssSbasSettings SBASOptions& operator=(int val) { value = val; return *this; } SBASOptions& operator|=(uint16_t val) { return *this = value | val; } SBASOptions& operator&=(uint16_t val) { return *this = value & val; } + + bool enableRanging() const { return (value & ENABLE_RANGING) > 0; } + void enableRanging(bool val) { if(val) value |= ENABLE_RANGING; else value &= ~ENABLE_RANGING; } + bool enableCorrections() const { return (value & ENABLE_CORRECTIONS) > 0; } + void enableCorrections(bool val) { if(val) value |= ENABLE_CORRECTIONS; else value &= ~ENABLE_CORRECTIONS; } + bool applyIntegrity() const { return (value & APPLY_INTEGRITY) > 0; } + void applyIntegrity(bool val) { if(val) value |= APPLY_INTEGRITY; else value &= ~APPLY_INTEGRITY; } }; FunctionSelector function = static_cast(0); @@ -1198,6 +1205,13 @@ struct GpioConfig PinMode& operator=(int val) { value = val; return *this; } PinMode& operator|=(uint8_t val) { return *this = value | val; } PinMode& operator&=(uint8_t val) { return *this = value & val; } + + bool openDrain() const { return (value & OPEN_DRAIN) > 0; } + void openDrain(bool val) { if(val) value |= OPEN_DRAIN; else value &= ~OPEN_DRAIN; } + bool pulldown() const { return (value & PULLDOWN) > 0; } + void pulldown(bool val) { if(val) value |= PULLDOWN; else value &= ~PULLDOWN; } + bool pullup() const { return (value & PULLUP) > 0; } + void pullup(bool val) { if(val) value |= PULLUP; else value &= ~PULLUP; } }; FunctionSelector function = static_cast(0); @@ -1498,6 +1512,13 @@ struct GetEventTriggerStatus Status& operator=(int val) { value = val; return *this; } Status& operator|=(uint8_t val) { return *this = value | val; } Status& operator&=(uint8_t val) { return *this = value & val; } + + bool active() const { return (value & ACTIVE) > 0; } + void active(bool val) { if(val) value |= ACTIVE; else value &= ~ACTIVE; } + bool enabled() const { return (value & ENABLED) > 0; } + void enabled(bool val) { if(val) value |= ENABLED; else value &= ~ENABLED; } + bool test() const { return (value & TEST) > 0; } + void test(bool val) { if(val) value |= TEST; else value &= ~TEST; } }; struct Entry diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 42a9128c2..9bb661ce2 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -119,6 +119,61 @@ struct CommandedTestBitsGq7 : Bitfield CommandedTestBitsGq7& operator=(int val) { value = val; return *this; } CommandedTestBitsGq7& operator|=(uint32_t val) { return *this = value | val; } CommandedTestBitsGq7& operator&=(uint32_t val) { return *this = value & val; } + + bool generalHardwareFault() const { return (value & GENERAL_HARDWARE_FAULT) > 0; } + void generalHardwareFault(bool val) { if(val) value |= GENERAL_HARDWARE_FAULT; else value &= ~GENERAL_HARDWARE_FAULT; } + bool generalFirmwareFault() const { return (value & GENERAL_FIRMWARE_FAULT) > 0; } + void generalFirmwareFault(bool val) { if(val) value |= GENERAL_FIRMWARE_FAULT; else value &= ~GENERAL_FIRMWARE_FAULT; } + bool timingOverload() const { return (value & TIMING_OVERLOAD) > 0; } + void timingOverload(bool val) { if(val) value |= TIMING_OVERLOAD; else value &= ~TIMING_OVERLOAD; } + bool bufferOverrun() const { return (value & BUFFER_OVERRUN) > 0; } + void bufferOverrun(bool val) { if(val) value |= BUFFER_OVERRUN; else value &= ~BUFFER_OVERRUN; } + uint32_t reserved() const { return (value & RESERVED) >> 4; } + void reserved(uint32_t val) { value = (value & ~RESERVED) | (val << 4); } + bool ipcImuFault() const { return (value & IPC_IMU_FAULT) > 0; } + void ipcImuFault(bool val) { if(val) value |= IPC_IMU_FAULT; else value &= ~IPC_IMU_FAULT; } + bool ipcNavFault() const { return (value & IPC_NAV_FAULT) > 0; } + void ipcNavFault(bool val) { if(val) value |= IPC_NAV_FAULT; else value &= ~IPC_NAV_FAULT; } + bool ipcGnssFault() const { return (value & IPC_GNSS_FAULT) > 0; } + void ipcGnssFault(bool val) { if(val) value |= IPC_GNSS_FAULT; else value &= ~IPC_GNSS_FAULT; } + bool commsFault() const { return (value & COMMS_FAULT) > 0; } + void commsFault(bool val) { if(val) value |= COMMS_FAULT; else value &= ~COMMS_FAULT; } + bool imuAccelFault() const { return (value & IMU_ACCEL_FAULT) > 0; } + void imuAccelFault(bool val) { if(val) value |= IMU_ACCEL_FAULT; else value &= ~IMU_ACCEL_FAULT; } + bool imuGyroFault() const { return (value & IMU_GYRO_FAULT) > 0; } + void imuGyroFault(bool val) { if(val) value |= IMU_GYRO_FAULT; else value &= ~IMU_GYRO_FAULT; } + bool imuMagFault() const { return (value & IMU_MAG_FAULT) > 0; } + void imuMagFault(bool val) { if(val) value |= IMU_MAG_FAULT; else value &= ~IMU_MAG_FAULT; } + bool imuPressFault() const { return (value & IMU_PRESS_FAULT) > 0; } + void imuPressFault(bool val) { if(val) value |= IMU_PRESS_FAULT; else value &= ~IMU_PRESS_FAULT; } + uint32_t imuReserved() const { return (value & IMU_RESERVED) >> 16; } + void imuReserved(uint32_t val) { value = (value & ~IMU_RESERVED) | (val << 16); } + bool imuCalError() const { return (value & IMU_CAL_ERROR) > 0; } + void imuCalError(bool val) { if(val) value |= IMU_CAL_ERROR; else value &= ~IMU_CAL_ERROR; } + bool imuGeneralFault() const { return (value & IMU_GENERAL_FAULT) > 0; } + void imuGeneralFault(bool val) { if(val) value |= IMU_GENERAL_FAULT; else value &= ~IMU_GENERAL_FAULT; } + uint32_t filtReserved() const { return (value & FILT_RESERVED) >> 20; } + void filtReserved(uint32_t val) { value = (value & ~FILT_RESERVED) | (val << 20); } + bool filtSolutionFault() const { return (value & FILT_SOLUTION_FAULT) > 0; } + void filtSolutionFault(bool val) { if(val) value |= FILT_SOLUTION_FAULT; else value &= ~FILT_SOLUTION_FAULT; } + bool filtGeneralFault() const { return (value & FILT_GENERAL_FAULT) > 0; } + void filtGeneralFault(bool val) { if(val) value |= FILT_GENERAL_FAULT; else value &= ~FILT_GENERAL_FAULT; } + bool gnssReceiver1Fault() const { return (value & GNSS_RECEIVER1_FAULT) > 0; } + void gnssReceiver1Fault(bool val) { if(val) value |= GNSS_RECEIVER1_FAULT; else value &= ~GNSS_RECEIVER1_FAULT; } + bool gnssAntenna1Fault() const { return (value & GNSS_ANTENNA1_FAULT) > 0; } + void gnssAntenna1Fault(bool val) { if(val) value |= GNSS_ANTENNA1_FAULT; else value &= ~GNSS_ANTENNA1_FAULT; } + bool gnssReceiver2Fault() const { return (value & GNSS_RECEIVER2_FAULT) > 0; } + void gnssReceiver2Fault(bool val) { if(val) value |= GNSS_RECEIVER2_FAULT; else value &= ~GNSS_RECEIVER2_FAULT; } + bool gnssAntenna2Fault() const { return (value & GNSS_ANTENNA2_FAULT) > 0; } + void gnssAntenna2Fault(bool val) { if(val) value |= GNSS_ANTENNA2_FAULT; else value &= ~GNSS_ANTENNA2_FAULT; } + bool gnssRtcmFailure() const { return (value & GNSS_RTCM_FAILURE) > 0; } + void gnssRtcmFailure(bool val) { if(val) value |= GNSS_RTCM_FAILURE; else value &= ~GNSS_RTCM_FAILURE; } + bool gnssRtkFault() const { return (value & GNSS_RTK_FAULT) > 0; } + void gnssRtkFault(bool val) { if(val) value |= GNSS_RTK_FAULT; else value &= ~GNSS_RTK_FAULT; } + bool gnssSolutionFault() const { return (value & GNSS_SOLUTION_FAULT) > 0; } + void gnssSolutionFault(bool val) { if(val) value |= GNSS_SOLUTION_FAULT; else value &= ~GNSS_SOLUTION_FAULT; } + bool gnssGeneralFault() const { return (value & GNSS_GENERAL_FAULT) > 0; } + void gnssGeneralFault(bool val) { if(val) value |= GNSS_GENERAL_FAULT; else value &= ~GNSS_GENERAL_FAULT; } }; diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index c2409196f..f9ebdc736 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -280,6 +280,21 @@ struct EstimationControl EnableFlags& operator=(int val) { value = val; return *this; } EnableFlags& operator|=(uint16_t val) { return *this = value | val; } EnableFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool gyroBias() const { return (value & GYRO_BIAS) > 0; } + void gyroBias(bool val) { if(val) value |= GYRO_BIAS; else value &= ~GYRO_BIAS; } + bool accelBias() const { return (value & ACCEL_BIAS) > 0; } + void accelBias(bool val) { if(val) value |= ACCEL_BIAS; else value &= ~ACCEL_BIAS; } + bool gyroScaleFactor() const { return (value & GYRO_SCALE_FACTOR) > 0; } + void gyroScaleFactor(bool val) { if(val) value |= GYRO_SCALE_FACTOR; else value &= ~GYRO_SCALE_FACTOR; } + bool accelScaleFactor() const { return (value & ACCEL_SCALE_FACTOR) > 0; } + void accelScaleFactor(bool val) { if(val) value |= ACCEL_SCALE_FACTOR; else value &= ~ACCEL_SCALE_FACTOR; } + bool antennaOffset() const { return (value & ANTENNA_OFFSET) > 0; } + void antennaOffset(bool val) { if(val) value |= ANTENNA_OFFSET; else value &= ~ANTENNA_OFFSET; } + bool autoMagHardIron() const { return (value & AUTO_MAG_HARD_IRON) > 0; } + void autoMagHardIron(bool val) { if(val) value |= AUTO_MAG_HARD_IRON; else value &= ~AUTO_MAG_HARD_IRON; } + bool autoMagSoftIron() const { return (value & AUTO_MAG_SOFT_IRON) > 0; } + void autoMagSoftIron(bool val) { if(val) value |= AUTO_MAG_SOFT_IRON; else value &= ~AUTO_MAG_SOFT_IRON; } }; FunctionSelector function = static_cast(0); @@ -457,6 +472,13 @@ struct TareOrientation MipTareAxes& operator=(int val) { value = val; return *this; } MipTareAxes& operator|=(uint8_t val) { return *this = value | val; } MipTareAxes& operator&=(uint8_t val) { return *this = value & val; } + + bool roll() const { return (value & ROLL) > 0; } + void roll(bool val) { if(val) value |= ROLL; else value &= ~ROLL; } + bool pitch() const { return (value & PITCH) > 0; } + void pitch(bool val) { if(val) value |= PITCH; else value &= ~PITCH; } + bool yaw() const { return (value & YAW) > 0; } + void yaw(bool val) { if(val) value |= YAW; else value &= ~YAW; } }; FunctionSelector function = static_cast(0); @@ -1313,6 +1335,13 @@ struct InitializationConfiguration AlignmentSelector& operator=(int val) { value = val; return *this; } AlignmentSelector& operator|=(uint8_t val) { return *this = value | val; } AlignmentSelector& operator&=(uint8_t val) { return *this = value & val; } + + bool dualAntenna() const { return (value & DUAL_ANTENNA) > 0; } + void dualAntenna(bool val) { if(val) value |= DUAL_ANTENNA; else value &= ~DUAL_ANTENNA; } + bool kinematic() const { return (value & KINEMATIC) > 0; } + void kinematic(bool val) { if(val) value |= KINEMATIC; else value &= ~KINEMATIC; } + bool magnetometer() const { return (value & MAGNETOMETER) > 0; } + void magnetometer(bool val) { if(val) value |= MAGNETOMETER; else value &= ~MAGNETOMETER; } }; enum class InitialConditionSource : uint8_t diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 43364dbf5..8cdf9b891 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -115,6 +115,29 @@ struct GetStatusFlags StatusFlagsLegacy& operator=(int val) { value = val; return *this; } StatusFlagsLegacy& operator|=(uint32_t val) { return *this = value | val; } StatusFlagsLegacy& operator&=(uint32_t val) { return *this = value & val; } + + uint32_t controllerstate() const { return (value & CONTROLLERSTATE) >> 0; } + void controllerstate(uint32_t val) { value = (value & ~CONTROLLERSTATE) | (val << 0); } + uint32_t platformstate() const { return (value & PLATFORMSTATE) >> 3; } + void platformstate(uint32_t val) { value = (value & ~PLATFORMSTATE) | (val << 3); } + uint32_t controllerstatuscode() const { return (value & CONTROLLERSTATUSCODE) >> 8; } + void controllerstatuscode(uint32_t val) { value = (value & ~CONTROLLERSTATUSCODE) | (val << 8); } + uint32_t platformstatuscode() const { return (value & PLATFORMSTATUSCODE) >> 11; } + void platformstatuscode(uint32_t val) { value = (value & ~PLATFORMSTATUSCODE) | (val << 11); } + uint32_t resetcode() const { return (value & RESETCODE) >> 14; } + void resetcode(uint32_t val) { value = (value & ~RESETCODE) | (val << 14); } + uint32_t signalquality() const { return (value & SIGNALQUALITY) >> 16; } + void signalquality(uint32_t val) { value = (value & ~SIGNALQUALITY) | (val << 16); } + uint32_t reserved() const { return (value & RESERVED) >> 20; } + void reserved(uint32_t val) { value = (value & ~RESERVED) | (val << 20); } + uint32_t rssi() const { return (value & RSSI) >> 20; } + void rssi(uint32_t val) { value = (value & ~RSSI) | (val << 20); } + uint32_t rsrp() const { return (value & RSRP) >> 26; } + void rsrp(uint32_t val) { value = (value & ~RSRP) | (val << 26); } + uint32_t rsrq() const { return (value & RSRQ) >> 28; } + void rsrq(uint32_t val) { value = (value & ~RSRQ) | (val << 28); } + uint32_t sinr() const { return (value & SINR) >> 30; } + void sinr(uint32_t val) { value = (value & ~SINR) | (val << 30); } }; struct StatusFlags : Bitfield @@ -144,6 +167,31 @@ struct GetStatusFlags StatusFlags& operator=(int val) { value = val; return *this; } StatusFlags& operator|=(uint32_t val) { return *this = value | val; } StatusFlags& operator&=(uint32_t val) { return *this = value & val; } + + uint32_t modemState() const { return (value & MODEM_STATE) >> 0; } + void modemState(uint32_t val) { value = (value & ~MODEM_STATE) | (val << 0); } + uint32_t connectionType() const { return (value & CONNECTION_TYPE) >> 4; } + void connectionType(uint32_t val) { value = (value & ~CONNECTION_TYPE) | (val << 4); } + uint32_t rssi() const { return (value & RSSI) >> 8; } + void rssi(uint32_t val) { value = (value & ~RSSI) | (val << 8); } + uint32_t signalQuality() const { return (value & SIGNAL_QUALITY) >> 16; } + void signalQuality(uint32_t val) { value = (value & ~SIGNAL_QUALITY) | (val << 16); } + uint32_t towerChangeIndicator() const { return (value & TOWER_CHANGE_INDICATOR) >> 20; } + void towerChangeIndicator(uint32_t val) { value = (value & ~TOWER_CHANGE_INDICATOR) | (val << 20); } + bool nmeaTimeout() const { return (value & NMEA_TIMEOUT) > 0; } + void nmeaTimeout(bool val) { if(val) value |= NMEA_TIMEOUT; else value &= ~NMEA_TIMEOUT; } + bool serverTimeout() const { return (value & SERVER_TIMEOUT) > 0; } + void serverTimeout(bool val) { if(val) value |= SERVER_TIMEOUT; else value &= ~SERVER_TIMEOUT; } + bool rtcmTimeout() const { return (value & RTCM_TIMEOUT) > 0; } + void rtcmTimeout(bool val) { if(val) value |= RTCM_TIMEOUT; else value &= ~RTCM_TIMEOUT; } + bool deviceOutOfRange() const { return (value & DEVICE_OUT_OF_RANGE) > 0; } + void deviceOutOfRange(bool val) { if(val) value |= DEVICE_OUT_OF_RANGE; else value &= ~DEVICE_OUT_OF_RANGE; } + bool correctionsUnavailable() const { return (value & CORRECTIONS_UNAVAILABLE) > 0; } + void correctionsUnavailable(bool val) { if(val) value |= CORRECTIONS_UNAVAILABLE; else value &= ~CORRECTIONS_UNAVAILABLE; } + bool reserved() const { return (value & RESERVED) > 0; } + void reserved(bool val) { if(val) value |= RESERVED; else value &= ~RESERVED; } + uint32_t version() const { return (value & VERSION) >> 30; } + void version(uint32_t val) { value = (value & ~VERSION) | (val << 30); } }; @@ -432,6 +480,13 @@ struct ServiceStatus ServiceFlags& operator=(int val) { value = val; return *this; } ServiceFlags& operator|=(uint8_t val) { return *this = value | val; } ServiceFlags& operator&=(uint8_t val) { return *this = value & val; } + + bool throttle() const { return (value & THROTTLE) > 0; } + void throttle(bool val) { if(val) value |= THROTTLE; else value &= ~THROTTLE; } + bool correctionsUnavailable() const { return (value & CORRECTIONS_UNAVAILABLE) > 0; } + void correctionsUnavailable(bool val) { if(val) value |= CORRECTIONS_UNAVAILABLE; else value &= ~CORRECTIONS_UNAVAILABLE; } + uint8_t reserved() const { return (value & RESERVED) >> 2; } + void reserved(uint8_t val) { value = (value & ~RESERVED) | (val << 2); } }; uint32_t reserved1 = 0; diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index c878a52e8..d6a9b1599 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -161,6 +161,63 @@ struct FilterStatusFlags : Bitfield FilterStatusFlags& operator=(int val) { value = val; return *this; } FilterStatusFlags& operator|=(uint16_t val) { return *this = value | val; } FilterStatusFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool gx5InitNoAttitude() const { return (value & GX5_INIT_NO_ATTITUDE) > 0; } + void gx5InitNoAttitude(bool val) { if(val) value |= GX5_INIT_NO_ATTITUDE; else value &= ~GX5_INIT_NO_ATTITUDE; } + bool gx5InitNoPositionVelocity() const { return (value & GX5_INIT_NO_POSITION_VELOCITY) > 0; } + void gx5InitNoPositionVelocity(bool val) { if(val) value |= GX5_INIT_NO_POSITION_VELOCITY; else value &= ~GX5_INIT_NO_POSITION_VELOCITY; } + bool gx5RunImuUnavailable() const { return (value & GX5_RUN_IMU_UNAVAILABLE) > 0; } + void gx5RunImuUnavailable(bool val) { if(val) value |= GX5_RUN_IMU_UNAVAILABLE; else value &= ~GX5_RUN_IMU_UNAVAILABLE; } + bool gx5RunGpsUnavailable() const { return (value & GX5_RUN_GPS_UNAVAILABLE) > 0; } + void gx5RunGpsUnavailable(bool val) { if(val) value |= GX5_RUN_GPS_UNAVAILABLE; else value &= ~GX5_RUN_GPS_UNAVAILABLE; } + bool gx5RunMatrixSingularity() const { return (value & GX5_RUN_MATRIX_SINGULARITY) > 0; } + void gx5RunMatrixSingularity(bool val) { if(val) value |= GX5_RUN_MATRIX_SINGULARITY; else value &= ~GX5_RUN_MATRIX_SINGULARITY; } + bool gx5RunPositionCovarianceWarning() const { return (value & GX5_RUN_POSITION_COVARIANCE_WARNING) > 0; } + void gx5RunPositionCovarianceWarning(bool val) { if(val) value |= GX5_RUN_POSITION_COVARIANCE_WARNING; else value &= ~GX5_RUN_POSITION_COVARIANCE_WARNING; } + bool gx5RunVelocityCovarianceWarning() const { return (value & GX5_RUN_VELOCITY_COVARIANCE_WARNING) > 0; } + void gx5RunVelocityCovarianceWarning(bool val) { if(val) value |= GX5_RUN_VELOCITY_COVARIANCE_WARNING; else value &= ~GX5_RUN_VELOCITY_COVARIANCE_WARNING; } + bool gx5RunAttitudeCovarianceWarning() const { return (value & GX5_RUN_ATTITUDE_COVARIANCE_WARNING) > 0; } + void gx5RunAttitudeCovarianceWarning(bool val) { if(val) value |= GX5_RUN_ATTITUDE_COVARIANCE_WARNING; else value &= ~GX5_RUN_ATTITUDE_COVARIANCE_WARNING; } + bool gx5RunNanInSolutionWarning() const { return (value & GX5_RUN_NAN_IN_SOLUTION_WARNING) > 0; } + void gx5RunNanInSolutionWarning(bool val) { if(val) value |= GX5_RUN_NAN_IN_SOLUTION_WARNING; else value &= ~GX5_RUN_NAN_IN_SOLUTION_WARNING; } + bool gx5RunGyroBiasEstHighWarning() const { return (value & GX5_RUN_GYRO_BIAS_EST_HIGH_WARNING) > 0; } + void gx5RunGyroBiasEstHighWarning(bool val) { if(val) value |= GX5_RUN_GYRO_BIAS_EST_HIGH_WARNING; else value &= ~GX5_RUN_GYRO_BIAS_EST_HIGH_WARNING; } + bool gx5RunAccelBiasEstHighWarning() const { return (value & GX5_RUN_ACCEL_BIAS_EST_HIGH_WARNING) > 0; } + void gx5RunAccelBiasEstHighWarning(bool val) { if(val) value |= GX5_RUN_ACCEL_BIAS_EST_HIGH_WARNING; else value &= ~GX5_RUN_ACCEL_BIAS_EST_HIGH_WARNING; } + bool gx5RunGyroScaleFactorEstHighWarning() const { return (value & GX5_RUN_GYRO_SCALE_FACTOR_EST_HIGH_WARNING) > 0; } + void gx5RunGyroScaleFactorEstHighWarning(bool val) { if(val) value |= GX5_RUN_GYRO_SCALE_FACTOR_EST_HIGH_WARNING; else value &= ~GX5_RUN_GYRO_SCALE_FACTOR_EST_HIGH_WARNING; } + bool gx5RunAccelScaleFactorEstHighWarning() const { return (value & GX5_RUN_ACCEL_SCALE_FACTOR_EST_HIGH_WARNING) > 0; } + void gx5RunAccelScaleFactorEstHighWarning(bool val) { if(val) value |= GX5_RUN_ACCEL_SCALE_FACTOR_EST_HIGH_WARNING; else value &= ~GX5_RUN_ACCEL_SCALE_FACTOR_EST_HIGH_WARNING; } + bool gx5RunMagBiasEstHighWarning() const { return (value & GX5_RUN_MAG_BIAS_EST_HIGH_WARNING) > 0; } + void gx5RunMagBiasEstHighWarning(bool val) { if(val) value |= GX5_RUN_MAG_BIAS_EST_HIGH_WARNING; else value &= ~GX5_RUN_MAG_BIAS_EST_HIGH_WARNING; } + bool gx5RunAntOffsetCorrectionEstHighWarning() const { return (value & GX5_RUN_ANT_OFFSET_CORRECTION_EST_HIGH_WARNING) > 0; } + void gx5RunAntOffsetCorrectionEstHighWarning(bool val) { if(val) value |= GX5_RUN_ANT_OFFSET_CORRECTION_EST_HIGH_WARNING; else value &= ~GX5_RUN_ANT_OFFSET_CORRECTION_EST_HIGH_WARNING; } + bool gx5RunMagHardIronEstHighWarning() const { return (value & GX5_RUN_MAG_HARD_IRON_EST_HIGH_WARNING) > 0; } + void gx5RunMagHardIronEstHighWarning(bool val) { if(val) value |= GX5_RUN_MAG_HARD_IRON_EST_HIGH_WARNING; else value &= ~GX5_RUN_MAG_HARD_IRON_EST_HIGH_WARNING; } + bool gx5RunMagSoftIronEstHighWarning() const { return (value & GX5_RUN_MAG_SOFT_IRON_EST_HIGH_WARNING) > 0; } + void gx5RunMagSoftIronEstHighWarning(bool val) { if(val) value |= GX5_RUN_MAG_SOFT_IRON_EST_HIGH_WARNING; else value &= ~GX5_RUN_MAG_SOFT_IRON_EST_HIGH_WARNING; } + uint16_t gq7FilterCondition() const { return (value & GQ7_FILTER_CONDITION) >> 0; } + void gq7FilterCondition(uint16_t val) { value = (value & ~GQ7_FILTER_CONDITION) | (val << 0); } + bool gq7RollPitchWarning() const { return (value & GQ7_ROLL_PITCH_WARNING) > 0; } + void gq7RollPitchWarning(bool val) { if(val) value |= GQ7_ROLL_PITCH_WARNING; else value &= ~GQ7_ROLL_PITCH_WARNING; } + bool gq7HeadingWarning() const { return (value & GQ7_HEADING_WARNING) > 0; } + void gq7HeadingWarning(bool val) { if(val) value |= GQ7_HEADING_WARNING; else value &= ~GQ7_HEADING_WARNING; } + bool gq7PositionWarning() const { return (value & GQ7_POSITION_WARNING) > 0; } + void gq7PositionWarning(bool val) { if(val) value |= GQ7_POSITION_WARNING; else value &= ~GQ7_POSITION_WARNING; } + bool gq7VelocityWarning() const { return (value & GQ7_VELOCITY_WARNING) > 0; } + void gq7VelocityWarning(bool val) { if(val) value |= GQ7_VELOCITY_WARNING; else value &= ~GQ7_VELOCITY_WARNING; } + bool gq7ImuBiasWarning() const { return (value & GQ7_IMU_BIAS_WARNING) > 0; } + void gq7ImuBiasWarning(bool val) { if(val) value |= GQ7_IMU_BIAS_WARNING; else value &= ~GQ7_IMU_BIAS_WARNING; } + bool gq7GnssClkWarning() const { return (value & GQ7_GNSS_CLK_WARNING) > 0; } + void gq7GnssClkWarning(bool val) { if(val) value |= GQ7_GNSS_CLK_WARNING; else value &= ~GQ7_GNSS_CLK_WARNING; } + bool gq7AntennaLeverArmWarning() const { return (value & GQ7_ANTENNA_LEVER_ARM_WARNING) > 0; } + void gq7AntennaLeverArmWarning(bool val) { if(val) value |= GQ7_ANTENNA_LEVER_ARM_WARNING; else value &= ~GQ7_ANTENNA_LEVER_ARM_WARNING; } + bool gq7MountingTransformWarning() const { return (value & GQ7_MOUNTING_TRANSFORM_WARNING) > 0; } + void gq7MountingTransformWarning(bool val) { if(val) value |= GQ7_MOUNTING_TRANSFORM_WARNING; else value &= ~GQ7_MOUNTING_TRANSFORM_WARNING; } + bool gq7TimeSyncWarning() const { return (value & GQ7_TIME_SYNC_WARNING) > 0; } + void gq7TimeSyncWarning(bool val) { if(val) value |= GQ7_TIME_SYNC_WARNING; else value &= ~GQ7_TIME_SYNC_WARNING; } + uint16_t gq7SolutionError() const { return (value & GQ7_SOLUTION_ERROR) >> 12; } + void gq7SolutionError(uint16_t val) { value = (value & ~GQ7_SOLUTION_ERROR) | (val << 12); } }; enum class FilterAidingMeasurementType : uint8_t @@ -194,6 +251,19 @@ struct FilterMeasurementIndicator : Bitfield FilterMeasurementIndicator& operator=(int val) { value = val; return *this; } FilterMeasurementIndicator& operator|=(uint8_t val) { return *this = value | val; } FilterMeasurementIndicator& operator&=(uint8_t val) { return *this = value & val; } + + bool enabled() const { return (value & ENABLED) > 0; } + void enabled(bool val) { if(val) value |= ENABLED; else value &= ~ENABLED; } + bool used() const { return (value & USED) > 0; } + void used(bool val) { if(val) value |= USED; else value &= ~USED; } + bool residualHighWarning() const { return (value & RESIDUAL_HIGH_WARNING) > 0; } + void residualHighWarning(bool val) { if(val) value |= RESIDUAL_HIGH_WARNING; else value &= ~RESIDUAL_HIGH_WARNING; } + bool sampleTimeWarning() const { return (value & SAMPLE_TIME_WARNING) > 0; } + void sampleTimeWarning(bool val) { if(val) value |= SAMPLE_TIME_WARNING; else value &= ~SAMPLE_TIME_WARNING; } + bool configurationError() const { return (value & CONFIGURATION_ERROR) > 0; } + void configurationError(bool val) { if(val) value |= CONFIGURATION_ERROR; else value &= ~CONFIGURATION_ERROR; } + bool maxNumMeasExceeded() const { return (value & MAX_NUM_MEAS_EXCEEDED) > 0; } + void maxNumMeasExceeded(bool val) { if(val) value |= MAX_NUM_MEAS_EXCEEDED; else value &= ~MAX_NUM_MEAS_EXCEEDED; } }; struct GnssAidStatusFlags : Bitfield @@ -227,6 +297,39 @@ struct GnssAidStatusFlags : Bitfield GnssAidStatusFlags& operator=(int val) { value = val; return *this; } GnssAidStatusFlags& operator|=(uint16_t val) { return *this = value | val; } GnssAidStatusFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tightCoupling() const { return (value & TIGHT_COUPLING) > 0; } + void tightCoupling(bool val) { if(val) value |= TIGHT_COUPLING; else value &= ~TIGHT_COUPLING; } + bool differential() const { return (value & DIFFERENTIAL) > 0; } + void differential(bool val) { if(val) value |= DIFFERENTIAL; else value &= ~DIFFERENTIAL; } + bool integerFix() const { return (value & INTEGER_FIX) > 0; } + void integerFix(bool val) { if(val) value |= INTEGER_FIX; else value &= ~INTEGER_FIX; } + bool gpsL1() const { return (value & GPS_L1) > 0; } + void gpsL1(bool val) { if(val) value |= GPS_L1; else value &= ~GPS_L1; } + bool gpsL2() const { return (value & GPS_L2) > 0; } + void gpsL2(bool val) { if(val) value |= GPS_L2; else value &= ~GPS_L2; } + bool gpsL5() const { return (value & GPS_L5) > 0; } + void gpsL5(bool val) { if(val) value |= GPS_L5; else value &= ~GPS_L5; } + bool gloL1() const { return (value & GLO_L1) > 0; } + void gloL1(bool val) { if(val) value |= GLO_L1; else value &= ~GLO_L1; } + bool gloL2() const { return (value & GLO_L2) > 0; } + void gloL2(bool val) { if(val) value |= GLO_L2; else value &= ~GLO_L2; } + bool galE1() const { return (value & GAL_E1) > 0; } + void galE1(bool val) { if(val) value |= GAL_E1; else value &= ~GAL_E1; } + bool galE5() const { return (value & GAL_E5) > 0; } + void galE5(bool val) { if(val) value |= GAL_E5; else value &= ~GAL_E5; } + bool galE6() const { return (value & GAL_E6) > 0; } + void galE6(bool val) { if(val) value |= GAL_E6; else value &= ~GAL_E6; } + bool beiB1() const { return (value & BEI_B1) > 0; } + void beiB1(bool val) { if(val) value |= BEI_B1; else value &= ~BEI_B1; } + bool beiB2() const { return (value & BEI_B2) > 0; } + void beiB2(bool val) { if(val) value |= BEI_B2; else value &= ~BEI_B2; } + bool beiB3() const { return (value & BEI_B3) > 0; } + void beiB3(bool val) { if(val) value |= BEI_B3; else value &= ~BEI_B3; } + bool noFix() const { return (value & NO_FIX) > 0; } + void noFix(bool val) { if(val) value |= NO_FIX; else value &= ~NO_FIX; } + bool configError() const { return (value & CONFIG_ERROR) > 0; } + void configError(bool val) { if(val) value |= CONFIG_ERROR; else value &= ~CONFIG_ERROR; } }; @@ -1829,6 +1932,13 @@ struct GnssDualAntennaStatus DualAntennaStatusFlags& operator=(int val) { value = val; return *this; } DualAntennaStatusFlags& operator|=(uint16_t val) { return *this = value | val; } DualAntennaStatusFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool rcv1DataValid() const { return (value & RCV_1_DATA_VALID) > 0; } + void rcv1DataValid(bool val) { if(val) value |= RCV_1_DATA_VALID; else value &= ~RCV_1_DATA_VALID; } + bool rcv2DataValid() const { return (value & RCV_2_DATA_VALID) > 0; } + void rcv2DataValid(bool val) { if(val) value |= RCV_2_DATA_VALID; else value &= ~RCV_2_DATA_VALID; } + bool antennaOffsetsValid() const { return (value & ANTENNA_OFFSETS_VALID) > 0; } + void antennaOffsetsValid(bool val) { if(val) value |= ANTENNA_OFFSETS_VALID; else value &= ~ANTENNA_OFFSETS_VALID; } }; float time_of_week = 0; ///< Last dual-antenna GNSS aiding measurement time of week [seconds] diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 33cca17a3..a8a465bd4 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -206,6 +206,19 @@ struct PosLlh ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool latLon() const { return (value & LAT_LON) > 0; } + void latLon(bool val) { if(val) value |= LAT_LON; else value &= ~LAT_LON; } + bool ellipsoidHeight() const { return (value & ELLIPSOID_HEIGHT) > 0; } + void ellipsoidHeight(bool val) { if(val) value |= ELLIPSOID_HEIGHT; else value &= ~ELLIPSOID_HEIGHT; } + bool mslHeight() const { return (value & MSL_HEIGHT) > 0; } + void mslHeight(bool val) { if(val) value |= MSL_HEIGHT; else value &= ~MSL_HEIGHT; } + bool horizontalAccuracy() const { return (value & HORIZONTAL_ACCURACY) > 0; } + void horizontalAccuracy(bool val) { if(val) value |= HORIZONTAL_ACCURACY; else value &= ~HORIZONTAL_ACCURACY; } + bool verticalAccuracy() const { return (value & VERTICAL_ACCURACY) > 0; } + void verticalAccuracy(bool val) { if(val) value |= VERTICAL_ACCURACY; else value &= ~VERTICAL_ACCURACY; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double latitude = 0; ///< [degrees] @@ -258,6 +271,13 @@ struct PosEcef ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool position() const { return (value & POSITION) > 0; } + void position(bool val) { if(val) value |= POSITION; else value &= ~POSITION; } + bool positionAccuracy() const { return (value & POSITION_ACCURACY) > 0; } + void positionAccuracy(bool val) { if(val) value |= POSITION_ACCURACY; else value &= ~POSITION_ACCURACY; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double x[3] = {0}; ///< [meters] @@ -310,6 +330,21 @@ struct VelNed ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool velocity() const { return (value & VELOCITY) > 0; } + void velocity(bool val) { if(val) value |= VELOCITY; else value &= ~VELOCITY; } + bool speed3d() const { return (value & SPEED_3D) > 0; } + void speed3d(bool val) { if(val) value |= SPEED_3D; else value &= ~SPEED_3D; } + bool groundSpeed() const { return (value & GROUND_SPEED) > 0; } + void groundSpeed(bool val) { if(val) value |= GROUND_SPEED; else value &= ~GROUND_SPEED; } + bool heading() const { return (value & HEADING) > 0; } + void heading(bool val) { if(val) value |= HEADING; else value &= ~HEADING; } + bool speedAccuracy() const { return (value & SPEED_ACCURACY) > 0; } + void speedAccuracy(bool val) { if(val) value |= SPEED_ACCURACY; else value &= ~SPEED_ACCURACY; } + bool headingAccuracy() const { return (value & HEADING_ACCURACY) > 0; } + void headingAccuracy(bool val) { if(val) value |= HEADING_ACCURACY; else value &= ~HEADING_ACCURACY; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; float v[3] = {0}; ///< [meters/second] @@ -362,6 +397,13 @@ struct VelEcef ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool velocity() const { return (value & VELOCITY) > 0; } + void velocity(bool val) { if(val) value |= VELOCITY; else value &= ~VELOCITY; } + bool velocityAccuracy() const { return (value & VELOCITY_ACCURACY) > 0; } + void velocityAccuracy(bool val) { if(val) value |= VELOCITY_ACCURACY; else value &= ~VELOCITY_ACCURACY; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; float v[3] = {0}; ///< [meters/second] @@ -415,6 +457,23 @@ struct Dop ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool gdop() const { return (value & GDOP) > 0; } + void gdop(bool val) { if(val) value |= GDOP; else value &= ~GDOP; } + bool pdop() const { return (value & PDOP) > 0; } + void pdop(bool val) { if(val) value |= PDOP; else value &= ~PDOP; } + bool hdop() const { return (value & HDOP) > 0; } + void hdop(bool val) { if(val) value |= HDOP; else value &= ~HDOP; } + bool vdop() const { return (value & VDOP) > 0; } + void vdop(bool val) { if(val) value |= VDOP; else value &= ~VDOP; } + bool tdop() const { return (value & TDOP) > 0; } + void tdop(bool val) { if(val) value |= TDOP; else value &= ~TDOP; } + bool ndop() const { return (value & NDOP) > 0; } + void ndop(bool val) { if(val) value |= NDOP; else value &= ~NDOP; } + bool edop() const { return (value & EDOP) > 0; } + void edop(bool val) { if(val) value |= EDOP; else value &= ~EDOP; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; float gdop = 0; ///< Geometric DOP @@ -468,6 +527,13 @@ struct UtcTime ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool gnssDateTime() const { return (value & GNSS_DATE_TIME) > 0; } + void gnssDateTime(bool val) { if(val) value |= GNSS_DATE_TIME; else value &= ~GNSS_DATE_TIME; } + bool leapSecondsKnown() const { return (value & LEAP_SECONDS_KNOWN) > 0; } + void leapSecondsKnown(bool val) { if(val) value |= LEAP_SECONDS_KNOWN; else value &= ~LEAP_SECONDS_KNOWN; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint16_t year = 0; @@ -521,6 +587,13 @@ struct GpsTime ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double tow = 0; ///< GPS Time of week [seconds] @@ -570,6 +643,15 @@ struct ClockInfo ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool bias() const { return (value & BIAS) > 0; } + void bias(bool val) { if(val) value |= BIAS; else value &= ~BIAS; } + bool drift() const { return (value & DRIFT) > 0; } + void drift(bool val) { if(val) value |= DRIFT; else value &= ~DRIFT; } + bool accuracyEstimate() const { return (value & ACCURACY_ESTIMATE) > 0; } + void accuracyEstimate(bool val) { if(val) value |= ACCURACY_ESTIMATE; else value &= ~ACCURACY_ESTIMATE; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double bias = 0; ///< [seconds] @@ -629,6 +711,11 @@ struct FixInfo FixFlags& operator=(int val) { value = val; return *this; } FixFlags& operator|=(uint16_t val) { return *this = value | val; } FixFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool sbasUsed() const { return (value & SBAS_USED) > 0; } + void sbasUsed(bool val) { if(val) value |= SBAS_USED; else value &= ~SBAS_USED; } + bool dgnssUsed() const { return (value & DGNSS_USED) > 0; } + void dgnssUsed(bool val) { if(val) value |= DGNSS_USED; else value &= ~DGNSS_USED; } }; struct ValidFlags : Bitfield @@ -650,6 +737,15 @@ struct FixInfo ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool fixType() const { return (value & FIX_TYPE) > 0; } + void fixType(bool val) { if(val) value |= FIX_TYPE; else value &= ~FIX_TYPE; } + bool numSv() const { return (value & NUM_SV) > 0; } + void numSv(bool val) { if(val) value |= NUM_SV; else value &= ~NUM_SV; } + bool fixFlags() const { return (value & FIX_FLAGS) > 0; } + void fixFlags(bool val) { if(val) value |= FIX_FLAGS; else value &= ~FIX_FLAGS; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; FixType fix_type = static_cast(0); @@ -700,6 +796,11 @@ struct SvInfo SVFlags& operator=(int val) { value = val; return *this; } SVFlags& operator|=(uint16_t val) { return *this = value | val; } SVFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool usedForNavigation() const { return (value & USED_FOR_NAVIGATION) > 0; } + void usedForNavigation(bool val) { if(val) value |= USED_FOR_NAVIGATION; else value &= ~USED_FOR_NAVIGATION; } + bool healthy() const { return (value & HEALTHY) > 0; } + void healthy(bool val) { if(val) value |= HEALTHY; else value &= ~HEALTHY; } }; struct ValidFlags : Bitfield @@ -724,6 +825,21 @@ struct SvInfo ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool channel() const { return (value & CHANNEL) > 0; } + void channel(bool val) { if(val) value |= CHANNEL; else value &= ~CHANNEL; } + bool svId() const { return (value & SV_ID) > 0; } + void svId(bool val) { if(val) value |= SV_ID; else value &= ~SV_ID; } + bool carrierNoiseRatio() const { return (value & CARRIER_NOISE_RATIO) > 0; } + void carrierNoiseRatio(bool val) { if(val) value |= CARRIER_NOISE_RATIO; else value &= ~CARRIER_NOISE_RATIO; } + bool azimuth() const { return (value & AZIMUTH) > 0; } + void azimuth(bool val) { if(val) value |= AZIMUTH; else value &= ~AZIMUTH; } + bool elevation() const { return (value & ELEVATION) > 0; } + void elevation(bool val) { if(val) value |= ELEVATION; else value &= ~ELEVATION; } + bool svFlags() const { return (value & SV_FLAGS) > 0; } + void svFlags(bool val) { if(val) value |= SV_FLAGS; else value &= ~SV_FLAGS; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t channel = 0; ///< Receiver channel number @@ -800,6 +916,15 @@ struct HwStatus ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool sensorState() const { return (value & SENSOR_STATE) > 0; } + void sensorState(bool val) { if(val) value |= SENSOR_STATE; else value &= ~SENSOR_STATE; } + bool antennaState() const { return (value & ANTENNA_STATE) > 0; } + void antennaState(bool val) { if(val) value |= ANTENNA_STATE; else value &= ~ANTENNA_STATE; } + bool antennaPower() const { return (value & ANTENNA_POWER) > 0; } + void antennaPower(bool val) { if(val) value |= ANTENNA_POWER; else value &= ~ANTENNA_POWER; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; ReceiverState receiver_state = static_cast(0); @@ -863,6 +988,17 @@ struct DgpsInfo ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool age() const { return (value & AGE) > 0; } + void age(bool val) { if(val) value |= AGE; else value &= ~AGE; } + bool baseStationId() const { return (value & BASE_STATION_ID) > 0; } + void baseStationId(bool val) { if(val) value |= BASE_STATION_ID; else value &= ~BASE_STATION_ID; } + bool baseStationStatus() const { return (value & BASE_STATION_STATUS) > 0; } + void baseStationStatus(bool val) { if(val) value |= BASE_STATION_STATUS; else value &= ~BASE_STATION_STATUS; } + bool numChannels() const { return (value & NUM_CHANNELS) > 0; } + void numChannels(bool val) { if(val) value |= NUM_CHANNELS; else value &= ~NUM_CHANNELS; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t sv_id = 0; @@ -917,6 +1053,17 @@ struct DgpsChannel ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool id() const { return (value & ID) > 0; } + void id(bool val) { if(val) value |= ID; else value &= ~ID; } + bool age() const { return (value & AGE) > 0; } + void age(bool val) { if(val) value |= AGE; else value &= ~AGE; } + bool rangeCorrection() const { return (value & RANGE_CORRECTION) > 0; } + void rangeCorrection(bool val) { if(val) value |= RANGE_CORRECTION; else value &= ~RANGE_CORRECTION; } + bool rangeRateCorrection() const { return (value & RANGE_RATE_CORRECTION) > 0; } + void rangeRateCorrection(bool val) { if(val) value |= RANGE_RATE_CORRECTION; else value &= ~RANGE_RATE_CORRECTION; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t sv_id = 0; @@ -971,6 +1118,17 @@ struct ClockInfo2 ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool bias() const { return (value & BIAS) > 0; } + void bias(bool val) { if(val) value |= BIAS; else value &= ~BIAS; } + bool drift() const { return (value & DRIFT) > 0; } + void drift(bool val) { if(val) value |= DRIFT; else value &= ~DRIFT; } + bool biasAccuracy() const { return (value & BIAS_ACCURACY) > 0; } + void biasAccuracy(bool val) { if(val) value |= BIAS_ACCURACY; else value &= ~BIAS_ACCURACY; } + bool driftAccuracy() const { return (value & DRIFT_ACCURACY) > 0; } + void driftAccuracy(bool val) { if(val) value |= DRIFT_ACCURACY; else value &= ~DRIFT_ACCURACY; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double bias = 0; @@ -1019,6 +1177,9 @@ struct GpsLeapSeconds ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool leapSeconds() const { return (value & LEAP_SECONDS) > 0; } + void leapSeconds(bool val) { if(val) value |= LEAP_SECONDS; else value &= ~LEAP_SECONDS; } }; uint8_t leap_seconds = 0; ///< [s] @@ -1067,6 +1228,15 @@ struct SbasInfo SbasStatus& operator=(int val) { value = val; return *this; } SbasStatus& operator|=(uint8_t val) { return *this = value | val; } SbasStatus& operator&=(uint8_t val) { return *this = value & val; } + + bool rangeAvailable() const { return (value & RANGE_AVAILABLE) > 0; } + void rangeAvailable(bool val) { if(val) value |= RANGE_AVAILABLE; else value &= ~RANGE_AVAILABLE; } + bool correctionsAvailable() const { return (value & CORRECTIONS_AVAILABLE) > 0; } + void correctionsAvailable(bool val) { if(val) value |= CORRECTIONS_AVAILABLE; else value &= ~CORRECTIONS_AVAILABLE; } + bool integrityAvailable() const { return (value & INTEGRITY_AVAILABLE) > 0; } + void integrityAvailable(bool val) { if(val) value |= INTEGRITY_AVAILABLE; else value &= ~INTEGRITY_AVAILABLE; } + bool testMode() const { return (value & TEST_MODE) > 0; } + void testMode(bool val) { if(val) value |= TEST_MODE; else value &= ~TEST_MODE; } }; struct ValidFlags : Bitfield @@ -1091,6 +1261,21 @@ struct SbasInfo ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + bool sbasSystem() const { return (value & SBAS_SYSTEM) > 0; } + void sbasSystem(bool val) { if(val) value |= SBAS_SYSTEM; else value &= ~SBAS_SYSTEM; } + bool sbasId() const { return (value & SBAS_ID) > 0; } + void sbasId(bool val) { if(val) value |= SBAS_ID; else value &= ~SBAS_ID; } + bool count() const { return (value & COUNT) > 0; } + void count(bool val) { if(val) value |= COUNT; else value &= ~COUNT; } + bool sbasStatus() const { return (value & SBAS_STATUS) > 0; } + void sbasStatus(bool val) { if(val) value |= SBAS_STATUS; else value &= ~SBAS_STATUS; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -1166,6 +1351,15 @@ struct SbasCorrection ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool udrei() const { return (value & UDREI) > 0; } + void udrei(bool val) { if(val) value |= UDREI; else value &= ~UDREI; } + bool pseudorangeCorrection() const { return (value & PSEUDORANGE_CORRECTION) > 0; } + void pseudorangeCorrection(bool val) { if(val) value |= PSEUDORANGE_CORRECTION; else value &= ~PSEUDORANGE_CORRECTION; } + bool ionoCorrection() const { return (value & IONO_CORRECTION) > 0; } + void ionoCorrection(bool val) { if(val) value |= IONO_CORRECTION; else value &= ~IONO_CORRECTION; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1246,6 +1440,15 @@ struct RfErrorDetection ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool rfBand() const { return (value & RF_BAND) > 0; } + void rfBand(bool val) { if(val) value |= RF_BAND; else value &= ~RF_BAND; } + bool jammingState() const { return (value & JAMMING_STATE) > 0; } + void jammingState(bool val) { if(val) value |= JAMMING_STATE; else value &= ~JAMMING_STATE; } + bool spoofingState() const { return (value & SPOOFING_STATE) > 0; } + void spoofingState(bool val) { if(val) value |= SPOOFING_STATE; else value &= ~SPOOFING_STATE; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; RFBand rf_band = static_cast(0); ///< RF Band of the reported information @@ -1304,6 +1507,25 @@ struct BaseStationInfo IndicatorFlags& operator=(int val) { value = val; return *this; } IndicatorFlags& operator|=(uint16_t val) { return *this = value | val; } IndicatorFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool gps() const { return (value & GPS) > 0; } + void gps(bool val) { if(val) value |= GPS; else value &= ~GPS; } + bool glonass() const { return (value & GLONASS) > 0; } + void glonass(bool val) { if(val) value |= GLONASS; else value &= ~GLONASS; } + bool galileo() const { return (value & GALILEO) > 0; } + void galileo(bool val) { if(val) value |= GALILEO; else value &= ~GALILEO; } + bool beidou() const { return (value & BEIDOU) > 0; } + void beidou(bool val) { if(val) value |= BEIDOU; else value &= ~BEIDOU; } + bool refStation() const { return (value & REF_STATION) > 0; } + void refStation(bool val) { if(val) value |= REF_STATION; else value &= ~REF_STATION; } + bool singleReceiver() const { return (value & SINGLE_RECEIVER) > 0; } + void singleReceiver(bool val) { if(val) value |= SINGLE_RECEIVER; else value &= ~SINGLE_RECEIVER; } + bool quarterCycleBit1() const { return (value & QUARTER_CYCLE_BIT1) > 0; } + void quarterCycleBit1(bool val) { if(val) value |= QUARTER_CYCLE_BIT1; else value &= ~QUARTER_CYCLE_BIT1; } + bool quarterCycleBit2() const { return (value & QUARTER_CYCLE_BIT2) > 0; } + void quarterCycleBit2(bool val) { if(val) value |= QUARTER_CYCLE_BIT2; else value &= ~QUARTER_CYCLE_BIT2; } + uint16_t quarterCycleBits() const { return (value & QUARTER_CYCLE_BITS) >> 6; } + void quarterCycleBits(uint16_t val) { value = (value & ~QUARTER_CYCLE_BITS) | (val << 6); } }; struct ValidFlags : Bitfield @@ -1328,6 +1550,21 @@ struct BaseStationInfo ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + bool ecefPosition() const { return (value & ECEF_POSITION) > 0; } + void ecefPosition(bool val) { if(val) value |= ECEF_POSITION; else value &= ~ECEF_POSITION; } + bool height() const { return (value & HEIGHT) > 0; } + void height(bool val) { if(val) value |= HEIGHT; else value &= ~HEIGHT; } + bool stationId() const { return (value & STATION_ID) > 0; } + void stationId(bool val) { if(val) value |= STATION_ID; else value &= ~STATION_ID; } + bool indicators() const { return (value & INDICATORS) > 0; } + void indicators(bool val) { if(val) value |= INDICATORS; else value &= ~INDICATORS; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double time_of_week = 0; ///< GPS Time of week the message was received [seconds] @@ -1385,6 +1622,25 @@ struct RtkCorrectionsStatus ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + bool epochStatus() const { return (value & EPOCH_STATUS) > 0; } + void epochStatus(bool val) { if(val) value |= EPOCH_STATUS; else value &= ~EPOCH_STATUS; } + bool dongleStatus() const { return (value & DONGLE_STATUS) > 0; } + void dongleStatus(bool val) { if(val) value |= DONGLE_STATUS; else value &= ~DONGLE_STATUS; } + bool gpsLatency() const { return (value & GPS_LATENCY) > 0; } + void gpsLatency(bool val) { if(val) value |= GPS_LATENCY; else value &= ~GPS_LATENCY; } + bool glonassLatency() const { return (value & GLONASS_LATENCY) > 0; } + void glonassLatency(bool val) { if(val) value |= GLONASS_LATENCY; else value &= ~GLONASS_LATENCY; } + bool galileoLatency() const { return (value & GALILEO_LATENCY) > 0; } + void galileoLatency(bool val) { if(val) value |= GALILEO_LATENCY; else value &= ~GALILEO_LATENCY; } + bool beidouLatency() const { return (value & BEIDOU_LATENCY) > 0; } + void beidouLatency(bool val) { if(val) value |= BEIDOU_LATENCY; else value &= ~BEIDOU_LATENCY; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; struct EpochStatus : Bitfield @@ -1411,6 +1667,25 @@ struct RtkCorrectionsStatus EpochStatus& operator=(int val) { value = val; return *this; } EpochStatus& operator|=(uint16_t val) { return *this = value | val; } EpochStatus& operator&=(uint16_t val) { return *this = value & val; } + + bool antennaLocationReceived() const { return (value & ANTENNA_LOCATION_RECEIVED) > 0; } + void antennaLocationReceived(bool val) { if(val) value |= ANTENNA_LOCATION_RECEIVED; else value &= ~ANTENNA_LOCATION_RECEIVED; } + bool antennaDescriptionReceived() const { return (value & ANTENNA_DESCRIPTION_RECEIVED) > 0; } + void antennaDescriptionReceived(bool val) { if(val) value |= ANTENNA_DESCRIPTION_RECEIVED; else value &= ~ANTENNA_DESCRIPTION_RECEIVED; } + bool gpsReceived() const { return (value & GPS_RECEIVED) > 0; } + void gpsReceived(bool val) { if(val) value |= GPS_RECEIVED; else value &= ~GPS_RECEIVED; } + bool glonassReceived() const { return (value & GLONASS_RECEIVED) > 0; } + void glonassReceived(bool val) { if(val) value |= GLONASS_RECEIVED; else value &= ~GLONASS_RECEIVED; } + bool galileoReceived() const { return (value & GALILEO_RECEIVED) > 0; } + void galileoReceived(bool val) { if(val) value |= GALILEO_RECEIVED; else value &= ~GALILEO_RECEIVED; } + bool beidouReceived() const { return (value & BEIDOU_RECEIVED) > 0; } + void beidouReceived(bool val) { if(val) value |= BEIDOU_RECEIVED; else value &= ~BEIDOU_RECEIVED; } + bool usingGpsMsmMessages() const { return (value & USING_GPS_MSM_MESSAGES) > 0; } + void usingGpsMsmMessages(bool val) { if(val) value |= USING_GPS_MSM_MESSAGES; else value &= ~USING_GPS_MSM_MESSAGES; } + bool usingGlonassMsmMessages() const { return (value & USING_GLONASS_MSM_MESSAGES) > 0; } + void usingGlonassMsmMessages(bool val) { if(val) value |= USING_GLONASS_MSM_MESSAGES; else value &= ~USING_GLONASS_MSM_MESSAGES; } + bool dongleStatusReadFailed() const { return (value & DONGLE_STATUS_READ_FAILED) > 0; } + void dongleStatusReadFailed(bool val) { if(val) value |= DONGLE_STATUS_READ_FAILED; else value &= ~DONGLE_STATUS_READ_FAILED; } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -1471,6 +1746,23 @@ struct SatelliteStatus ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + bool gnssId() const { return (value & GNSS_ID) > 0; } + void gnssId(bool val) { if(val) value |= GNSS_ID; else value &= ~GNSS_ID; } + bool satelliteId() const { return (value & SATELLITE_ID) > 0; } + void satelliteId(bool val) { if(val) value |= SATELLITE_ID; else value &= ~SATELLITE_ID; } + bool elevation() const { return (value & ELEVATION) > 0; } + void elevation(bool val) { if(val) value |= ELEVATION; else value &= ~ELEVATION; } + bool azimuth() const { return (value & AZIMUTH) > 0; } + void azimuth(bool val) { if(val) value |= AZIMUTH; else value &= ~AZIMUTH; } + bool health() const { return (value & HEALTH) > 0; } + void health(bool val) { if(val) value |= HEALTH; else value &= ~HEALTH; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1550,6 +1842,41 @@ struct Raw ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + bool receiverId() const { return (value & RECEIVER_ID) > 0; } + void receiverId(bool val) { if(val) value |= RECEIVER_ID; else value &= ~RECEIVER_ID; } + bool trackingChannel() const { return (value & TRACKING_CHANNEL) > 0; } + void trackingChannel(bool val) { if(val) value |= TRACKING_CHANNEL; else value &= ~TRACKING_CHANNEL; } + bool gnssId() const { return (value & GNSS_ID) > 0; } + void gnssId(bool val) { if(val) value |= GNSS_ID; else value &= ~GNSS_ID; } + bool satelliteId() const { return (value & SATELLITE_ID) > 0; } + void satelliteId(bool val) { if(val) value |= SATELLITE_ID; else value &= ~SATELLITE_ID; } + bool signalId() const { return (value & SIGNAL_ID) > 0; } + void signalId(bool val) { if(val) value |= SIGNAL_ID; else value &= ~SIGNAL_ID; } + bool signalStrength() const { return (value & SIGNAL_STRENGTH) > 0; } + void signalStrength(bool val) { if(val) value |= SIGNAL_STRENGTH; else value &= ~SIGNAL_STRENGTH; } + bool quality() const { return (value & QUALITY) > 0; } + void quality(bool val) { if(val) value |= QUALITY; else value &= ~QUALITY; } + bool pseudorange() const { return (value & PSEUDORANGE) > 0; } + void pseudorange(bool val) { if(val) value |= PSEUDORANGE; else value &= ~PSEUDORANGE; } + bool carrierPhase() const { return (value & CARRIER_PHASE) > 0; } + void carrierPhase(bool val) { if(val) value |= CARRIER_PHASE; else value &= ~CARRIER_PHASE; } + bool doppler() const { return (value & DOPPLER) > 0; } + void doppler(bool val) { if(val) value |= DOPPLER; else value &= ~DOPPLER; } + bool rangeUncertainty() const { return (value & RANGE_UNCERTAINTY) > 0; } + void rangeUncertainty(bool val) { if(val) value |= RANGE_UNCERTAINTY; else value &= ~RANGE_UNCERTAINTY; } + bool carrierPhaseUncertainty() const { return (value & CARRIER_PHASE_UNCERTAINTY) > 0; } + void carrierPhaseUncertainty(bool val) { if(val) value |= CARRIER_PHASE_UNCERTAINTY; else value &= ~CARRIER_PHASE_UNCERTAINTY; } + bool dopplerUncertainty() const { return (value & DOPPLER_UNCERTAINTY) > 0; } + void dopplerUncertainty(bool val) { if(val) value |= DOPPLER_UNCERTAINTY; else value &= ~DOPPLER_UNCERTAINTY; } + bool lockTime() const { return (value & LOCK_TIME) > 0; } + void lockTime(bool val) { if(val) value |= LOCK_TIME; else value &= ~LOCK_TIME; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1614,6 +1941,13 @@ struct GpsEphemeris ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool ephemeris() const { return (value & EPHEMERIS) > 0; } + void ephemeris(bool val) { if(val) value |= EPHEMERIS; else value &= ~EPHEMERIS; } + bool modernData() const { return (value & MODERN_DATA) > 0; } + void modernData(bool val) { if(val) value |= MODERN_DATA; else value &= ~MODERN_DATA; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1693,6 +2027,13 @@ struct GalileoEphemeris ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool ephemeris() const { return (value & EPHEMERIS) > 0; } + void ephemeris(bool val) { if(val) value |= EPHEMERIS; else value &= ~EPHEMERIS; } + bool modernData() const { return (value & MODERN_DATA) > 0; } + void modernData(bool val) { if(val) value |= MODERN_DATA; else value &= ~MODERN_DATA; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1771,6 +2112,11 @@ struct GloEphemeris ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool ephemeris() const { return (value & EPHEMERIS) > 0; } + void ephemeris(bool val) { if(val) value |= EPHEMERIS; else value &= ~EPHEMERIS; } + bool flags() const { return (value & FLAGS) > 0; } + void flags(bool val) { if(val) value |= FLAGS; else value &= ~FLAGS; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1843,6 +2189,17 @@ struct GpsIonoCorr ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + bool alpha() const { return (value & ALPHA) > 0; } + void alpha(bool val) { if(val) value |= ALPHA; else value &= ~ALPHA; } + bool beta() const { return (value & BETA) > 0; } + void beta(bool val) { if(val) value |= BETA; else value &= ~BETA; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -1895,6 +2252,17 @@ struct GalileoIonoCorr ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + bool alpha() const { return (value & ALPHA) > 0; } + void alpha(bool val) { if(val) value |= ALPHA; else value &= ~ALPHA; } + bool disturbanceFlags() const { return (value & DISTURBANCE_FLAGS) > 0; } + void disturbanceFlags(bool val) { if(val) value |= DISTURBANCE_FLAGS; else value &= ~DISTURBANCE_FLAGS; } + uint16_t flags() const { return (value & FLAGS) >> 0; } + void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } }; double time_of_week = 0; ///< GPS Time of week [seconds] diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 9ef2aed3a..0621df768 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -586,6 +586,17 @@ struct GpsTimestamp ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool ppsValid() const { return (value & PPS_VALID) > 0; } + void ppsValid(bool val) { if(val) value |= PPS_VALID; else value &= ~PPS_VALID; } + bool timeRefresh() const { return (value & TIME_REFRESH) > 0; } + void timeRefresh(bool val) { if(val) value |= TIME_REFRESH; else value &= ~TIME_REFRESH; } + bool timeInitialized() const { return (value & TIME_INITIALIZED) > 0; } + void timeInitialized(bool val) { if(val) value |= TIME_INITIALIZED; else value &= ~TIME_INITIALIZED; } + bool towValid() const { return (value & TOW_VALID) > 0; } + void towValid(bool val) { if(val) value |= TOW_VALID; else value &= ~TOW_VALID; } + bool weekNumberValid() const { return (value & WEEK_NUMBER_VALID) > 0; } + void weekNumberValid(bool val) { if(val) value |= WEEK_NUMBER_VALID; else value &= ~WEEK_NUMBER_VALID; } }; double tow = 0; ///< GPS Time of Week [seconds] @@ -733,6 +744,27 @@ struct OverrangeStatus Status& operator=(int val) { value = val; return *this; } Status& operator|=(uint16_t val) { return *this = value | val; } Status& operator&=(uint16_t val) { return *this = value & val; } + + bool accelX() const { return (value & ACCEL_X) > 0; } + void accelX(bool val) { if(val) value |= ACCEL_X; else value &= ~ACCEL_X; } + bool accelY() const { return (value & ACCEL_Y) > 0; } + void accelY(bool val) { if(val) value |= ACCEL_Y; else value &= ~ACCEL_Y; } + bool accelZ() const { return (value & ACCEL_Z) > 0; } + void accelZ(bool val) { if(val) value |= ACCEL_Z; else value &= ~ACCEL_Z; } + bool gyroX() const { return (value & GYRO_X) > 0; } + void gyroX(bool val) { if(val) value |= GYRO_X; else value &= ~GYRO_X; } + bool gyroY() const { return (value & GYRO_Y) > 0; } + void gyroY(bool val) { if(val) value |= GYRO_Y; else value &= ~GYRO_Y; } + bool gyroZ() const { return (value & GYRO_Z) > 0; } + void gyroZ(bool val) { if(val) value |= GYRO_Z; else value &= ~GYRO_Z; } + bool magX() const { return (value & MAG_X) > 0; } + void magX(bool val) { if(val) value |= MAG_X; else value &= ~MAG_X; } + bool magY() const { return (value & MAG_Y) > 0; } + void magY(bool val) { if(val) value |= MAG_Y; else value &= ~MAG_Y; } + bool magZ() const { return (value & MAG_Z) > 0; } + void magZ(bool val) { if(val) value |= MAG_Z; else value &= ~MAG_Z; } + bool press() const { return (value & PRESS) > 0; } + void press(bool val) { if(val) value |= PRESS; else value &= ~PRESS; } }; Status status; diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index c210f2790..7963b132f 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -181,6 +181,13 @@ struct GpsTimestamp ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool tow() const { return (value & TOW) > 0; } + void tow(bool val) { if(val) value |= TOW; else value &= ~TOW; } + bool weekNumber() const { return (value & WEEK_NUMBER) > 0; } + void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } + uint16_t timeValid() const { return (value & TIME_VALID) >> 0; } + void timeValid(uint16_t val) { value = (value & ~TIME_VALID) | (val << 0); } }; double tow = 0; ///< GPS Time of Week [seconds] @@ -338,6 +345,9 @@ struct ExternalTimestamp ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool nanoseconds() const { return (value & NANOSECONDS) > 0; } + void nanoseconds(bool val) { if(val) value |= NANOSECONDS; else value &= ~NANOSECONDS; } }; uint64_t nanoseconds = 0; @@ -395,6 +405,9 @@ struct ExternalTimeDelta ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool dtNanos() const { return (value & DT_NANOS) > 0; } + void dtNanos(bool val) { if(val) value |= DT_NANOS; else value &= ~DT_NANOS; } }; uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. diff --git a/src/mip/definitions/descriptors.c b/src/mip/definitions/descriptors.c index 8b5c134e6..9d11708e4 100644 --- a/src/mip/definitions/descriptors.c +++ b/src/mip/definitions/descriptors.c @@ -1,171 +1,171 @@ - -#include "descriptors.h" - -#include "../utils/serialization.h" - -#ifdef __cplusplus -namespace mip { -extern "C" { -#endif // __cplusplus - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the descriptor set is valid. -/// -///@param descriptor_set -/// -///@returns true if the descriptor set is valid. -/// -bool mip_is_valid_descriptor_set(uint8_t descriptor_set) -{ - return descriptor_set != MIP_INVALID_DESCRIPTOR_SET; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the descriptor set represents some kind of data. -/// -///@param descriptor_set -/// -///@returns true if the descriptor set represents data. -/// -bool mip_is_data_descriptor_set(uint8_t descriptor_set) -{ - return (descriptor_set >= MIP_DATA_DESCRIPTOR_SET_START) && (descriptor_set < MIP_RESERVED_DESCRIPTOR_SET_START); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the descriptor set contains commands. -/// -///@param descriptor_set -/// -///@returns true if the descriptor set contains commands. -/// -bool mip_is_cmd_descriptor_set(uint8_t descriptor_set) -{ - return (descriptor_set < MIP_DATA_DESCRIPTOR_SET_START); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the descriptor is reserved for special purposes. -/// -///@param descriptor_set -/// -///@returns true if the descriptor set is reserved. -/// -bool mip_is_reserved_descriptor_set(uint8_t descriptor_set) -{ - return (descriptor_set >= MIP_RESERVED_DESCRIPTOR_SET_START); -} - - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the field descriptor is valid. -/// -///@param field_descriptor -/// -///@returns true if the field descriptor is valid. -/// -bool mip_is_valid_field_descriptor(uint8_t field_descriptor) -{ - return field_descriptor != MIP_INVALID_FIELD_DESCRIPTOR; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the field descriptor is a command. -/// -///@param field_descriptor -/// -///@returns true if the field descriptor represents a command. -/// -bool mip_is_cmd_field_descriptor(uint8_t field_descriptor) -{ - return (field_descriptor < MIP_RESPONSE_DESCRIPTOR_START); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the field descriptor is for an ack/nack reply. -/// -///@param field_descriptor -/// -///@returns true if the field descriptor represents an ack/nack reply code. -/// -bool mip_is_reply_field_descriptor(uint8_t field_descriptor) -{ - return (field_descriptor == MIP_REPLY_DESCRIPTOR); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the field descriptor contains response data from a -/// command. -/// -/// The descriptor set is assumed to be a command set. -/// -///@param field_descriptor -/// -///@returns true if the associated field contains response data. -/// -bool mip_is_response_field_descriptor(uint8_t field_descriptor) -{ - return field_descriptor >= MIP_RESPONSE_DESCRIPTOR_START && !mip_is_reserved_cmd_field_descriptor(field_descriptor); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the field descriptor is reserved. -/// -/// The descriptor set is assumed to be a command set. -/// -///@param field_descriptor -/// -///@returns true if the associated field is neither a command nor response. -/// -bool mip_is_reserved_cmd_field_descriptor(uint8_t field_descriptor) -{ - return ((field_descriptor|MIP_RESPONSE_DESCRIPTOR_START) >= MIP_RESERVED_DESCRIPTOR_START); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the field descriptor is from the shared data set. -/// -/// The descriptor set is assumed to be a data set. -/// -///@param field_descriptor -/// -///@returns true if the associated field is from the shared data set. -/// -bool mip_is_shared_data_field_descriptor(uint8_t field_descriptor) -{ - return field_descriptor >= MIP_SHARED_DATA_FIELD_DESCRIPTOR_START; -} - - -void insert_mip_function_selector(mip_serializer* serializer, enum mip_function_selector self) -{ - insert_u8(serializer, self); -} - -void extract_mip_function_selector(mip_serializer* serializer, enum mip_function_selector* self) -{ - uint8_t tmp; - extract_u8(serializer, &tmp); - *self = (enum mip_function_selector)tmp; -} - - -void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self) -{ - insert_u8(serializer, self->descriptor); - insert_u16(serializer, self->decimation); -} - -void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self) -{ - extract_u8(serializer, &self->descriptor); - extract_u16(serializer, &self->decimation); -} - - -#ifdef __cplusplus -} // namespace mip -} // extern "C" -#endif // __cplusplus + +#include "descriptors.h" + +#include "../utils/serialization.h" + +#ifdef __cplusplus +namespace mip { +extern "C" { +#endif // __cplusplus + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the descriptor set is valid. +/// +///@param descriptor_set +/// +///@returns true if the descriptor set is valid. +/// +bool mip_is_valid_descriptor_set(uint8_t descriptor_set) +{ + return descriptor_set != MIP_INVALID_DESCRIPTOR_SET; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the descriptor set represents some kind of data. +/// +///@param descriptor_set +/// +///@returns true if the descriptor set represents data. +/// +bool mip_is_data_descriptor_set(uint8_t descriptor_set) +{ + return (descriptor_set >= MIP_DATA_DESCRIPTOR_SET_START) && (descriptor_set < MIP_RESERVED_DESCRIPTOR_SET_START); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the descriptor set contains commands. +/// +///@param descriptor_set +/// +///@returns true if the descriptor set contains commands. +/// +bool mip_is_cmd_descriptor_set(uint8_t descriptor_set) +{ + return (descriptor_set < MIP_DATA_DESCRIPTOR_SET_START); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the descriptor is reserved for special purposes. +/// +///@param descriptor_set +/// +///@returns true if the descriptor set is reserved. +/// +bool mip_is_reserved_descriptor_set(uint8_t descriptor_set) +{ + return (descriptor_set >= MIP_RESERVED_DESCRIPTOR_SET_START); +} + + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the field descriptor is valid. +/// +///@param field_descriptor +/// +///@returns true if the field descriptor is valid. +/// +bool mip_is_valid_field_descriptor(uint8_t field_descriptor) +{ + return field_descriptor != MIP_INVALID_FIELD_DESCRIPTOR; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the field descriptor is a command. +/// +///@param field_descriptor +/// +///@returns true if the field descriptor represents a command. +/// +bool mip_is_cmd_field_descriptor(uint8_t field_descriptor) +{ + return (field_descriptor < MIP_RESPONSE_DESCRIPTOR_START); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the field descriptor is for an ack/nack reply. +/// +///@param field_descriptor +/// +///@returns true if the field descriptor represents an ack/nack reply code. +/// +bool mip_is_reply_field_descriptor(uint8_t field_descriptor) +{ + return (field_descriptor == MIP_REPLY_DESCRIPTOR); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the field descriptor contains response data from a +/// command. +/// +/// The descriptor set is assumed to be a command set. +/// +///@param field_descriptor +/// +///@returns true if the associated field contains response data. +/// +bool mip_is_response_field_descriptor(uint8_t field_descriptor) +{ + return field_descriptor >= MIP_RESPONSE_DESCRIPTOR_START && !mip_is_reserved_cmd_field_descriptor(field_descriptor); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the field descriptor is reserved. +/// +/// The descriptor set is assumed to be a command set. +/// +///@param field_descriptor +/// +///@returns true if the associated field is neither a command nor response. +/// +bool mip_is_reserved_cmd_field_descriptor(uint8_t field_descriptor) +{ + return ((field_descriptor|MIP_RESPONSE_DESCRIPTOR_START) >= MIP_RESERVED_DESCRIPTOR_START); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the field descriptor is from the shared data set. +/// +/// The descriptor set is assumed to be a data set. +/// +///@param field_descriptor +/// +///@returns true if the associated field is from the shared data set. +/// +bool mip_is_shared_data_field_descriptor(uint8_t field_descriptor) +{ + return field_descriptor >= MIP_SHARED_DATA_FIELD_DESCRIPTOR_START; +} + + +void insert_mip_function_selector(mip_serializer* serializer, enum mip_function_selector self) +{ + insert_u8(serializer, self); +} + +void extract_mip_function_selector(mip_serializer* serializer, enum mip_function_selector* self) +{ + uint8_t tmp; + extract_u8(serializer, &tmp); + *self = (enum mip_function_selector)tmp; +} + + +void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self) +{ + insert_u8(serializer, self->descriptor); + insert_u16(serializer, self->decimation); +} + +void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self) +{ + extract_u8(serializer, &self->descriptor); + extract_u16(serializer, &self->decimation); +} + + +#ifdef __cplusplus +} // namespace mip +} // extern "C" +#endif // __cplusplus From 85649026a93b9f0d3a71bcbe0391823c154b3fb5 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Wed, 2 Nov 2022 11:58:09 -0400 Subject: [PATCH 015/252] Generate MIP definitions from branches/dev@53813. (#37) Co-authored-by: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> --- src/mip/definitions/commands_3dm.c | 108 ++ src/mip/definitions/commands_3dm.cpp | 97 ++ src/mip/definitions/commands_3dm.h | 32 + src/mip/definitions/commands_3dm.hpp | 42 + src/mip/definitions/commands_filter.c | 1725 +++++++++++++++++++++-- src/mip/definitions/commands_filter.cpp | 1625 +++++++++++++++++++-- src/mip/definitions/commands_filter.h | 470 ++++++ src/mip/definitions/commands_filter.hpp | 568 ++++++++ 8 files changed, 4483 insertions(+), 184 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index c48418d79..caef68235 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -3933,6 +3933,114 @@ mip_cmd_result mip_3dm_default_mag_soft_iron_matrix(struct mip_interface* device return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_3dm_coning_sculling_enable_command(mip_serializer* serializer, const mip_3dm_coning_sculling_enable_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_bool(serializer, self->enable); + + } +} +void extract_mip_3dm_coning_sculling_enable_command(mip_serializer* serializer, mip_3dm_coning_sculling_enable_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_bool(serializer, &self->enable); + + } +} + +void insert_mip_3dm_coning_sculling_enable_response(mip_serializer* serializer, const mip_3dm_coning_sculling_enable_response* self) +{ + insert_bool(serializer, self->enable); + +} +void extract_mip_3dm_coning_sculling_enable_response(mip_serializer* serializer, mip_3dm_coning_sculling_enable_response* self) +{ + extract_bool(serializer, &self->enable); + +} + +mip_cmd_result mip_3dm_write_coning_sculling_enable(struct mip_interface* device, bool enable) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_bool(&serializer, enable); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_read_coning_sculling_enable(struct mip_interface* device, bool* enable_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_bool(&deserializer, enable_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_3dm_save_coning_sculling_enable(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_load_coning_sculling_enable(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_default_coning_sculling_enable(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_3dm_sensor_2_vehicle_transform_euler_command(mip_serializer* serializer, const mip_3dm_sensor_2_vehicle_transform_euler_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index 423cb121f..fd5471d53 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -3472,6 +3472,103 @@ CmdResult defaultMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const ConingScullingEnable& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + } +} +void extract(Serializer& serializer, ConingScullingEnable& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + } +} + +void insert(Serializer& serializer, const ConingScullingEnable::Response& self) +{ + insert(serializer, self.enable); + +} +void extract(Serializer& serializer, ConingScullingEnable::Response& self) +{ + extract(serializer, self.enable); + +} + +CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveConingScullingEnable(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadConingScullingEnable(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultConingScullingEnable(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const Sensor2VehicleTransformEuler& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index b4da6c501..3d96ab5ed 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -1699,6 +1699,38 @@ mip_cmd_result mip_3dm_default_mag_soft_iron_matrix(struct mip_interface* device ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_3dm_coning_sculling_enable (0x0C,0x3E) Coning Sculling Enable [C] +/// Controls the Coning and Sculling Compenstation setting. +/// +///@{ + +struct mip_3dm_coning_sculling_enable_command +{ + mip_function_selector function; + bool enable; ///< If true, coning and sculling compensation is enabled. + +}; +typedef struct mip_3dm_coning_sculling_enable_command mip_3dm_coning_sculling_enable_command; +void insert_mip_3dm_coning_sculling_enable_command(struct mip_serializer* serializer, const mip_3dm_coning_sculling_enable_command* self); +void extract_mip_3dm_coning_sculling_enable_command(struct mip_serializer* serializer, mip_3dm_coning_sculling_enable_command* self); + +struct mip_3dm_coning_sculling_enable_response +{ + bool enable; ///< If true, coning and sculling compensation is enabled. + +}; +typedef struct mip_3dm_coning_sculling_enable_response mip_3dm_coning_sculling_enable_response; +void insert_mip_3dm_coning_sculling_enable_response(struct mip_serializer* serializer, const mip_3dm_coning_sculling_enable_response* self); +void extract_mip_3dm_coning_sculling_enable_response(struct mip_serializer* serializer, mip_3dm_coning_sculling_enable_response* self); + +mip_cmd_result mip_3dm_write_coning_sculling_enable(struct mip_interface* device, bool enable); +mip_cmd_result mip_3dm_read_coning_sculling_enable(struct mip_interface* device, bool* enable_out); +mip_cmd_result mip_3dm_save_coning_sculling_enable(struct mip_interface* device); +mip_cmd_result mip_3dm_load_coning_sculling_enable(struct mip_interface* device); +mip_cmd_result mip_3dm_default_coning_sculling_enable(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_3dm_sensor_2_vehicle_transform_euler (0x0C,0x31) Sensor 2 Vehicle Transform Euler [C] /// Sets the sensor-to-vehicle frame transformation using Yaw, Pitch, Roll Euler angles. /// These are the Yaw, Pitch, and Roll mounting angles of the sensor with respect to vehicle frame of reference, diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 621dd12dd..989d08120 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -2042,6 +2042,48 @@ CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_3dm_coning_sculling_enable (0x0C,0x3E) Coning Sculling Enable [CPP] +/// Controls the Coning and Sculling Compenstation setting. +/// +///@{ + +struct ConingScullingEnable +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + bool enable = 0; ///< If true, coning and sculling compensation is enabled. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; + + bool enable = 0; ///< If true, coning and sculling compensation is enabled. + + }; +}; +void insert(Serializer& serializer, const ConingScullingEnable& self); +void extract(Serializer& serializer, ConingScullingEnable& self); + +void insert(Serializer& serializer, const ConingScullingEnable::Response& self); +void extract(Serializer& serializer, ConingScullingEnable::Response& self); + +CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable); +CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); +CmdResult saveConingScullingEnable(C::mip_interface& device); +CmdResult loadConingScullingEnable(C::mip_interface& device); +CmdResult defaultConingScullingEnable(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_3dm_sensor_2_vehicle_transform_euler (0x0C,0x31) Sensor 2 Vehicle Transform Euler [CPP] /// Sets the sensor-to-vehicle frame transformation using Yaw, Pitch, Roll Euler angles. /// These are the Yaw, Pitch, and Roll mounting angles of the sensor with respect to vehicle frame of reference, diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index 4edc45da0..ccbca3b69 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -44,6 +44,17 @@ void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer *self = tmp; } +void insert_mip_filter_adaptive_measurement(struct mip_serializer* serializer, const mip_filter_adaptive_measurement self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_adaptive_measurement(struct mip_serializer* serializer, mip_filter_adaptive_measurement* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -485,6 +496,125 @@ mip_cmd_result mip_filter_default_tare_orientation(struct mip_interface* device) return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_filter_vehicle_dynamics_mode_command(mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, self->mode); + + } +} +void extract_mip_filter_vehicle_dynamics_mode_command(mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, &self->mode); + + } +} + +void insert_mip_filter_vehicle_dynamics_mode_response(mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_response* self) +{ + insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, self->mode); + +} +void extract_mip_filter_vehicle_dynamics_mode_response(mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_response* self) +{ + extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(serializer, &self->mode); + +} + +void insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command_dynamics_mode self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_filter_write_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(&serializer, mode); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* mode_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(mode_out); + extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(&deserializer, mode_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_vehicle_dynamics_mode(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_vehicle_dynamics_mode(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_vehicle_dynamics_mode(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_filter_sensor_to_vehicle_rotation_euler_command(mip_serializer* serializer, const mip_filter_sensor_to_vehicle_rotation_euler_command* self) { insert_mip_function_selector(serializer, self->function); @@ -1425,39 +1555,1206 @@ mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_altitude_aiding_command(mip_serializer* serializer, const mip_filter_altitude_aiding_command* self) +void insert_mip_filter_accel_noise_command(mip_serializer* serializer, const mip_filter_accel_noise_command* self) { insert_mip_function_selector(serializer, self->function); if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->aiding_selector); + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); } } -void extract_mip_filter_altitude_aiding_command(mip_serializer* serializer, mip_filter_altitude_aiding_command* self) +void extract_mip_filter_accel_noise_command(mip_serializer* serializer, mip_filter_accel_noise_command* self) { extract_mip_function_selector(serializer, &self->function); if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->aiding_selector); + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); } } -void insert_mip_filter_altitude_aiding_response(mip_serializer* serializer, const mip_filter_altitude_aiding_response* self) +void insert_mip_filter_accel_noise_response(mip_serializer* serializer, const mip_filter_accel_noise_response* self) +{ + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_accel_noise_response(mip_serializer* serializer, mip_filter_accel_noise_response* self) +{ + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ACCEL_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_gyro_noise_command(mip_serializer* serializer, const mip_filter_gyro_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_gyro_noise_command(mip_serializer* serializer, mip_filter_gyro_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_gyro_noise_response(mip_serializer* serializer, const mip_filter_gyro_noise_response* self) +{ + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_gyro_noise_response(mip_serializer* serializer, mip_filter_gyro_noise_response* self) +{ + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_GYRO_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_accel_bias_model_command(mip_serializer* serializer, const mip_filter_accel_bias_model_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_accel_bias_model_command(mip_serializer* serializer, mip_filter_accel_bias_model_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_accel_bias_model_response(mip_serializer* serializer, const mip_filter_accel_bias_model_response* self) +{ + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_accel_bias_model_response(mip_serializer* serializer, mip_filter_accel_bias_model_response* self) +{ + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x_beta); + + insert_float(&serializer, y_beta); + + insert_float(&serializer, z_beta); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_beta_out); + extract_float(&deserializer, x_beta_out); + + assert(y_beta_out); + extract_float(&deserializer, y_beta_out); + + assert(z_beta_out); + extract_float(&deserializer, z_beta_out); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_gyro_bias_model_command(mip_serializer* serializer, const mip_filter_gyro_bias_model_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_gyro_bias_model_command(mip_serializer* serializer, mip_filter_gyro_bias_model_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_gyro_bias_model_response(mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self) +{ + insert_float(serializer, self->x_beta); + + insert_float(serializer, self->y_beta); + + insert_float(serializer, self->z_beta); + + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_gyro_bias_model_response(mip_serializer* serializer, mip_filter_gyro_bias_model_response* self) +{ + extract_float(serializer, &self->x_beta); + + extract_float(serializer, &self->y_beta); + + extract_float(serializer, &self->z_beta); + + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x_beta); + + insert_float(&serializer, y_beta); + + insert_float(&serializer, z_beta); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_GYRO_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_beta_out); + extract_float(&deserializer, x_beta_out); + + assert(y_beta_out); + extract_float(&deserializer, y_beta_out); + + assert(z_beta_out); + extract_float(&deserializer, z_beta_out); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_altitude_aiding_command(mip_serializer* serializer, const mip_filter_altitude_aiding_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->aiding_selector); + + } +} +void extract_mip_filter_altitude_aiding_command(mip_serializer* serializer, mip_filter_altitude_aiding_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->aiding_selector); + + } +} + +void insert_mip_filter_altitude_aiding_response(mip_serializer* serializer, const mip_filter_altitude_aiding_response* self) +{ + insert_u8(serializer, self->aiding_selector); + +} +void extract_mip_filter_altitude_aiding_response(mip_serializer* serializer, mip_filter_altitude_aiding_response* self) +{ + extract_u8(serializer, &self->aiding_selector); + +} + +mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, aiding_selector); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(aiding_selector_out); + extract_u8(&deserializer, aiding_selector_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_auto_zupt_command(mip_serializer* serializer, const mip_filter_auto_zupt_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + + } +} +void extract_mip_filter_auto_zupt_command(mip_serializer* serializer, mip_filter_auto_zupt_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + + } +} + +void insert_mip_filter_auto_zupt_response(mip_serializer* serializer, const mip_filter_auto_zupt_response* self) +{ + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + +} +void extract_mip_filter_auto_zupt_response(mip_serializer* serializer, mip_filter_auto_zupt_response* self) +{ + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + +} + +mip_cmd_result mip_filter_write_auto_zupt(struct mip_interface* device, uint8_t enable, float threshold) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, enable); + + insert_float(&serializer, threshold); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_auto_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_u8(&deserializer, enable_out); + + assert(threshold_out); + extract_float(&deserializer, threshold_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_auto_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_auto_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_auto_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, const mip_filter_auto_angular_zupt_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + + } +} +void extract_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, mip_filter_auto_angular_zupt_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + + } +} + +void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +{ + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + +} +void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +{ + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + +} + +mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, enable); + + insert_float(&serializer, threshold); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_u8(&deserializer, enable_out); + + assert(threshold_out); + extract_float(&deserializer, threshold_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); +} +mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert_mip_filter_reference_position_command(mip_serializer* serializer, const mip_filter_reference_position_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_bool(serializer, self->enable); + + insert_double(serializer, self->latitude); + + insert_double(serializer, self->longitude); + + insert_double(serializer, self->altitude); + + } +} +void extract_mip_filter_reference_position_command(mip_serializer* serializer, mip_filter_reference_position_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_bool(serializer, &self->enable); + + extract_double(serializer, &self->latitude); + + extract_double(serializer, &self->longitude); + + extract_double(serializer, &self->altitude); + + } +} + +void insert_mip_filter_reference_position_response(mip_serializer* serializer, const mip_filter_reference_position_response* self) +{ + insert_bool(serializer, self->enable); + + insert_double(serializer, self->latitude); + + insert_double(serializer, self->longitude); + + insert_double(serializer, self->altitude); + +} +void extract_mip_filter_reference_position_response(mip_serializer* serializer, mip_filter_reference_position_response* self) +{ + extract_bool(serializer, &self->enable); + + extract_double(serializer, &self->latitude); + + extract_double(serializer, &self->longitude); + + extract_double(serializer, &self->altitude); + +} + +mip_cmd_result mip_filter_write_reference_position(struct mip_interface* device, bool enable, double latitude, double longitude, double altitude) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_bool(&serializer, enable); + + insert_double(&serializer, latitude); + + insert_double(&serializer, longitude); + + insert_double(&serializer, altitude); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_reference_position(struct mip_interface* device, bool* enable_out, double* latitude_out, double* longitude_out, double* altitude_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_REFERENCE_POSITION, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_bool(&deserializer, enable_out); + + assert(latitude_out); + extract_double(&deserializer, latitude_out); + + assert(longitude_out); + extract_double(&deserializer, longitude_out); + + assert(altitude_out); + extract_double(&deserializer, altitude_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_reference_position(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_reference_position(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_reference_position(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); + + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); + + } +} +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); + + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); + + } +} + +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_response* self) { - insert_u8(serializer, self->aiding_selector); + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); + + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } -void extract_mip_filter_altitude_aiding_response(mip_serializer* serializer, mip_filter_altitude_aiding_response* self) +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_response* self) { - extract_u8(serializer, &self->aiding_selector); + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); + + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } -mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector) +mip_cmd_result mip_filter_write_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1465,13 +2762,25 @@ mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, ui insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, aiding_selector); + insert_mip_filter_adaptive_measurement(&serializer, adaptive_measurement); + + insert_float(&serializer, frequency); + + insert_float(&serializer, low_limit); + + insert_float(&serializer, high_limit); + + insert_float(&serializer, low_limit_uncertainty); + + insert_float(&serializer, high_limit_uncertainty); + + insert_float(&serializer, minimum_uncertainty); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out) +mip_cmd_result mip_filter_read_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1482,22 +2791,40 @@ mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uin assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(aiding_selector_out); - extract_u8(&deserializer, aiding_selector_out); + assert(adaptive_measurement_out); + extract_mip_filter_adaptive_measurement(&deserializer, adaptive_measurement_out); + + assert(frequency_out); + extract_float(&deserializer, frequency_out); + + assert(low_limit_out); + extract_float(&deserializer, low_limit_out); + + assert(high_limit_out); + extract_float(&deserializer, high_limit_out); + + assert(low_limit_uncertainty_out); + extract_float(&deserializer, low_limit_uncertainty_out); + + assert(high_limit_uncertainty_out); + extract_float(&deserializer, high_limit_uncertainty_out); + + assert(minimum_uncertainty_out); + extract_float(&deserializer, minimum_uncertainty_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device) +mip_cmd_result mip_filter_save_accel_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1507,9 +2834,9 @@ mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device) +mip_cmd_result mip_filter_load_accel_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1519,9 +2846,9 @@ mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) +mip_cmd_result mip_filter_default_accel_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1531,49 +2858,89 @@ mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_auto_zupt_command(mip_serializer* serializer, const mip_filter_auto_zupt_command* self) +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_command* self) { insert_mip_function_selector(serializer, self->function); if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->enable); + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } } -void extract_mip_filter_auto_zupt_command(mip_serializer* serializer, mip_filter_auto_zupt_command* self) +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_command(mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_command* self) { extract_mip_function_selector(serializer, &self->function); if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->enable); + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } } -void insert_mip_filter_auto_zupt_response(mip_serializer* serializer, const mip_filter_auto_zupt_response* self) +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_response* self) { - insert_u8(serializer, self->enable); + insert_mip_filter_adaptive_measurement(serializer, self->adaptive_measurement); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->low_limit); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->low_limit_uncertainty); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } -void extract_mip_filter_auto_zupt_response(mip_serializer* serializer, mip_filter_auto_zupt_response* self) +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_response(mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_response* self) { - extract_u8(serializer, &self->enable); + extract_mip_filter_adaptive_measurement(serializer, &self->adaptive_measurement); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->low_limit); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->low_limit_uncertainty); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } -mip_cmd_result mip_filter_write_auto_zupt(struct mip_interface* device, uint8_t enable, float threshold) +mip_cmd_result mip_filter_write_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1581,15 +2948,25 @@ mip_cmd_result mip_filter_write_auto_zupt(struct mip_interface* device, uint8_t insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, enable); + insert_mip_filter_adaptive_measurement(&serializer, adaptive_measurement); - insert_float(&serializer, threshold); + insert_float(&serializer, frequency); + + insert_float(&serializer, low_limit); + + insert_float(&serializer, high_limit); + + insert_float(&serializer, low_limit_uncertainty); + + insert_float(&serializer, high_limit_uncertainty); + + insert_float(&serializer, minimum_uncertainty); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_auto_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +mip_cmd_result mip_filter_read_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1600,25 +2977,40 @@ mip_cmd_result mip_filter_read_auto_zupt(struct mip_interface* device, uint8_t* assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ZUPT_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(enable_out); - extract_u8(&deserializer, enable_out); + assert(adaptive_measurement_out); + extract_mip_filter_adaptive_measurement(&deserializer, adaptive_measurement_out); - assert(threshold_out); - extract_float(&deserializer, threshold_out); + assert(frequency_out); + extract_float(&deserializer, frequency_out); + + assert(low_limit_out); + extract_float(&deserializer, low_limit_out); + + assert(high_limit_out); + extract_float(&deserializer, high_limit_out); + + assert(low_limit_uncertainty_out); + extract_float(&deserializer, low_limit_uncertainty_out); + + assert(high_limit_uncertainty_out); + extract_float(&deserializer, high_limit_uncertainty_out); + + assert(minimum_uncertainty_out); + extract_float(&deserializer, minimum_uncertainty_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_auto_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_save_mag_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1628,9 +3020,9 @@ mip_cmd_result mip_filter_save_auto_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_auto_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_load_mag_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1640,9 +3032,9 @@ mip_cmd_result mip_filter_load_auto_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_auto_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_default_mag_magnitude_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1652,49 +3044,73 @@ mip_cmd_result mip_filter_default_auto_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, const mip_filter_auto_angular_zupt_command* self) +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_command(mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_command* self) { insert_mip_function_selector(serializer, self->function); if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->enable); + insert_bool(serializer, self->enable); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } } -void extract_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, mip_filter_auto_angular_zupt_command* self) +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_command(mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_command* self) { extract_mip_function_selector(serializer, &self->function); if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->enable); + extract_bool(serializer, &self->enable); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } } -void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_response(mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_response* self) { - insert_u8(serializer, self->enable); + insert_bool(serializer, self->enable); - insert_float(serializer, self->threshold); + insert_float(serializer, self->frequency); + + insert_float(serializer, self->high_limit); + + insert_float(serializer, self->high_limit_uncertainty); + + insert_float(serializer, self->minimum_uncertainty); } -void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_response(mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_response* self) { - extract_u8(serializer, &self->enable); + extract_bool(serializer, &self->enable); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->frequency); + + extract_float(serializer, &self->high_limit); + + extract_float(serializer, &self->high_limit_uncertainty); + + extract_float(serializer, &self->minimum_uncertainty); } -mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +mip_cmd_result mip_filter_write_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool enable, float frequency, float high_limit, float high_limit_uncertainty, float minimum_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1702,15 +3118,21 @@ mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, enable); + insert_bool(&serializer, enable); - insert_float(&serializer, threshold); + insert_float(&serializer, frequency); + + insert_float(&serializer, high_limit); + + insert_float(&serializer, high_limit_uncertainty); + + insert_float(&serializer, minimum_uncertainty); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +mip_cmd_result mip_filter_read_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool* enable_out, float* frequency_out, float* high_limit_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1721,7 +3143,7 @@ mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, u assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1729,17 +3151,26 @@ mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, u mip_serializer_init_insertion(&deserializer, buffer, responseLength); assert(enable_out); - extract_u8(&deserializer, enable_out); + extract_bool(&deserializer, enable_out); - assert(threshold_out); - extract_float(&deserializer, threshold_out); + assert(frequency_out); + extract_float(&deserializer, frequency_out); + + assert(high_limit_out); + extract_float(&deserializer, high_limit_out); + + assert(high_limit_uncertainty_out); + extract_float(&deserializer, high_limit_uncertainty_out); + + assert(minimum_uncertainty_out); + extract_float(&deserializer, minimum_uncertainty_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_save_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1749,9 +3180,9 @@ mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_load_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1761,9 +3192,9 @@ mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_default_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1773,15 +3204,7 @@ mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); -} -mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert_mip_filter_aiding_measurement_enable_command(mip_serializer* serializer, const mip_filter_aiding_measurement_enable_command* self) { @@ -3357,6 +4780,140 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_GNSS_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + + } +} +void extract_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + + } +} + +void insert_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self) +{ + insert_float(serializer, self->x); + + insert_float(serializer, self->y); + + insert_float(serializer, self->z); + +} +void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self) +{ + extract_float(serializer, &self->x); + + extract_float(serializer, &self->y); + + extract_float(serializer, &self->z); + +} + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, x); + + insert_float(&serializer, y); + + insert_float(&serializer, z); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(x_out); + extract_float(&deserializer, x_out); + + assert(y_out); + extract_float(&deserializer, y_out); + + assert(z_out); + extract_float(&deserializer, z_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 12e176284..7048f63d0 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -428,6 +428,103 @@ CmdResult defaultTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const VehicleDynamicsMode& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.mode); + + } +} +void extract(Serializer& serializer, VehicleDynamicsMode& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.mode); + + } +} + +void insert(Serializer& serializer, const VehicleDynamicsMode::Response& self) +{ + insert(serializer, self.mode); + +} +void extract(Serializer& serializer, VehicleDynamicsMode::Response& self) +{ + extract(serializer, self.mode); + +} + +CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, mode); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(modeOut); + extract(deserializer, *modeOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveVehicleDynamicsMode(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadVehicleDynamicsMode(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultVehicleDynamicsMode(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const SensorToVehicleRotationEuler& self) { insert(serializer, self.function); @@ -1258,51 +1355,1164 @@ CmdResult defaultAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const AltitudeAiding& self) +void insert(Serializer& serializer, const AccelNoise& self) { insert(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.aiding_selector); + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); } } -void extract(Serializer& serializer, AltitudeAiding& self) +void extract(Serializer& serializer, AccelNoise& self) { extract(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.aiding_selector); + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const AccelNoise::Response& self) +{ + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, AccelNoise::Response& self) +{ + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAccelNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAccelNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAccelNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const GyroNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, GyroNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const GyroNoise::Response& self) +{ + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, GyroNoise::Response& self) +{ + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGyroNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGyroNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGyroNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AccelBiasModel& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, AccelBiasModel& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const AccelBiasModel::Response& self) +{ + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, AccelBiasModel::Response& self) +{ + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, xBeta); + + insert(serializer, yBeta); + + insert(serializer, zBeta); + + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xBetaOut); + extract(deserializer, *xBetaOut); + + assert(yBetaOut); + extract(deserializer, *yBetaOut); + + assert(zBetaOut); + extract(deserializer, *zBetaOut); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAccelBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAccelBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAccelBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const GyroBiasModel& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, GyroBiasModel& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const GyroBiasModel::Response& self) +{ + insert(serializer, self.x_beta); + + insert(serializer, self.y_beta); + + insert(serializer, self.z_beta); + + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, GyroBiasModel::Response& self) +{ + extract(serializer, self.x_beta); + + extract(serializer, self.y_beta); + + extract(serializer, self.z_beta); + + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, xBeta); + + insert(serializer, yBeta); + + insert(serializer, zBeta); + + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xBetaOut); + extract(deserializer, *xBetaOut); + + assert(yBetaOut); + extract(deserializer, *yBetaOut); + + assert(zBetaOut); + extract(deserializer, *zBetaOut); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGyroBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGyroBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGyroBiasModel(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AltitudeAiding& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.aiding_selector); + + } +} +void extract(Serializer& serializer, AltitudeAiding& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.aiding_selector); + + } +} + +void insert(Serializer& serializer, const AltitudeAiding::Response& self) +{ + insert(serializer, self.aiding_selector); + +} +void extract(Serializer& serializer, AltitudeAiding::Response& self) +{ + extract(serializer, self.aiding_selector); + +} + +CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, aidingSelector); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(aidingSelectorOut); + extract(deserializer, *aidingSelectorOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAltitudeAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAltitudeAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAltitudeAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AutoZupt& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.threshold); + + } +} +void extract(Serializer& serializer, AutoZupt& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.threshold); + + } +} + +void insert(Serializer& serializer, const AutoZupt::Response& self) +{ + insert(serializer, self.enable); + + insert(serializer, self.threshold); + +} +void extract(Serializer& serializer, AutoZupt::Response& self) +{ + extract(serializer, self.enable); + + extract(serializer, self.threshold); + +} + +CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + insert(serializer, threshold); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(thresholdOut); + extract(deserializer, *thresholdOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAutoZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAutoZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAutoZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AutoAngularZupt& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.threshold); + + } +} +void extract(Serializer& serializer, AutoAngularZupt& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.threshold); + + } +} + +void insert(Serializer& serializer, const AutoAngularZupt::Response& self) +{ + insert(serializer, self.enable); + + insert(serializer, self.threshold); + +} +void extract(Serializer& serializer, AutoAngularZupt::Response& self) +{ + extract(serializer, self.enable); + + extract(serializer, self.threshold); + +} + +CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + insert(serializer, threshold); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(thresholdOut); + extract(deserializer, *thresholdOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const CommandedZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedAngularZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const ReferencePosition& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.latitude); + + insert(serializer, self.longitude); + + insert(serializer, self.altitude); + + } +} +void extract(Serializer& serializer, ReferencePosition& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.latitude); + + extract(serializer, self.longitude); + + extract(serializer, self.altitude); + + } +} + +void insert(Serializer& serializer, const ReferencePosition::Response& self) +{ + insert(serializer, self.enable); + + insert(serializer, self.latitude); + + insert(serializer, self.longitude); + + insert(serializer, self.altitude); + +} +void extract(Serializer& serializer, ReferencePosition::Response& self) +{ + extract(serializer, self.enable); + + extract(serializer, self.latitude); + + extract(serializer, self.longitude); + + extract(serializer, self.altitude); + +} + +CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + insert(serializer, latitude); + + insert(serializer, longitude); + + insert(serializer, altitude); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(latitudeOut); + extract(deserializer, *latitudeOut); + + assert(longitudeOut); + extract(deserializer, *longitudeOut); + + assert(altitudeOut); + extract(deserializer, *altitudeOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveReferencePosition(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadReferencePosition(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultReferencePosition(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.adaptive_measurement); + + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); + + } +} +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.adaptive_measurement); + + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } } -void insert(Serializer& serializer, const AltitudeAiding::Response& self) +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement::Response& self) { - insert(serializer, self.aiding_selector); + insert(serializer, self.adaptive_measurement); + + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } -void extract(Serializer& serializer, AltitudeAiding::Response& self) +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Response& self) { - extract(serializer, self.aiding_selector); + extract(serializer, self.adaptive_measurement); + + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } -CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector) +CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, aidingSelector); + insert(serializer, adaptiveMeasurement); + + insert(serializer, frequency); + + insert(serializer, lowLimit); + + insert(serializer, highLimit); + + insert(serializer, lowLimitUncertainty); + + insert(serializer, highLimitUncertainty); + + insert(serializer, minimumUncertainty); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut) +CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1311,21 +2521,39 @@ CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(aidingSelectorOut); - extract(deserializer, *aidingSelectorOut); + assert(adaptiveMeasurementOut); + extract(deserializer, *adaptiveMeasurementOut); + + assert(frequencyOut); + extract(deserializer, *frequencyOut); + + assert(lowLimitOut); + extract(deserializer, *lowLimitOut); + + assert(highLimitOut); + extract(deserializer, *highLimitOut); + + assert(lowLimitUncertaintyOut); + extract(deserializer, *lowLimitUncertaintyOut); + + assert(highLimitUncertaintyOut); + extract(deserializer, *highLimitUncertaintyOut); + + assert(minimumUncertaintyOut); + extract(deserializer, *minimumUncertaintyOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAltitudeAiding(C::mip_interface& device) +CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1333,9 +2561,9 @@ CmdResult saveAltitudeAiding(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAltitudeAiding(C::mip_interface& device) +CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1343,9 +2571,9 @@ CmdResult loadAltitudeAiding(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAltitudeAiding(C::mip_interface& device) +CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1353,63 +2581,113 @@ CmdResult defaultAltitudeAiding(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const AutoZupt& self) +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement& self) { insert(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.enable); + insert(serializer, self.adaptive_measurement); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } } -void extract(Serializer& serializer, AutoZupt& self) +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement& self) { extract(serializer, self.function); if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.enable); + extract(serializer, self.adaptive_measurement); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } } -void insert(Serializer& serializer, const AutoZupt::Response& self) +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement::Response& self) { - insert(serializer, self.enable); + insert(serializer, self.adaptive_measurement); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.low_limit); + + insert(serializer, self.high_limit); + + insert(serializer, self.low_limit_uncertainty); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } -void extract(Serializer& serializer, AutoZupt::Response& self) +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Response& self) { - extract(serializer, self.enable); + extract(serializer, self.adaptive_measurement); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.low_limit); + + extract(serializer, self.high_limit); + + extract(serializer, self.low_limit_uncertainty); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } -CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) +CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, enable); + insert(serializer, adaptiveMeasurement); - insert(serializer, threshold); + insert(serializer, frequency); + + insert(serializer, lowLimit); + + insert(serializer, highLimit); + + insert(serializer, lowLimitUncertainty); + + insert(serializer, highLimitUncertainty); + + insert(serializer, minimumUncertainty); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1418,24 +2696,39 @@ CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thre assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(enableOut); - extract(deserializer, *enableOut); + assert(adaptiveMeasurementOut); + extract(deserializer, *adaptiveMeasurementOut); - assert(thresholdOut); - extract(deserializer, *thresholdOut); + assert(frequencyOut); + extract(deserializer, *frequencyOut); + + assert(lowLimitOut); + extract(deserializer, *lowLimitOut); + + assert(highLimitOut); + extract(deserializer, *highLimitOut); + + assert(lowLimitUncertaintyOut); + extract(deserializer, *lowLimitUncertaintyOut); + + assert(highLimitUncertaintyOut); + extract(deserializer, *highLimitUncertaintyOut); + + assert(minimumUncertaintyOut); + extract(deserializer, *minimumUncertaintyOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAutoZupt(C::mip_interface& device) +CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1443,9 +2736,9 @@ CmdResult saveAutoZupt(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoZupt(C::mip_interface& device) +CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1453,9 +2746,9 @@ CmdResult loadAutoZupt(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoZupt(C::mip_interface& device) +CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1463,9 +2756,9 @@ CmdResult defaultAutoZupt(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const AutoAngularZupt& self) +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement& self) { insert(serializer, self.function); @@ -1473,11 +2766,17 @@ void insert(Serializer& serializer, const AutoAngularZupt& self) { insert(serializer, self.enable); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.high_limit); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } } -void extract(Serializer& serializer, AutoAngularZupt& self) +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement& self) { extract(serializer, self.function); @@ -1485,27 +2784,45 @@ void extract(Serializer& serializer, AutoAngularZupt& self) { extract(serializer, self.enable); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.high_limit); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } } -void insert(Serializer& serializer, const AutoAngularZupt::Response& self) +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement::Response& self) { insert(serializer, self.enable); - insert(serializer, self.threshold); + insert(serializer, self.frequency); + + insert(serializer, self.high_limit); + + insert(serializer, self.high_limit_uncertainty); + + insert(serializer, self.minimum_uncertainty); } -void extract(Serializer& serializer, AutoAngularZupt::Response& self) +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Response& self) { extract(serializer, self.enable); - extract(serializer, self.threshold); + extract(serializer, self.frequency); + + extract(serializer, self.high_limit); + + extract(serializer, self.high_limit_uncertainty); + + extract(serializer, self.minimum_uncertainty); } -CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) +CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1513,13 +2830,19 @@ CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float t insert(serializer, FunctionSelector::WRITE); insert(serializer, enable); - insert(serializer, threshold); + insert(serializer, frequency); + + insert(serializer, highLimit); + + insert(serializer, highLimitUncertainty); + + insert(serializer, minimumUncertainty); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1528,7 +2851,7 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1537,15 +2860,24 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(enableOut); extract(deserializer, *enableOut); - assert(thresholdOut); - extract(deserializer, *thresholdOut); + assert(frequencyOut); + extract(deserializer, *frequencyOut); + + assert(highLimitOut); + extract(deserializer, *highLimitOut); + + assert(highLimitUncertaintyOut); + extract(deserializer, *highLimitUncertaintyOut); + + assert(minimumUncertaintyOut); + extract(deserializer, *minimumUncertaintyOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAutoAngularZupt(C::mip_interface& device) +CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1553,9 +2885,9 @@ CmdResult saveAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoAngularZupt(C::mip_interface& device) +CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1563,9 +2895,9 @@ CmdResult loadAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoAngularZupt(C::mip_interface& device) +CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1573,37 +2905,7 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert(Serializer& serializer, const CommandedZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); -} -void insert(Serializer& serializer, const CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedAngularZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert(Serializer& serializer, const AidingMeasurementEnable& self) { @@ -3024,6 +4326,129 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const HardIronOffsetNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + + } +} +void extract(Serializer& serializer, HardIronOffsetNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + + } +} + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self) +{ + insert(serializer, self.x); + + insert(serializer, self.y); + + insert(serializer, self.z); + +} +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) +{ + extract(serializer, self.x); + + extract(serializer, self.y); + + extract(serializer, self.z); + +} + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, x); + + insert(serializer, y); + + insert(serializer, z); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(xOut); + extract(deserializer, *xOut); + + assert(yOut); + extract(deserializer, *yOut); + + assert(zOut); + extract(deserializer, *zOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const MagneticDeclinationSource& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 9169cc66f..3b52d260f 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -170,6 +170,14 @@ static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE void insert_mip_filter_mag_declination_source(struct mip_serializer* serializer, const mip_filter_mag_declination_source self); void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer, mip_filter_mag_declination_source* self); +typedef uint8_t mip_filter_adaptive_measurement; +static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_DISABLED = 0; ///< No adaptive measurement +static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_FIXED = 1; ///< Enable fixed adaptive measurement (use specified limits) +static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_AUTO = 2; ///< Enable auto adaptive measurement + +void insert_mip_filter_adaptive_measurement(struct mip_serializer* serializer, const mip_filter_adaptive_measurement self); +void extract_mip_filter_adaptive_measurement(struct mip_serializer* serializer, mip_filter_adaptive_measurement* self); + //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -418,6 +426,47 @@ mip_cmd_result mip_filter_default_tare_orientation(struct mip_interface* device) ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_vehicle_dynamics_mode (0x0D,0x10) Vehicle Dynamics Mode [C] +/// Controls the vehicle dynamics mode. +/// +///@{ + +typedef uint8_t mip_filter_vehicle_dynamics_mode_command_dynamics_mode; +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_PORTABLE = 1; ///< +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_AUTOMOTIVE = 2; ///< +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_AIRBORNE = 3; ///< +static const mip_filter_vehicle_dynamics_mode_command_dynamics_mode MIP_FILTER_VEHICLE_DYNAMICS_MODE_COMMAND_DYNAMICS_MODE_AIRBORNE_HIGH_G = 4; ///< + +struct mip_filter_vehicle_dynamics_mode_command +{ + mip_function_selector function; + mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode; + +}; +typedef struct mip_filter_vehicle_dynamics_mode_command mip_filter_vehicle_dynamics_mode_command; +void insert_mip_filter_vehicle_dynamics_mode_command(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command* self); +void extract_mip_filter_vehicle_dynamics_mode_command(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command* self); + +void insert_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_command_dynamics_mode self); +void extract_mip_filter_vehicle_dynamics_mode_command_dynamics_mode(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* self); + +struct mip_filter_vehicle_dynamics_mode_response +{ + mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode; + +}; +typedef struct mip_filter_vehicle_dynamics_mode_response mip_filter_vehicle_dynamics_mode_response; +void insert_mip_filter_vehicle_dynamics_mode_response(struct mip_serializer* serializer, const mip_filter_vehicle_dynamics_mode_response* self); +void extract_mip_filter_vehicle_dynamics_mode_response(struct mip_serializer* serializer, mip_filter_vehicle_dynamics_mode_response* self); + +mip_cmd_result mip_filter_write_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode mode); +mip_cmd_result mip_filter_read_vehicle_dynamics_mode(struct mip_interface* device, mip_filter_vehicle_dynamics_mode_command_dynamics_mode* mode_out); +mip_cmd_result mip_filter_save_vehicle_dynamics_mode(struct mip_interface* device); +mip_cmd_result mip_filter_load_vehicle_dynamics_mode(struct mip_interface* device); +mip_cmd_result mip_filter_default_vehicle_dynamics_mode(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_sensor_to_vehicle_rotation_euler (0x0D,0x11) Sensor To Vehicle Rotation Euler [C] /// Set the sensor to vehicle frame rotation using Yaw, Pitch, Roll Euler angles. /// @@ -813,6 +862,182 @@ mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_accel_noise (0x0D,0x1A) Accel Noise [C] +/// Accelerometer Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0. +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct mip_filter_accel_noise_command +{ + mip_function_selector function; + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_noise_command mip_filter_accel_noise_command; +void insert_mip_filter_accel_noise_command(struct mip_serializer* serializer, const mip_filter_accel_noise_command* self); +void extract_mip_filter_accel_noise_command(struct mip_serializer* serializer, mip_filter_accel_noise_command* self); + +struct mip_filter_accel_noise_response +{ + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_noise_response mip_filter_accel_noise_response; +void insert_mip_filter_accel_noise_response(struct mip_serializer* serializer, const mip_filter_accel_noise_response* self); +void extract_mip_filter_accel_noise_response(struct mip_serializer* serializer, mip_filter_accel_noise_response* self); + +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z); +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_gyro_noise (0x0D,0x1B) Gyro Noise [C] +/// Gyroscope Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct mip_filter_gyro_noise_command +{ + mip_function_selector function; + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_noise_command mip_filter_gyro_noise_command; +void insert_mip_filter_gyro_noise_command(struct mip_serializer* serializer, const mip_filter_gyro_noise_command* self); +void extract_mip_filter_gyro_noise_command(struct mip_serializer* serializer, mip_filter_gyro_noise_command* self); + +struct mip_filter_gyro_noise_response +{ + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_noise_response mip_filter_gyro_noise_response; +void insert_mip_filter_gyro_noise_response(struct mip_serializer* serializer, const mip_filter_gyro_noise_response* self); +void extract_mip_filter_gyro_noise_response(struct mip_serializer* serializer, mip_filter_gyro_noise_response* self); + +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z); +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [C] +/// Accelerometer Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct mip_filter_accel_bias_model_command +{ + mip_function_selector function; + float x_beta; ///< Accel Bias Beta [1/second] + float y_beta; ///< Accel Bias Beta [1/second] + float z_beta; ///< Accel Bias Beta [1/second] + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_bias_model_command mip_filter_accel_bias_model_command; +void insert_mip_filter_accel_bias_model_command(struct mip_serializer* serializer, const mip_filter_accel_bias_model_command* self); +void extract_mip_filter_accel_bias_model_command(struct mip_serializer* serializer, mip_filter_accel_bias_model_command* self); + +struct mip_filter_accel_bias_model_response +{ + float x_beta; ///< Accel Bias Beta [1/second] + float y_beta; ///< Accel Bias Beta [1/second] + float z_beta; ///< Accel Bias Beta [1/second] + float x; ///< Accel Noise 1-sigma [meters/second^2] + float y; ///< Accel Noise 1-sigma [meters/second^2] + float z; ///< Accel Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_bias_model_response mip_filter_accel_bias_model_response; +void insert_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, const mip_filter_accel_bias_model_response* self); +void extract_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, mip_filter_accel_bias_model_response* self); + +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [C] +/// Gyroscope Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct mip_filter_gyro_bias_model_command +{ + mip_function_selector function; + float x_beta; ///< Gyro Bias Beta [1/second] + float y_beta; ///< Gyro Bias Beta [1/second] + float z_beta; ///< Gyro Bias Beta [1/second] + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_bias_model_command mip_filter_gyro_bias_model_command; +void insert_mip_filter_gyro_bias_model_command(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_command* self); +void extract_mip_filter_gyro_bias_model_command(struct mip_serializer* serializer, mip_filter_gyro_bias_model_command* self); + +struct mip_filter_gyro_bias_model_response +{ + float x_beta; ///< Gyro Bias Beta [1/second] + float y_beta; ///< Gyro Bias Beta [1/second] + float z_beta; ///< Gyro Bias Beta [1/second] + float x; ///< Gyro Noise 1-sigma [meters/second^2] + float y; ///< Gyro Noise 1-sigma [meters/second^2] + float z; ///< Gyro Noise 1-sigma [meters/second^2] + +}; +typedef struct mip_filter_gyro_bias_model_response mip_filter_gyro_bias_model_response; +void insert_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self); +void extract_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, mip_filter_gyro_bias_model_response* self); + +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device); +mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [C] /// Altitude Aiding Control /// @@ -948,6 +1173,206 @@ mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_reference_position (0x0D,0x26) Reference Position [C] +/// Set the Lat/Long/Alt reference position for the sensor. +/// +/// This position is used by the sensor to calculate the WGS84 gravity and WMM2015 magnetic field parameters. +/// +/// +///@{ + +struct mip_filter_reference_position_command +{ + mip_function_selector function; + bool enable; ///< enable/disable + double latitude; ///< [degrees] + double longitude; ///< [degrees] + double altitude; ///< [meters] + +}; +typedef struct mip_filter_reference_position_command mip_filter_reference_position_command; +void insert_mip_filter_reference_position_command(struct mip_serializer* serializer, const mip_filter_reference_position_command* self); +void extract_mip_filter_reference_position_command(struct mip_serializer* serializer, mip_filter_reference_position_command* self); + +struct mip_filter_reference_position_response +{ + bool enable; ///< enable/disable + double latitude; ///< [degrees] + double longitude; ///< [degrees] + double altitude; ///< [meters] + +}; +typedef struct mip_filter_reference_position_response mip_filter_reference_position_response; +void insert_mip_filter_reference_position_response(struct mip_serializer* serializer, const mip_filter_reference_position_response* self); +void extract_mip_filter_reference_position_response(struct mip_serializer* serializer, mip_filter_reference_position_response* self); + +mip_cmd_result mip_filter_write_reference_position(struct mip_interface* device, bool enable, double latitude, double longitude, double altitude); +mip_cmd_result mip_filter_read_reference_position(struct mip_interface* device, bool* enable_out, double* latitude_out, double* longitude_out, double* altitude_out); +mip_cmd_result mip_filter_save_reference_position(struct mip_interface* device); +mip_cmd_result mip_filter_load_reference_position(struct mip_interface* device); +mip_cmd_result mip_filter_default_reference_position(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_accel_magnitude_error_adaptive_measurement (0x0D,0x44) Accel Magnitude Error Adaptive Measurement [C] +/// Enable or disable the gravity magnitude error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// Adaptive measurements can be enabled/disabled without the need for providing the additional parameters. +/// In this case, only the function selector and enable value are required; all other parameters will remain at their previous values. +/// When “auto-adaptive” is selected, the filter and limit parameters are ignored. +/// Instead, aiding measurements which rely on the gravity vector will be automatically reweighted by the Kalman filter according to the perceived measurement quality. +/// +/// +///@{ + +struct mip_filter_accel_magnitude_error_adaptive_measurement_command +{ + mip_function_selector function; + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_magnitude_error_adaptive_measurement_command mip_filter_accel_magnitude_error_adaptive_measurement_command; +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_command* self); +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_command* self); + +struct mip_filter_accel_magnitude_error_adaptive_measurement_response +{ + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_accel_magnitude_error_adaptive_measurement_response mip_filter_accel_magnitude_error_adaptive_measurement_response; +void insert_mip_filter_accel_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, const mip_filter_accel_magnitude_error_adaptive_measurement_response* self); +void extract_mip_filter_accel_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, mip_filter_accel_magnitude_error_adaptive_measurement_response* self); + +mip_cmd_result mip_filter_write_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty); +mip_cmd_result mip_filter_read_accel_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out); +mip_cmd_result mip_filter_save_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_load_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_default_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_magnitude_error_adaptive_measurement (0x0D,0x45) Mag Magnitude Error Adaptive Measurement [C] +/// Enable or disable the magnetometer magnitude error adaptive measurement. +/// This feature will reject magnetometer readings that are out of range of the thresholds specified (fixed adaptive) or calculated internally (auto-adaptive). +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// +///@{ + +struct mip_filter_mag_magnitude_error_adaptive_measurement_command +{ + mip_function_selector function; + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_magnitude_error_adaptive_measurement_command mip_filter_mag_magnitude_error_adaptive_measurement_command; +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_command* self); +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_command(struct mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_command* self); + +struct mip_filter_mag_magnitude_error_adaptive_measurement_response +{ + mip_filter_adaptive_measurement adaptive_measurement; ///< Adaptive measurement selector + float frequency; ///< Low-pass filter curoff frequency [hertz] + float low_limit; ///< [meters/second^2] + float high_limit; ///< [meters/second^2] + float low_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_magnitude_error_adaptive_measurement_response mip_filter_mag_magnitude_error_adaptive_measurement_response; +void insert_mip_filter_mag_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, const mip_filter_mag_magnitude_error_adaptive_measurement_response* self); +void extract_mip_filter_mag_magnitude_error_adaptive_measurement_response(struct mip_serializer* serializer, mip_filter_mag_magnitude_error_adaptive_measurement_response* self); + +mip_cmd_result mip_filter_write_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement adaptive_measurement, float frequency, float low_limit, float high_limit, float low_limit_uncertainty, float high_limit_uncertainty, float minimum_uncertainty); +mip_cmd_result mip_filter_read_mag_magnitude_error_adaptive_measurement(struct mip_interface* device, mip_filter_adaptive_measurement* adaptive_measurement_out, float* frequency_out, float* low_limit_out, float* high_limit_out, float* low_limit_uncertainty_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out); +mip_cmd_result mip_filter_save_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_dip_angle_error_adaptive_measurement (0x0D,0x46) Mag Dip Angle Error Adaptive Measurement [C] +/// Enable or disable the magnetometer dip angle error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// The magnetometer dip angle adaptive measurement is ignored if the auto-adaptive magnetometer magnitude or auto-adaptive accel magnitude options are selected. +/// +/// +///@{ + +struct mip_filter_mag_dip_angle_error_adaptive_measurement_command +{ + mip_function_selector function; + bool enable; ///< Enable/Disable + float frequency; ///< Low-pass filter curoff frequency [hertz] + float high_limit; ///< [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_dip_angle_error_adaptive_measurement_command mip_filter_mag_dip_angle_error_adaptive_measurement_command; +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_command(struct mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_command* self); +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_command(struct mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_command* self); + +struct mip_filter_mag_dip_angle_error_adaptive_measurement_response +{ + bool enable; ///< Enable/Disable + float frequency; ///< Low-pass filter curoff frequency [hertz] + float high_limit; ///< [meters/second^2] + float high_limit_uncertainty; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty; ///< 1-Sigma [meters/second^2] + +}; +typedef struct mip_filter_mag_dip_angle_error_adaptive_measurement_response mip_filter_mag_dip_angle_error_adaptive_measurement_response; +void insert_mip_filter_mag_dip_angle_error_adaptive_measurement_response(struct mip_serializer* serializer, const mip_filter_mag_dip_angle_error_adaptive_measurement_response* self); +void extract_mip_filter_mag_dip_angle_error_adaptive_measurement_response(struct mip_serializer* serializer, mip_filter_mag_dip_angle_error_adaptive_measurement_response* self); + +mip_cmd_result mip_filter_write_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool enable, float frequency, float high_limit, float high_limit_uncertainty, float minimum_uncertainty); +mip_cmd_result mip_filter_read_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device, bool* enable_out, float* frequency_out, float* high_limit_out, float* high_limit_uncertainty_out, float* minimum_uncertainty_out); +mip_cmd_result mip_filter_save_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_aiding_measurement_enable (0x0D,0x50) Aiding Measurement Enable [C] /// Enables / disables the specified aiding measurement source. /// @@ -1442,6 +1867,51 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [C] +/// Set the expected hard iron offset noise 1-sigma values +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct mip_filter_hard_iron_offset_noise_command +{ + mip_function_selector function; + float x; ///< HI Offset Noise 1-sima [gauss] + float y; ///< HI Offset Noise 1-sima [gauss] + float z; ///< HI Offset Noise 1-sima [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_command mip_filter_hard_iron_offset_noise_command; +void insert_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self); +void extract_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self); + +struct mip_filter_hard_iron_offset_noise_response +{ + float x; ///< HI Offset Noise 1-sima [gauss] + float y; ///< HI Offset Noise 1-sima [gauss] + float z; ///< HI Offset Noise 1-sima [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_offset_noise_response; +void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); +void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z); +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [C] /// Source for magnetic declination angle, and user supplied value for manual selection. /// diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index f9ebdc736..28a0ef255 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -167,6 +167,13 @@ enum class FilterMagDeclinationSource : uint8_t MANUAL = 3, ///< Magnetic field is assumed to have the declination angle specified by the user. }; +enum class FilterAdaptiveMeasurement : uint8_t +{ + DISABLED = 0, ///< No adaptive measurement + FIXED = 1, ///< Enable fixed adaptive measurement (use specified limits) + AUTO = 2, ///< Enable auto adaptive measurement +}; + //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -507,6 +514,56 @@ CmdResult defaultTareOrientation(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_vehicle_dynamics_mode (0x0D,0x10) Vehicle Dynamics Mode [CPP] +/// Controls the vehicle dynamics mode. +/// +///@{ + +struct VehicleDynamicsMode +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class DynamicsMode : uint8_t + { + PORTABLE = 1, ///< + AUTOMOTIVE = 2, ///< + AIRBORNE = 3, ///< + AIRBORNE_HIGH_G = 4, ///< + }; + + FunctionSelector function = static_cast(0); + DynamicsMode mode = static_cast(0); + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; + + DynamicsMode mode = static_cast(0); + + }; +}; +void insert(Serializer& serializer, const VehicleDynamicsMode& self); +void extract(Serializer& serializer, VehicleDynamicsMode& self); + +void insert(Serializer& serializer, const VehicleDynamicsMode::Response& self); +void extract(Serializer& serializer, VehicleDynamicsMode::Response& self); + +CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); +CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); +CmdResult saveVehicleDynamicsMode(C::mip_interface& device); +CmdResult loadVehicleDynamicsMode(C::mip_interface& device); +CmdResult defaultVehicleDynamicsMode(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_sensor_to_vehicle_rotation_euler (0x0D,0x11) Sensor To Vehicle Rotation Euler [CPP] /// Set the sensor to vehicle frame rotation using Yaw, Pitch, Roll Euler angles. /// @@ -980,6 +1037,222 @@ CmdResult defaultAutoInitControl(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_accel_noise (0x0D,0x1A) Accel Noise [CPP] +/// Accelerometer Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0. +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct AccelNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; + + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const AccelNoise& self); +void extract(Serializer& serializer, AccelNoise& self); + +void insert(Serializer& serializer, const AccelNoise::Response& self); +void extract(Serializer& serializer, AccelNoise::Response& self); + +CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z); +CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult saveAccelNoise(C::mip_interface& device); +CmdResult loadAccelNoise(C::mip_interface& device); +CmdResult defaultAccelNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_gyro_noise (0x0D,0x1B) Gyro Noise [CPP] +/// Gyroscope Noise Standard Deviation +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct GyroNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; + + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const GyroNoise& self); +void extract(Serializer& serializer, GyroNoise& self); + +void insert(Serializer& serializer, const GyroNoise::Response& self); +void extract(Serializer& serializer, GyroNoise::Response& self); + +CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z); +CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult saveGyroNoise(C::mip_interface& device); +CmdResult loadGyroNoise(C::mip_interface& device); +CmdResult defaultGyroNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [CPP] +/// Accelerometer Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct AccelBiasModel +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x_beta = 0; ///< Accel Bias Beta [1/second] + float y_beta = 0; ///< Accel Bias Beta [1/second] + float z_beta = 0; ///< Accel Bias Beta [1/second] + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; + + float x_beta = 0; ///< Accel Bias Beta [1/second] + float y_beta = 0; ///< Accel Bias Beta [1/second] + float z_beta = 0; ///< Accel Bias Beta [1/second] + float x = 0; ///< Accel Noise 1-sigma [meters/second^2] + float y = 0; ///< Accel Noise 1-sigma [meters/second^2] + float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const AccelBiasModel& self); +void extract(Serializer& serializer, AccelBiasModel& self); + +void insert(Serializer& serializer, const AccelBiasModel::Response& self); +void extract(Serializer& serializer, AccelBiasModel::Response& self); + +CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); +CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult saveAccelBiasModel(C::mip_interface& device); +CmdResult loadAccelBiasModel(C::mip_interface& device); +CmdResult defaultAccelBiasModel(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [CPP] +/// Gyroscope Bias Model Parameters +/// +/// Each of the noise values must be greater than 0.0 +/// +/// +///@{ + +struct GyroBiasModel +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x_beta = 0; ///< Gyro Bias Beta [1/second] + float y_beta = 0; ///< Gyro Bias Beta [1/second] + float z_beta = 0; ///< Gyro Bias Beta [1/second] + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; + + float x_beta = 0; ///< Gyro Bias Beta [1/second] + float y_beta = 0; ///< Gyro Bias Beta [1/second] + float z_beta = 0; ///< Gyro Bias Beta [1/second] + float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const GyroBiasModel& self); +void extract(Serializer& serializer, GyroBiasModel& self); + +void insert(Serializer& serializer, const GyroBiasModel::Response& self); +void extract(Serializer& serializer, GyroBiasModel::Response& self); + +CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); +CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult saveGyroBiasModel(C::mip_interface& device); +CmdResult loadGyroBiasModel(C::mip_interface& device); +CmdResult defaultGyroBiasModel(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [CPP] /// Altitude Aiding Control /// @@ -1169,6 +1442,246 @@ CmdResult commandedAngularZupt(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_reference_position (0x0D,0x26) Reference Position [CPP] +/// Set the Lat/Long/Alt reference position for the sensor. +/// +/// This position is used by the sensor to calculate the WGS84 gravity and WMM2015 magnetic field parameters. +/// +/// +///@{ + +struct ReferencePosition +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + bool enable = 0; ///< enable/disable + double latitude = 0; ///< [degrees] + double longitude = 0; ///< [degrees] + double altitude = 0; ///< [meters] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; + + bool enable = 0; ///< enable/disable + double latitude = 0; ///< [degrees] + double longitude = 0; ///< [degrees] + double altitude = 0; ///< [meters] + + }; +}; +void insert(Serializer& serializer, const ReferencePosition& self); +void extract(Serializer& serializer, ReferencePosition& self); + +void insert(Serializer& serializer, const ReferencePosition::Response& self); +void extract(Serializer& serializer, ReferencePosition::Response& self); + +CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); +CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); +CmdResult saveReferencePosition(C::mip_interface& device); +CmdResult loadReferencePosition(C::mip_interface& device); +CmdResult defaultReferencePosition(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_accel_magnitude_error_adaptive_measurement (0x0D,0x44) Accel Magnitude Error Adaptive Measurement [CPP] +/// Enable or disable the gravity magnitude error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// Adaptive measurements can be enabled/disabled without the need for providing the additional parameters. +/// In this case, only the function selector and enable value are required; all other parameters will remain at their previous values. +/// When “auto-adaptive” is selected, the filter and limit parameters are ignored. +/// Instead, aiding measurements which rely on the gravity vector will be automatically reweighted by the Kalman filter according to the perceived measurement quality. +/// +/// +///@{ + +struct AccelMagnitudeErrorAdaptiveMeasurement +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement& self); +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement& self); + +void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement::Response& self); +void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Response& self); + +CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_magnitude_error_adaptive_measurement (0x0D,0x45) Mag Magnitude Error Adaptive Measurement [CPP] +/// Enable or disable the magnetometer magnitude error adaptive measurement. +/// This feature will reject magnetometer readings that are out of range of the thresholds specified (fixed adaptive) or calculated internally (auto-adaptive). +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// +///@{ + +struct MagMagnitudeErrorAdaptiveMeasurement +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float low_limit = 0; ///< [meters/second^2] + float high_limit = 0; ///< [meters/second^2] + float low_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement& self); +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement& self); + +void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement::Response& self); +void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Response& self); + +CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_dip_angle_error_adaptive_measurement (0x0D,0x46) Mag Dip Angle Error Adaptive Measurement [CPP] +/// Enable or disable the magnetometer dip angle error adaptive measurement. +/// This function can be used to tune the filter performance in the target application +/// +/// Pick values that give you the least occurrence of invalid EF attitude output. +/// The default values are good for standard low dynamics applications. +/// Increase values for higher dynamic conditions, lower values for lower dynamic. +/// Too low a value will result in excessive heading errors. +/// Higher values increase heading errors when undergoing magnetic field anomalies caused by DC currents, magnets, steel structures,etc. +/// +/// The magnetometer dip angle adaptive measurement is ignored if the auto-adaptive magnetometer magnitude or auto-adaptive accel magnitude options are selected. +/// +/// +///@{ + +struct MagDipAngleErrorAdaptiveMeasurement +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + bool enable = 0; ///< Enable/Disable + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float high_limit = 0; ///< [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + bool enable = 0; ///< Enable/Disable + float frequency = 0; ///< Low-pass filter curoff frequency [hertz] + float high_limit = 0; ///< [meters/second^2] + float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] + float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + }; +}; +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement& self); +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement& self); + +void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement::Response& self); +void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Response& self); + +CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); +CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_aiding_measurement_enable (0x0D,0x50) Aiding Measurement Enable [CPP] /// Enables / disables the specified aiding measurement source. /// @@ -1804,6 +2317,61 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [CPP] +/// Set the expected hard iron offset noise 1-sigma values +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Each of the noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. +/// Default values provide good performance for most laboratory conditions. +/// +/// +///@{ + +struct HardIronOffsetNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float x = 0; ///< HI Offset Noise 1-sima [gauss] + float y = 0; ///< HI Offset Noise 1-sima [gauss] + float z = 0; ///< HI Offset Noise 1-sima [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + + float x = 0; ///< HI Offset Noise 1-sima [gauss] + float y = 0; ///< HI Offset Noise 1-sima [gauss] + float z = 0; ///< HI Offset Noise 1-sima [gauss] + + }; +}; +void insert(Serializer& serializer, const HardIronOffsetNoise& self); +void extract(Serializer& serializer, HardIronOffsetNoise& self); + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z); +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult saveHardIronOffsetNoise(C::mip_interface& device); +CmdResult loadHardIronOffsetNoise(C::mip_interface& device); +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [CPP] /// Source for magnetic declination angle, and user supplied value for manual selection. /// From 127378157592e34b931ca0e0368a541296206716 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 2 Nov 2022 18:05:45 -0400 Subject: [PATCH 016/252] Change extern C functions to callbacks for use in dynamic libraries (#34) * Add remaining_time accessor to pending commands. * Refactor mip_interface and MipDevice: - Use callbacks. - Add wait_time parameter to recv_from_device. * Update serial and TCP connections to include wait_time parameter. * Add nonmember callback setters to DeviceInterface. * Update recording connection. * Update examples. * Fix serial port build on Windows. * Removes extern timestamp function from connections Co-authored-by: Rob Fisher --- examples/CV7/CV7_example.c | 11 +- examples/GQ7/GQ7_example.c | 12 +- examples/GX5_45/GX5_45_example.c | 12 +- examples/threading.cpp | 8 +- examples/watch_imu.c | 10 +- src/mip/extras/recording_connection.cpp | 4 +- src/mip/extras/recording_connection.hpp | 2 +- src/mip/mip_cmdqueue.c | 25 +- src/mip/mip_cmdqueue.h | 1 + src/mip/mip_device.cpp | 51 ++++- src/mip/mip_device.hpp | 277 +++++++++++++++++++---- src/mip/mip_interface.c | 289 +++++++++++++++++++----- src/mip/mip_interface.h | 107 +++------ src/mip/platform/serial_connection.cpp | 9 +- src/mip/platform/serial_connection.hpp | 5 +- src/mip/platform/tcp_connection.cpp | 7 +- src/mip/platform/tcp_connection.hpp | 5 +- src/mip/utils/serial_port.c | 42 ++-- src/mip/utils/serial_port.h | 2 +- 19 files changed, 641 insertions(+), 238 deletions(-) diff --git a/examples/CV7/CV7_example.c b/examples/CV7/CV7_example.c index 30366d804..a36465a5c 100644 --- a/examples/CV7/CV7_example.c +++ b/examples/CV7/CV7_example.c @@ -73,7 +73,7 @@ const uint8_t FILTER_PITCH_EVENT_ACTION_ID = 2; //Required MIP interface user-defined functions timestamp_type get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -125,7 +125,10 @@ int main(int argc, const char* argv[]) //Initialize the MIP interface // - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); // @@ -383,10 +386,10 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, out_length); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); } diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index 9d0f8e01f..f56765a5a 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -80,7 +80,7 @@ bool filter_state_full_nav = false; //Required MIP interface user-defined functions timestamp_type get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -130,7 +130,11 @@ int main(int argc, const char* argv[]) //Initialize the MIP interface // - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + // @@ -393,10 +397,10 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, out_length); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); } diff --git a/examples/GX5_45/GX5_45_example.c b/examples/GX5_45/GX5_45_example.c index c10fd20f2..745f3b645 100644 --- a/examples/GX5_45/GX5_45_example.c +++ b/examples/GX5_45/GX5_45_example.c @@ -79,7 +79,7 @@ bool filter_state_running = false; //Required MIP interface user-defined functions timestamp_type get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -129,7 +129,11 @@ int main(int argc, const char* argv[]) //Initialize the MIP interface // - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + // @@ -362,10 +366,10 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, out_length); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); } diff --git a/examples/threading.cpp b/examples/threading.cpp index 46cf29662..21b4a178d 100644 --- a/examples/threading.cpp +++ b/examples/threading.cpp @@ -50,7 +50,7 @@ void device_thread_loop(mip::DeviceInterface* device) { while(!stop) { - if( !device->update(false) ) + if( !device->update(0) ) { device->cmdQueue().clear(); // Avoid deadlocks if the socket is closed. break; @@ -60,10 +60,10 @@ void device_thread_loop(mip::DeviceInterface* device) } } -bool update_device(mip::DeviceInterface& device, bool blocking) +bool update_device(mip::DeviceInterface& device, mip::Timeout wait_time) { - if( !blocking ) - return device.defaultUpdate(blocking); + if( wait_time > 0 ) + return device.defaultUpdate(wait_time); // Optionally display progress while waiting for command replies. // Displaying it here makes it update more frequently. diff --git a/examples/watch_imu.c b/examples/watch_imu.c index 937b6a53b..4c6d88a5f 100644 --- a/examples/watch_imu.c +++ b/examples/watch_imu.c @@ -95,12 +95,12 @@ timestamp_type get_current_timestamp() } -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { (void)device; *timestamp_out = get_current_timestamp(); - if( !serial_port_read(&port, buffer, max_length, out_length) ) + if( !serial_port_read(&port, buffer, max_length, wait_time, out_length) ) return false; return true; @@ -142,7 +142,11 @@ int main(int argc, const char* argv[]) if( !open_port(argv[1], baudrate) ) return 1; - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + // Record program start time for use with difftime in getTimestamp(). time(&startTime); diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp index 82bd86208..725bdfbf6 100644 --- a/src/mip/extras/recording_connection.cpp +++ b/src/mip/extras/recording_connection.cpp @@ -25,9 +25,9 @@ bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) } ///@copydoc mip::Connection::recvFromDevice -bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* count_out, Timestamp* timestamp_out) +bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* count_out, Timestamp* timestamp_out) { - const bool ok = mConnection->recvFromDevice(buffer, max_length, count_out, timestamp_out); + const bool ok = mConnection->recvFromDevice(buffer, max_length, wait_time, count_out, timestamp_out); if( ok && mRecvFile != nullptr ) mRecvFile->write(reinterpret_cast(buffer), *count_out); return ok; diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index d05ea8069..7446d8324 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -24,7 +24,7 @@ class RecordingConnection : public Connection RecordingConnection(Connection* connection, std::ostream* recvStream=nullptr, std::ostream* sendStream=nullptr); bool sendToDevice(const uint8_t* data, size_t length) final; - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* count_out, Timestamp* timestamp_out) final; + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) final; protected: Connection* mConnection; diff --git a/src/mip/mip_cmdqueue.c b/src/mip/mip_cmdqueue.c index dbb6cdc5e..62d2ce4dd 100644 --- a/src/mip/mip_cmdqueue.c +++ b/src/mip/mip_cmdqueue.c @@ -134,6 +134,29 @@ uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) return cmd->_response_length; } +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines how much time is remaining before the command times out. +/// +/// For most cases you should instead call mip_pending_cmd_check_timeout() to +/// know if the command has timed out or not. +/// +///@param cmd The command to check. Must be in MIP_STATUS_WAITING state. +///@param now The current timestamp. +/// +///@warning The command must be in the MIP_STATUS_WAITING state, otherwise the +/// result is unspecified. +/// +///@returns The time remaining before the command times out. The value will be +/// negative if the timeout time has passed. +/// +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now) +{ + assert(cmd->_status == MIP_STATUS_WAITING); + + // result <= 0 if timed out. + // Note: this still works with unsigned overflow. + return (int)(now - cmd->_timeout_time); +} //////////////////////////////////////////////////////////////////////////////// ///@brief Checks if the command should time out. @@ -148,7 +171,7 @@ bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type no { if( cmd->_status == MIP_STATUS_WAITING ) { - if( (int)(now - cmd->_timeout_time) > 0 ) + if( mip_pending_cmd_remaining_time(cmd, now) > 0 ) { return true; } diff --git a/src/mip/mip_cmdqueue.h b/src/mip/mip_cmdqueue.h index 4515a1b25..d18a60620 100644 --- a/src/mip/mip_cmdqueue.h +++ b/src/mip/mip_cmdqueue.h @@ -67,6 +67,7 @@ enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd); const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd); uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd); +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now); bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now); ///@} diff --git a/src/mip/mip_device.cpp b/src/mip/mip_device.cpp index f1d36b234..5e68c032d 100644 --- a/src/mip/mip_device.cpp +++ b/src/mip/mip_device.cpp @@ -2,19 +2,50 @@ #include "mip_device.hpp" namespace mip { -namespace C { -extern "C" { -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) -{ - return static_cast(device)->recvFromDevice(buffer, max_length, out_length, timestamp_out); -} +//////////////////////////////////////////////////////////////////////////////// +///@fn Connection::sendToDevice +/// +///@brief Sends bytes to the device +/// +///@param data The data to send to the device +///@param length Length of data in bytes +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +///@fn Connection::recvFromDevice +/// +///@brief Receives bytes from the device +/// +///@param buffer Buffer to store the received data in +///@param max_length Max number of bytes that can be read. Should be at most the length of buffer. +///@param length_out Number of bytes actually read. +///@param timestamp Timestamp of when the data was received +//////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length) + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets up the mip interface callbacks to point at this object. +/// +///@note This sets the interface's user pointer to this connection object. +/// +///@param device Device to configure. +/// +void Connection::connect_interface(C::mip_interface* device) { - return static_cast(device)->sendToDevice(data, length); + auto send = [](C::mip_interface* device, const uint8_t* data, size_t length)->bool + { + return static_cast(C::mip_interface_user_pointer(device))->sendToDevice(data, length); + }; + auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out)->bool + { + return static_cast(C::mip_interface_user_pointer(device))->recvFromDevice(buffer, max_length, wait_time, length_out, timestamp_out); + }; + + C::mip_interface_set_user_pointer(device, this); + + C::mip_interface_set_send_function(device, send); + C::mip_interface_set_recv_function(device, recv); } -} // extern "C" -} // namespace C } // namespace mip diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index bd8717ec7..1db0813c3 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -135,19 +135,10 @@ template bool startCommand(C::mip_interface& device, C::mip_pending_c class Connection { public: - ///@brief Sends bytes to the device - /// - ///@param data The data to send to the device - ///@param length Length of data in bytes - virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; // Must be implemented by a derived class. + virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; + virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) = 0; - ///@brief Receives bytes from the device - /// - ///@param buffer Buffer to store the received data in - ///@param max_length Max number of bytes that can be read. Should be at most the length of buffer. - ///@param length_out Number of bytes actually read. - ///@param timestamp Timestamp of when the data was received - virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, Timestamp* timestamp) = 0; // Must be implemented by a derived class. + void connect_interface(C::mip_interface* device); }; @@ -161,9 +152,21 @@ class DeviceInterface : public C::mip_interface // Constructors // + ///@copydoc mip::C::mip_interface_init + /// The interface callbacks must be assigned separately (e.g. with Connection::connect_interface()) + DeviceInterface(uint8_t* parseBuffer, size_t parseBufferSize, Timeout parseTimeout, Timeout baseReplyTimeout) + { + C::mip_interface_init(this, parseBuffer, parseBufferSize, parseTimeout, baseReplyTimeout, nullptr, nullptr, &C::mip_interface_default_update, nullptr); + } + ///@copydoc mip::C::mip_interface_init ///@param connection The connection object used to communicate with the device. This object must exist for the life of the DeviceInterface object - DeviceInterface(Connection* connection, uint8_t* parseBuffer, size_t parseBufferSize, Timeout parseTimeout, Timeout baseReplyTimeout) : mConnection(connection) { C::mip_interface_init(this, parseBuffer, parseBufferSize, parseTimeout, baseReplyTimeout); } + DeviceInterface(Connection* connection, uint8_t* parseBuffer, size_t parseBufferSize, Timeout parseTimeout, Timeout baseReplyTimeout) : + DeviceInterface(parseBuffer, parseBufferSize, parseTimeout, baseReplyTimeout) + { + if(connection) + connection->connect_interface(this); + } DeviceInterface(const DeviceInterface&) = delete; DeviceInterface& operator=(const DeviceInterface&) = delete; @@ -171,50 +174,84 @@ class DeviceInterface : public C::mip_interface ~DeviceInterface() = default; // - // Accessors + // Callback functions // - ///@copydoc C::mip_interface_set_update_function + // C function callbacks + + C::mip_send_callback sendFunction() const { return C::mip_interface_send_function(this); } + C::mip_recv_callback recvFunction() const { return C::mip_interface_recv_function(this); } + C::mip_update_callback updateFunction() const { return C::mip_interface_update_function(this); } + + void setSendFunction (C::mip_send_callback callback) { C::mip_interface_set_send_function (this, callback); } + void setRecvFunction (C::mip_recv_callback callback) { C::mip_interface_set_recv_function (this, callback); } void setUpdateFunction(C::mip_update_callback function) { C::mip_interface_set_update_function(this, function); } - template + // free/nonmember function callbacks + + template + void setSendFunction(); + + template + void setRecvFunction(); + + template + void setUpdateFunction(); + + // derived member function callbacks + + template + void setSendFunction(); + + template + void setRecvFunction(); + + template void setUpdateFunction(); + // Separate class object callbacks + + template< + class T, + bool (T::*Send)(const uint8_t*, size_t), + bool (T::*Recv)(uint8_t*, size_t, Timeout, size_t*, Timestamp*), + bool (T::*Update)(Timeout) = nullptr + > + void setCallbacks(T* object); + + // + // General accessors + // + void setMaxPacketsPerPoll(unsigned int maxPackets) { C::mip_interface_set_max_packets_per_update(this, maxPackets); } unsigned int maxPacketsPerPoll() const { return C::mip_interface_max_packets_per_update(this); } Timeout baseReplyTimeout() const { return C::mip_cmd_queue_base_reply_timeout(&cmdQueue()); } void setBaseReplyTimeout(Timeout timeout) { C::mip_cmd_queue_set_base_reply_timeout(&cmdQueue(), timeout); } + Parser& parser() { return *static_cast(C::mip_interface_parser(this)); } CmdQueue& cmdQueue() { return *static_cast(C::mip_interface_cmd_queue(this)); } const Parser& parser() const { return const_cast(this)->parser(); } const CmdQueue& cmdQueue() const { return const_cast(this)->cmdQueue(); } - const Connection* connection() const { return mConnection; } - void setConnection(Connection* connection) { mConnection = connection; } - // // Communications // - RemainingCount receiveBytes(const uint8_t* data, size_t length, Timestamp timestamp) { return C::mip_interface_receive_bytes(this, data, length, timestamp); } - - void receivePacket(const C::mip_packet& packet, Timestamp timestamp) { C::mip_interface_receive_packet(this, &packet, timestamp); } - - bool sendToDevice(const uint8_t* data, size_t length) { return mConnection->sendToDevice(data, length); } + bool sendToDevice(const uint8_t* data, size_t length) { return C::mip_interface_send_to_device(this, data, length); } bool sendToDevice(const C::mip_packet& packet) { return sendToDevice(C::mip_packet_pointer(&packet), C::mip_packet_total_length(&packet)); } - + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp) { return C::mip_interface_recv_from_device(this, buffer, max_length, wait_time, length_out, timestamp); } bool update(bool blocking=false) { return C::mip_interface_update(this, blocking); } - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, Timestamp* timestamp) { return mConnection->recvFromDevice(buffer, max_length, length_out, timestamp); } + bool defaultUpdate(bool blocking=false) { return C::mip_interface_default_update(this, blocking); } + RemainingCount receiveBytes(const uint8_t* data, size_t length, Timestamp timestamp) { return C::mip_interface_receive_bytes(this, data, length, timestamp); } + void receivePacket(const C::mip_packet& packet, Timestamp timestamp) { C::mip_interface_receive_packet(this, &packet, timestamp); } void processUnparsedPackets() { C::mip_interface_process_unparsed_packets(this); } CmdResult waitForReply(C::mip_pending_cmd& cmd) { return C::mip_interface_wait_for_reply(this, &cmd); } - bool defaultUpdate(bool blocking=false) { return C::mip_interface_default_update(this, blocking); } - // // Data Callbacks // @@ -272,36 +309,198 @@ class DeviceInterface : public C::mip_interface // template // bool startCommand(PendingCmd& pending, const Cmd& cmd, uint8_t* responseBuffer, uint8_t responseBufferSize, Timeout additionalTime=0) { return mip::startCommand(pending, cmd, responseBuffer, responseBufferSize, additionalTime); } - -private: - Connection* mConnection; }; + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the send callback function (free function version). +/// +///@tparam Send A compile-time pointer to the callback function. +/// +template +void DeviceInterface::setSendFunction() +{ + setSendFunction([](C::mip_interface* device, const uint8_t* data, size_t length){ + return (*Send)(*static_cast(device), data, length); + }); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the receive callback function (free function version). +/// +///@tparam Send A compile-time pointer to the callback function. +/// +template +void DeviceInterface::setRecvFunction() +{ + setRecvFunction([](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out){ + return (*Recv)(*static_cast(device), buffer, max_length, wait_time, length_out, timestamp_out); + }); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the update callback function (free function version). +/// +///@tparam Send A compile-time pointer to the callback function. +/// +template +void DeviceInterface::setUpdateFunction() +{ + setUpdateFunction([](C::mip_interface* device, C::timeout_type wait_time){ + return (*Update)(*static_cast(device), wait_time); + }); +} + + //////////////////////////////////////////////////////////////////////////////// -///@brief Sets the update function to a function taking a MipDevice reference. +///@brief Sets the send callback function (derived member function version). +/// +///@tparam Derived Derived class type. Must inherit from DeviceInterface. +///@tparam Send Compile-time pointer to member function of Derived. /// ///@code{.cpp} -/// bool updateDevice(DeviceInterface& device, bool blocking) +/// class MyClass : public mip::DeviceInterface /// { -/// return device.defaultUpdate(blocking); -/// } +/// bool sendToDevice(const uint8_t* data, size_t size); +/// bool recvFromDevice(uint8_t* data, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out); +/// }; +/// +/// MyClass instance; +/// instance.setSendFunction(); +/// instance.setRecvFunction(); /// -/// device.setUpdateFunction<&updateDevice>(); +/// instance.setCallbacks(connection); ///@endcode /// -template + +template +void DeviceInterface::setSendFunction() +{ + static_assert(std::is_base_of::value, "Derived must be derived from C::mip_interface."); + + setSendFunction( + [](C::mip_interface* device, const uint8_t* data, size_t length) + { + return (static_cast(device)->*Send)(data, length); + } + ); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the receive callback function (derived member function version). +/// +///@tparam Derived Derived class type. Must inherit from DeviceInterface. +///@tparam Recv Compile-time pointer to member function of Derived. +/// +///@see DeviceInterface::setSendFunction() +/// +template +void DeviceInterface::setRecvFunction() +{ + static_assert(std::is_base_of::value, "Derived must be derived from C::mip_interface."); + + setRecvFunction( + [](C::mip_interface* device, uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) + { + return (static_cast(device)->*Recv)(buffer, max_length, wait_time, length_out, timestamp_out); + } + ); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the update callback function (derived member function version). +/// +///@tparam Derived Derived class type. Must inherit from DeviceInterface. +///@tparam Update Compile-time pointer to member function of Derived. +/// +///@see DeviceInterface::setSendFunction() +/// +template void DeviceInterface::setUpdateFunction() { + static_assert(std::is_base_of::value, "Derived must be derived from C::mip_interface."); + setUpdateFunction( - [](C::mip_interface* device, bool blocking)->bool + [](C::mip_interface* device, C::timeout_type wait_time)->bool { - return Function(*static_cast(device), blocking); + return (static_cast(device)->*Update)(wait_time); } ); } +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the callback functions to a common class object. +/// +///@tparam T +/// A class type. +/// +///@tparam Send +/// A member function pointer for sending bytes to the device. +/// May be NULL. +/// +///@tparam Recv +/// A member function pointer for receiving bytes from the device. +/// May be NULL. +/// +///@tparam Update +/// A member function pointer for updating the device. +/// If both this and Recv are NULL, no update function is set. +/// If Update is NULL but Recv is not, the default update function +/// is used instead. +/// +///@param object +/// An instance of T. The interface's user pointer will be set to this +/// value. All of the callbacks will be invoked on this instance. +/// +///@code{.cpp} +/// class DeviceConnection +/// { +/// bool send(const uint8_t* data, size_t length); +/// bool recv(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out); +/// }; +/// +/// DeviceConnection connection; +/// mip::DeviceInterface interface; +/// +/// interface.setCallbacks(&connection); +///@endcode +/// +template< + class T, + bool (T::*Send)(const uint8_t*, size_t), + bool (T::*Recv)(uint8_t*, size_t, Timeout, size_t*, Timestamp*), + bool (T::*Update)(Timeout) +> +void DeviceInterface::setCallbacks(T* object) +{ + auto send = [](C::mip_interface* device, const uint8_t* data, size_t size) + { + return (static_cast(mip_interface_user_pointer(device))->*Send)(data, size); + }; + auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out) + { + return (static_cast(mip_interface_user_pointer(device))->*Recv)(buffer, max_length, wait_time, length_out, timestamp_out); + }; + auto update = [](C::mip_interface* device, C::timeout_type wait_time) + { + return (static_cast(mip_interface_user_pointer(device))->*Update)(wait_time); + }; + + C::mip_interface_set_user_pointer(this, object); + C::mip_interface_set_send_function(this, Send != nullptr ? send : nullptr); + C::mip_interface_set_recv_function(this, Recv != nullptr ? recv : nullptr); + + if( Update != nullptr ) + C::mip_interface_set_update_function(this, update); + else if( Recv != nullptr ) + C::mip_interface_set_update_function(this, &C::mip_interface_default_update); + else + C::mip_interface_set_update_function(this, nullptr); +} + + //////////////////////////////////////////////////////////////////////////////// ///@brief Registers a packet callback (free function version). /// diff --git a/src/mip/mip_interface.c b/src/mip/mip_interface.c index 98b8a8e3c..8a5e14f9a 100644 --- a/src/mip/mip_interface.c +++ b/src/mip/mip_interface.c @@ -9,21 +9,71 @@ #include + //////////////////////////////////////////////////////////////////////////////// -///@brief Wrapper around mip_interface_receive_packet for use with mip_parser. +///@typedef mip_send_callback /// -///@param device Void pointer to the device. Must be a mip_interface pointer. -///@param packet MIP Packet from the parser. -///@param timestamp timestamp_type of the packet. +///@copydoc mip_interface_send_to_device /// -///@returns True +///@note There are cases where the data will not be a MIP packet. /// -bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp) -{ - mip_interface_receive_packet(device, packet, timestamp); - return true; -} +//////////////////////////////////////////////////////////////////////////////// +///@typedef mip_recv_callback +/// +///@brief Receives new data from the device. Called repeatedly +/// by mip_interface_update() while waiting for command responses. +/// +///@param device The mip interface object +///@param buffer Buffer to fill with data. Should be allocated before +/// calling this function +///@param max_length Max number of bytes that can be read into the buffer. +///@param wait_time Time to wait for data from the device. The actual time +/// waited may be less than wait_time, but it should not +/// significantly exceed this value. +///@param out_length Number of bytes actually read into the buffer. +///@param timestamp_out Timestamp of the data was received. +/// +///@returns True if successful, even if no data is received. +///@returns False if the port cannot be read or some other error occurs. +/// +///@note Except in case of error (i.e. returning false), the timestamp must be +/// set even if no data is received. This is required to allow commands +/// to time out. +/// +///@note Applications may sleep the thread or enter a low-power state while +/// waiting for data. On posix-like (e.g. desktop) systems, applications +/// should call read() with a maximum timeout of wait_time. +/// If the actual wait time is less than the requested duration, this +/// function may be called again by the MIP SDK to wait the remaining time. +/// If the actual wait time exceeds wait_time, command timeouts may take +/// longer than intended. +/// + +//////////////////////////////////////////////////////////////////////////////// +///@typedef mip_update_callback +/// +///@brief Callback function typedef for custom update behavior. +/// +/// This function is called whenever data should be parsed from the port: +///@li While waiting for command responses +///@li To check for new data packets +/// +/// Generally an application should call mip_interface_recv_from_device() from +/// within this callback and pass the data to mip_interface_receive_bytes(). +/// Most applications can set this callback to mip_interface_default_update(). +/// +///@param device +/// The mip_interface object being updated. +///@param timeout +/// Amount of time to wait for data from the device. This will be zero +/// when checking for data and nonzero when waiting for commands. +/// +///@returns True if successful (even if no data is received). +///@returns False if an error occurs and the port cannot be read (e.g. if the +/// port is closed). Returning false will cause any pending commands to +/// fail with a status error code. +/// //////////////////////////////////////////////////////////////////////////////// ///@brief Initialize the mip_interface components. @@ -39,12 +89,19 @@ bool mip_interface_parse_callback(void* device, const mip_packet* packet, timest ///@param base_reply_timeout /// Minimum time for all commands. See mip_cmd_queue_init(). /// -void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, timeout_type parse_timeout, timeout_type base_reply_timeout) +void mip_interface_init( + mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, + timeout_type parse_timeout, timeout_type base_reply_timeout, + mip_send_callback send, mip_recv_callback recv, + mip_update_callback update, void* user_pointer) { mip_parser_init(&device->_parser, parse_buffer, parse_buffer_size, &mip_interface_parse_callback, device, parse_timeout); device->_max_update_pkts = MIPPARSER_UNLIMITED_PACKETS; - device->_update_function = &mip_interface_default_update; + device->_send_callback = send; + device->_recv_callback = recv; + device->_update_callback = update; + device->_user_pointer = user_pointer; mip_cmd_queue_init(&device->_queue, base_reply_timeout); @@ -52,6 +109,66 @@ void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t par } +//////////////////////////////////////////////////////////////////////////////// +// +// Accessors +// +//////////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the send callback function. +/// +///@param device +/// +///@param function +/// Function which sends raw bytes to the device. This can be NULL if no +/// commands will be issued (they would fail). +/// +void mip_interface_set_send_function(mip_interface* device, mip_send_callback callback) +{ + device->_send_callback = callback; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the send function pointer. +/// +///@param device +/// +///@returns The send callback function. May be NULL. +/// +mip_send_callback mip_interface_send_function(const mip_interface* device) +{ + return device->_send_callback; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the receive callback function. +/// +///@param device +/// +///@param function +/// Function which gets data from the device connection. +/// If this is NULL then commands will fail and no data will be received. +/// +void mip_interface_set_recv_function(mip_interface* device, mip_recv_callback callback) +{ + device->_recv_callback = callback; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the receive function pointer. +/// +///@param device +/// +///@returns The receive callback function. May be NULL. +/// +mip_recv_callback mip_interface_recv_function(const mip_interface* device) +{ + return device->_recv_callback; +} + //////////////////////////////////////////////////////////////////////////////// ///@brief Sets the update function. @@ -68,20 +185,21 @@ void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t par /// If this is NULL, then update calls will fail and no data or /// or command replies will be received. /// -void mip_interface_set_update_function(struct mip_interface* device, mip_update_callback function) +void mip_interface_set_update_function(mip_interface* device, mip_update_callback callback) { - device->_update_function = function; + device->_update_callback = callback; } + //////////////////////////////////////////////////////////////////////////////// ///@brief Gets the update function pointer. /// ///@returns The update function. Defaults to mip_interface_default_update. May /// be NULL. /// -mip_update_callback mip_interface_update_function(struct mip_interface* device) +mip_update_callback mip_interface_update_function(const mip_interface* device) { - return device->_update_function; + return device->_update_callback; } @@ -138,6 +256,66 @@ void mip_interface_set_max_packets_per_update(mip_interface* device, unsigned in } +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the MIP parser for the device. +/// +mip_parser* mip_interface_parser(mip_interface* device) +{ + return &device->_parser; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the commmand queue for the device. +/// +mip_cmd_queue* mip_interface_cmd_queue(mip_interface* device) +{ + return &device->_queue; +} + + +//////////////////////////////////////////////////////////////////////////////// +// +// Communications +// +//////////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sends data to the port (i.e. from this library to the physical device). +/// +///@param device The mip_interface object. +///@param data Data to be sent. +///@param length Length of data. +/// +///@returns True if the data was sent successfully. +///@returns False if the send callback is NULL. +///@returns False if some or all data could not be sent. +/// +/// This is called whenever bytes must be sent to the physical device. +/// +bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, size_t length) +{ + return device->_send_callback && device->_send_callback(device, data, length); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Checks for data at the port and reads it into buffer. +/// +///@param device +///@param buffer A place to store the data. +///@param max_length Maximum number of bytes to read into buffer. +///@param wait_time Maximum time to wait for data. May be 0. +///@param length_out The number of bytes successfully read into buffer. +///@param timestamp_out The timestamp of the received data. +/// +///@returns True if successful (even if 0 bytes were read). +///@returns False if the receive callback is NULL. +///@returns False if the receive callback failed (i.e. if it returned false). +/// +bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* length_out, timestamp_type* timestamp_out) +{ + return device->_recv_callback && device->_recv_callback(device, buffer, max_length, wait_time, length_out, timestamp_out); +} //////////////////////////////////////////////////////////////////////////////// ///@brief Call to process data from the device. @@ -145,42 +323,47 @@ void mip_interface_set_max_packets_per_update(mip_interface* device, unsigned in /// This function is also called while waiting for command replies. /// /// Call this periodically to process packets received from the device. It -/// should be called at a suitably-high rate to prevent the connection buffers +/// should be called at a suitably high rate to prevent the connection buffers /// from overflowing. The update rate affects the reception timestamp resolution. /// ///@param device /// -///@param blocking -/// This is true when called from within a blocking command function. -/// Applications should generally set this to false, e.g. when calling -/// this to process incoming data packets. +///@param wait_time +/// Time to wait for data from the device. This will be nonzero when +/// waiting for command replies. Applications calling this function +/// can pass 0 to avoid blocking when checking for new data. /// ///@returns true if operation should continue, or false if the device cannot be /// updated (e.g. if the serial port is not open). /// -bool mip_interface_update(struct mip_interface* device, bool blocking) +bool mip_interface_update(struct mip_interface* device, timeout_type wait_time) { - if( !device->_update_function ) + if( !device->_update_callback ) return false; - return device->_update_function(device, blocking); + return device->_update_callback(device, wait_time); } + //////////////////////////////////////////////////////////////////////////////// ///@brief Polls the port for new data or command replies. /// /// This is the default choice for the user update function. It ignores the /// blocking flag and always reads data from the device. /// -///@param device The mip_interface object. -///@param blocking Ignored. +///@param device +/// +///@param wait_time +/// Time to wait for data to be received. Passed directly to +/// mip_interface_recv_from_device(). /// ///@returns The value returned by mip_interface_user_recv_from_device. /// -bool mip_interface_default_update(struct mip_interface* device, bool blocking) +bool mip_interface_default_update(struct mip_interface* device, timeout_type wait_time) { - (void)blocking; + if( !device->_recv_callback ) + return false; uint8_t* ptr; mip_parser* parser = mip_interface_parser(device); @@ -188,37 +371,20 @@ bool mip_interface_default_update(struct mip_interface* device, bool blocking) size_t count = 0; timestamp_type timestamp = 0; - if ( !mip_interface_user_recv_from_device(device, ptr, max_count, &count, ×tamp) ) + if ( !mip_interface_recv_from_device(device, ptr, max_count, wait_time, &count, ×tamp) ) return false; assert(count <= max_count); mip_parser_process_written(parser, count, timestamp, 0); mip_cmd_queue_update(mip_interface_cmd_queue(device), timestamp); - return true; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Sends data to the port (i.e. from this library to the physical device). -/// -///@param device The mip_interface object. -///@param data Data to be sent. -///@param length Length of data. -/// -///@returns True if the data was sent successfully, false if some or all data -/// could not be sent. -/// -/// This is called whenever bytes must be sent to the physical device. -/// -bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, size_t length) -{ - return mip_interface_user_send_to_device(device, data, length); + return true; } //////////////////////////////////////////////////////////////////////////////// -///@brief Receive data from the port (i.e. the physical device) into the parser. +///@brief Passes data from the device into the parser. /// ///@param device /// @@ -237,7 +403,6 @@ remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t return mip_parser_parse(&device->_parser, data, length, timestamp, device->_max_update_pkts); } - //////////////////////////////////////////////////////////////////////////////// ///@brief Process more packets from the internal buffer. /// @@ -271,22 +436,28 @@ void mip_interface_receive_packet(mip_interface* device, const mip_packet* packe mip_dispatcher_dispatch_packet(&device->_dispatcher, packet, timestamp); } - //////////////////////////////////////////////////////////////////////////////// -///@brief Returns the MIP parser for the device. +///@brief Wrapper around mip_interface_receive_packet for use with mip_parser. /// -mip_parser* mip_interface_parser(mip_interface* device) +///@param device Void pointer to the device. Must be a mip_interface pointer. +///@param packet MIP Packet from the parser. +///@param timestamp timestamp_type of the packet. +/// +///@returns True +/// +bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp) { - return &device->_parser; + mip_interface_receive_packet(device, packet, timestamp); + + return true; } + +//////////////////////////////////////////////////////////////////////////////// +// +// Command and data processing +// //////////////////////////////////////////////////////////////////////////////// -///@brief Returns the commmand queue for the device. -/// -mip_cmd_queue* mip_interface_cmd_queue(mip_interface* device) -{ - return &device->_queue; -} //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/mip_interface.h b/src/mip/mip_interface.h index fafdb5f37..7784fbcc4 100644 --- a/src/mip/mip_interface.h +++ b/src/mip/mip_interface.h @@ -32,45 +32,46 @@ extern "C" { struct mip_interface; -//////////////////////////////////////////////////////////////////////////////// -///@brief Callback function typedef for custom update behavior. -/// -///@param device The mip_interface object being updated. -///@param blocking True if called from within a blocking command function. -/// -///@returns False if an error occurs and the port cannot be read (e.g. if the -/// port is closed). Returning false will cause any pending commands to -/// fail with a status error code. -///@returns True if successful (even if no data is received). -/// -typedef bool (*mip_update_callback)(struct mip_interface* device, bool blocking); +// Documentation is in source file. +typedef bool (*mip_send_callback)(struct mip_interface* device, const uint8_t* data, size_t length); +typedef bool (*mip_recv_callback)(struct mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +typedef bool (*mip_update_callback)(struct mip_interface* device, timeout_type timeout); + //////////////////////////////////////////////////////////////////////////////// ///@brief State of the interface for communicating with a MIP device. /// typedef struct mip_interface { - mip_parser _parser; ///<@private MIP Parser for incoming MIP packets. - mip_cmd_queue _queue; ///<@private Queue for checking command replies. - mip_dispatcher _dispatcher; ///<@private Dispatcher for data callbacks. - unsigned int _max_update_pkts; ///<@private Max number of MIP packets to parse at once. - mip_update_callback _update_function; ///<@private Optional function to call during updates. - void* _user_pointer; ///<@private Optional user-specified data pointer. + mip_parser _parser; ///<@private MIP Parser for incoming MIP packets. + mip_cmd_queue _queue; ///<@private Queue for checking command replies. + mip_dispatcher _dispatcher; ///<@private Dispatcher for data callbacks. + unsigned int _max_update_pkts; ///<@private Max number of MIP packets to parse at once. + mip_send_callback _send_callback; ///<@private Optional function which is called to send raw bytes to the device. + mip_recv_callback _recv_callback; ///<@private Optional function which is called to receive raw bytes from the device. + mip_update_callback _update_callback; ///<@private Optional function to call during updates. + void* _user_pointer; ///<@private Optional user-specified data pointer. } mip_interface; -void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, timeout_type parse_timeout, timeout_type base_reply_timeout); + +void mip_interface_init( + mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, + timeout_type parse_timeout, timeout_type base_reply_timeout, + mip_send_callback send, mip_recv_callback recv, + mip_update_callback update, void* user_pointer +); // // Communications // -remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, timestamp_type timestamp); -void mip_interface_process_unparsed_packets(mip_interface* device); -bool mip_interface_update(mip_interface* device, bool blocking); -bool mip_interface_default_update(mip_interface* device, bool blocking); - bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, size_t length); +bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type timeout, size_t* length_out, timestamp_type* now); +bool mip_interface_update(mip_interface* device, timeout_type wait_time); +bool mip_interface_default_update(mip_interface* device, timeout_type wait_time); +remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, timestamp_type timestamp); +void mip_interface_process_unparsed_packets(mip_interface* device); bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp); void mip_interface_receive_packet(mip_interface* device, const mip_packet* packet, timestamp_type timestamp); @@ -97,66 +98,24 @@ void mip_interface_register_extractor(mip_interface* device, mip_dispatch_handle // Accessors // +void mip_interface_set_recv_function(mip_interface* device, mip_recv_callback function); +void mip_interface_set_send_function(mip_interface* device, mip_send_callback function); void mip_interface_set_update_function(mip_interface* device, mip_update_callback function); void mip_interface_set_user_pointer(mip_interface* device, void* pointer); + void mip_interface_set_max_packets_per_update(mip_interface* device, unsigned int max_packets); unsigned int mip_interface_max_packets_per_update(const mip_interface* device); -mip_update_callback mip_interface_update_function(mip_interface* device); -void* mip_interface_user_pointer(const mip_interface* device); +mip_recv_callback mip_interface_recv_function(const mip_interface* device); +mip_send_callback mip_interface_send_function(const mip_interface* device); +mip_update_callback mip_interface_update_function(const mip_interface* device); +void* mip_interface_user_pointer(const mip_interface* device); + mip_parser* mip_interface_parser(mip_interface* device); mip_cmd_queue* mip_interface_cmd_queue(mip_interface* device); ///@} ///@} -//////////////////////////////////////////////////////////////////////////////// -///@defgroup user_callbacks User callback functions [C/CPP] -/// -///@{ - -//////////////////////////////////////////////////////////////////////////////// -///@brief Receives new data from the device. Called repeatedly -/// by mip_interface_update() while waiting for command responses. -/// -///@param device The mip interface object -///@param buffer Buffer to fill with data. Should be allocated before -/// calling this function -///@param max_length Max number of bytes that can be read into the buffer. -///@param out_length Number of bytes actually read into the buffer. -///@param timestamp_out Timestamp of the data was received. -/// -///@returns true if operation should continue, or false if the device cannot be -/// updated (e.g. if the serial port is not open). -/// -///@note Except in case of error (i.e. returning false), the timestamp must be -/// set even if no data is received. This is required to allow commands -/// to time out. -/// -///@note On systems where it makes sense, this is a good place to call sleep -/// or enter a low-power state until data arrives at the port. Typically -/// this function will wait a few milliseconds before returning. -/// -///@warning Do not block indefinitely as this will stall the system beyond the -/// normal command timeout. Use a sensible timeout (i.e. 1/10th of the -/// base reply timeout) or only sleep for a minimal amount of time. -/// -extern bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); - - -//////////////////////////////////////////////////////////////////////////////// -///@copydoc mip_interface_send_to_device -/// -///@note This is a good place to put logging code for debugging device -/// communications at the byte level. -/// -///@note There are cases where the data will not be a MIP packet. -/// -extern bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); - - -///@} -//////////////////////////////////////////////////////////////////////////////// - #ifdef __cplusplus } // namespace mip diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index f31722e06..3bc10794e 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -12,7 +12,7 @@ namespace platform ///@brief Creates a SerialConnection that will communicate with a device over serial /// ///@param portName Path to the port to connect to. On Windows, this usually looks like "COM", on linux, "/dev/tty" -///@param baudrate Baud rate to open the device at. Note that the device needs to be configured to +///@param baudrate Baud rate to open the device at. Note that the device needs to be configured to SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrate) { if (!serial_port_open(&mPort, portName.c_str(), baudrate)) @@ -26,10 +26,11 @@ SerialConnection::~SerialConnection() } ///@copydoc mip::Connection::recvFromDevice -bool SerialConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) +bool SerialConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) { - *timestamp = getCurrentTimestamp(); - return serial_port_read(&mPort, buffer, max_length, length_out); + *timestamp = std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + + return serial_port_read(&mPort, buffer, max_length, wait_time, length_out); } ///@copydoc mip::Connection::sendToDevice diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index 1e0e9f510..85adfc925 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -5,9 +5,6 @@ #include - -extern mip::Timestamp getCurrentTimestamp(); - namespace mip { namespace platform @@ -27,7 +24,7 @@ class SerialConnection : public mip::Connection SerialConnection(const std::string& portName, uint32_t baudrate); ~SerialConnection(); - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) final; + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; private: diff --git a/src/mip/platform/tcp_connection.cpp b/src/mip/platform/tcp_connection.cpp index 9d63a51fb..12780e4c2 100644 --- a/src/mip/platform/tcp_connection.cpp +++ b/src/mip/platform/tcp_connection.cpp @@ -27,9 +27,12 @@ TcpConnection::~TcpConnection() } ///@copydoc mip::Connection::sendToDevice -bool TcpConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) +bool TcpConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) { - *timestamp = getCurrentTimestamp(); + (void)wait_time; // Not used, timeout is always fixed + + *timestamp = std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + return tcp_socket_recv(&mSocket, buffer, max_length, length_out); } diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index 426cb40a4..85823bb01 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -6,9 +6,6 @@ #include - -extern mip::Timestamp getCurrentTimestamp(); - namespace mip { namespace platform @@ -28,7 +25,7 @@ class TcpConnection : public mip::Connection TcpConnection(const std::string& hostname, uint16_t port); ~TcpConnection(); - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) final; + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; private: diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 37c339ccb..bcd047c1f 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -63,7 +63,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Connect to the provided com port port->handle = CreateFile(port_str, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); - + //Check for an invalid handle if(port->handle == INVALID_HANDLE_VALUE) { @@ -74,7 +74,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Setup the com port buffer sizes if(SetupComm(port->handle, COM_PORT_BUFFER_SIZE, COM_PORT_BUFFER_SIZE) == 0) return false; - + //Set the timeouts COMMTIMEOUTS timeouts; GetCommTimeouts(port->handle, &timeouts); @@ -90,7 +90,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Setup the com port parameters ready = GetCommState(port->handle, &dcb); - + //Close the serial port, mutex, and exit if(!ready) { @@ -98,7 +98,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) return false; } - dcb.BaudRate = baudrate; //Baudrate is typically 115200 + dcb.BaudRate = baudrate; //Baudrate is typically 115200 dcb.ByteSize = 8; //Charsize is 8, default for MicroStrain dcb.Parity = NOPARITY; //Parity is none, default for MicroStrain dcb.StopBits = ONESTOPBIT; //Stopbits is 1, default for MicroStrain @@ -106,14 +106,14 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) dcb.fDtrControl = DTR_CONTROL_ENABLE; ready = SetCommState(port->handle, &dcb); - + //Close the serial port and exit if(!ready) { CloseHandle(port->handle); return false; } - + #else //Linux port->handle = open(port_str, O_RDWR | O_NOCTTY | O_SYNC); @@ -144,7 +144,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) // Persist the settings if(tcsetattr(port->handle, TCSANOW, &serial_port_settings) < 0) return false; - + // Flush any waiting data tcflush(port->handle, TCIOFLUSH); @@ -175,13 +175,13 @@ bool serial_port_close(serial_port *port) bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, size_t *bytes_written) { - + *bytes_written = 0; //Check for a valid port handle if(!port->is_open) return false; - + #ifdef WIN32 //Windows DWORD local_bytes_written; @@ -189,7 +189,7 @@ bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, if(WriteFile(port->handle, buffer, num_bytes, &local_bytes_written, NULL)) { *bytes_written = local_bytes_written; - + if(*bytes_written == num_bytes) return true; } @@ -199,23 +199,29 @@ bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, if(*bytes_written == num_bytes) return true; - + #endif return false; } -bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, size_t *bytes_read) +bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wait_time, size_t *bytes_read) { - //Set the bytes read to zero *bytes_read = 0; //Check for a valid port handle if(!port->is_open) return false; - + #ifdef WIN32 //Windows + + if( wait_time <= 0 ) + { + if( serial_port_read_count(port) == 0 ) + return true; + } + DWORD local_bytes_read; //Call the windows read function @@ -226,7 +232,7 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, size_t #else //Linux // Poll the device before attempting to read any data, so we will only block for 10ms if there is no data available struct pollfd poll_fd = { .fd = port->handle, .events = POLLIN }; - int poll_status = poll(&poll_fd, 1, 10); + int poll_status = poll(&poll_fd, 1, wait_time); // Keep reading and polling while there is still data available if (poll_status > 0 && poll_fd.revents & POLLIN) @@ -249,17 +255,17 @@ uint32_t serial_port_read_count(serial_port *port) //Check for a valid port handle if(!port->is_open) return 0; - + #ifdef WIN32 //Windows COMSTAT com_status; DWORD errors; - + //This function gets the current com status if(ClearCommError(port->handle, &errors, &com_status)) { return com_status.cbInQue; } - + #else //Linux int bytes_available; ioctl(port->handle, FIONREAD, &bytes_available); diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index a61a65ab9..8354974cd 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -46,7 +46,7 @@ typedef struct serial_port bool serial_port_open(serial_port *port, const char *port_str, int baudrate); bool serial_port_close(serial_port *port); bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, size_t *bytes_written); -bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, size_t *bytes_read); +bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wait_time, size_t *bytes_read); uint32_t serial_port_read_count(serial_port *port); bool serial_port_is_open(serial_port *port); From bde9e1e7087b5cd3606c2757814ec912ff707244 Mon Sep 17 00:00:00 2001 From: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Date: Thu, 3 Nov 2022 13:27:29 -0400 Subject: [PATCH 017/252] Update line endings (#38) --- CHANGELOG.md | 54 +-- LICENSE.txt | 40 +- src/mip/mip_cmdqueue.c | 924 ++++++++++++++++++++--------------------- 3 files changed, 509 insertions(+), 509 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6b32c8ac5..01aaf9dd7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,27 +1,27 @@ - -MIP SDK Change Log -================== - -The version number scheme for the MIP SDK is MAJOR.MINOR.PATCH. - -* The MAJOR number is incremented when breaking changes are made which are not backwards compatible. - This includes public API changes and especially behavioral changes. It is likely that existing code - will not work properly and/or may not compile without changes. -* The MINOR number is incremented when a new feature is added or a current feature is improved. - Minor revisions may incorporate bug fixes and other patches. -* The PATCH number is incremented when a bug is fixed or a small, non-breaking change is made. - Patches will not significantly affect the behavior of existing code, except where such behavior - is unintentional or erroneous. - -Major revisions will specify what caused the non-backwards compatible change. These will be specified like so: -CHANGED - A non-backwards compatible change was made to an existing function/class. -RENAMED - A function/class has been renamed. -REMOVED - A function/class has been removed. - -Forthcoming ------------ -* TBD - -v1.0.0 ------- -* Initial release of the MIP SDK + +MIP SDK Change Log +================== + +The version number scheme for the MIP SDK is MAJOR.MINOR.PATCH. + +* The MAJOR number is incremented when breaking changes are made which are not backwards compatible. + This includes public API changes and especially behavioral changes. It is likely that existing code + will not work properly and/or may not compile without changes. +* The MINOR number is incremented when a new feature is added or a current feature is improved. + Minor revisions may incorporate bug fixes and other patches. +* The PATCH number is incremented when a bug is fixed or a small, non-breaking change is made. + Patches will not significantly affect the behavior of existing code, except where such behavior + is unintentional or erroneous. + +Major revisions will specify what caused the non-backwards compatible change. These will be specified like so: +CHANGED - A non-backwards compatible change was made to an existing function/class. +RENAMED - A function/class has been renamed. +REMOVED - A function/class has been removed. + +Forthcoming +----------- +* TBD + +v1.0.0 +------ +* Initial release of the MIP SDK diff --git a/LICENSE.txt b/LICENSE.txt index c14e75f11..578555ed7 100644 --- a/LICENSE.txt +++ b/LICENSE.txt @@ -1,21 +1,21 @@ -The MIT License (MIT) - -Copyright(c) 2022 Parker Hannifin Corp. All rights reserved. - -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 +The MIT License (MIT) + +Copyright(c) 2022 Parker Hannifin Corp. All rights reserved. + +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. \ No newline at end of file diff --git a/src/mip/mip_cmdqueue.c b/src/mip/mip_cmdqueue.c index 62d2ce4dd..a2f97d0ab 100644 --- a/src/mip/mip_cmdqueue.c +++ b/src/mip/mip_cmdqueue.c @@ -1,462 +1,462 @@ - -#include "mip_cmdqueue.h" - -#include "mip_field.h" -#include "mip_packet.h" - -#include -#include - - -#define MIP_REPLY_DESC_GLOBAL_ACK_NACK 0xF1 - -#define MIP_INDEX_REPLY_DESCRIPTOR 0 -#define MIP_INDEX_REPLY_ACK_CODE 1 - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending command with no reponse data or additional time. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -/// -void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, 0); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with extra timeout time. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param additional_time -/// Additional time on top of the base reply timeout for this specific command. -/// -void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with expected response data. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param response_descriptor -/// Optional response data descriptor. Use 0x00 if no data is expected. -///@param response_buffer -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Size of the response buffer. The response will be limited to this size. -/// -void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, response_descriptor, response_buffer, response_buffer_size, 0); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with all parameters. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param response_descriptor -/// Optional response data descriptor. Use 0x00 if no data is expected. -///@param response_buffer -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Size of the response buffer. The response will be limited to this size. -///@param additional_time -/// Additional time on top of the base reply timeout for this specific command. -/// -void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time) -{ - cmd->_next = NULL; - cmd->_response_buffer = NULL; - cmd->_extra_timeout = additional_time; - cmd->_descriptor_set = descriptor_set; - cmd->_field_descriptor = field_descriptor; - cmd->_response_descriptor = response_descriptor; - cmd->_response_buffer = response_buffer; - cmd->_response_buffer_size = response_buffer_size; - // cmd->_ack_code = 0xFF; // invalid - cmd->_status = MIP_STATUS_NONE; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the status of the pending command. -/// -///@see mip_cmd_status -/// -enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd) -{ - return cmd->_status; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the response payload pointer. -/// -/// This function may only be called after the command finishes with an ACK. -/// -const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd) -{ - assert(mip_cmd_result_is_finished(cmd->_status)); - - return cmd->_response_buffer; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the length of the response data. -/// -/// This function may only be called after the command finishes. -/// If the command completed with a NACK, or if it timed out, the response -/// length will be zero. -/// -uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) -{ - assert(mip_cmd_result_is_finished(cmd->_status)); - - return cmd->_response_length; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines how much time is remaining before the command times out. -/// -/// For most cases you should instead call mip_pending_cmd_check_timeout() to -/// know if the command has timed out or not. -/// -///@param cmd The command to check. Must be in MIP_STATUS_WAITING state. -///@param now The current timestamp. -/// -///@warning The command must be in the MIP_STATUS_WAITING state, otherwise the -/// result is unspecified. -/// -///@returns The time remaining before the command times out. The value will be -/// negative if the timeout time has passed. -/// -int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now) -{ - assert(cmd->_status == MIP_STATUS_WAITING); - - // result <= 0 if timed out. - // Note: this still works with unsigned overflow. - return (int)(now - cmd->_timeout_time); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Checks if the command should time out. -/// -///@param cmd -///@param now Current time -/// -///@returns true if the command should time out. Only possible for MIP_STATUS_WAITING. -///@returns false if the command should not time out. -/// -bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now) -{ - if( cmd->_status == MIP_STATUS_WAITING ) - { - if( mip_pending_cmd_remaining_time(cmd, now) > 0 ) - { - return true; - } - } - - return false; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initializes a command queue. -/// -///@param queue -///@param base_reply_timeout -/// The minimum timeout given to all MIP commands. Additional time may be -/// given to specific commands which take longer. This is intended to be -/// used to accommodate long communication latencies, such as when using -/// a TCP connection. -/// -void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) -{ - queue->_first_pending_cmd = NULL; - queue->_base_timeout = base_reply_timeout; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Queue a command to wait for replies. -/// -///@param queue -///@param cmd Listens for replies to this command. -/// -///@warning The command must not be deallocated or go out of scope while the -/// mip_cmd_status_is_finished returns false. -/// -void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd) -{ - // For now only one command can be queued at a time. - if( queue->_first_pending_cmd ) - { - cmd->_status = MIP_STATUS_CANCELLED; - return; - } - - cmd->_status = MIP_STATUS_PENDING; - queue->_first_pending_cmd = cmd; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Removes a pending command from the queue. -/// -///@internal -/// -///@param queue -///@param cmd -/// -void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd) -{ - if( queue->_first_pending_cmd == cmd ) - { - queue->_first_pending_cmd = NULL; - cmd->_status = MIP_STATUS_CANCELLED; - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Iterate over a packet, checking for replies to the pending command. -/// -///@internal -/// -///@param pending -/// Pending command which is awaiting replies -///@param packet -///@param base_timeout -///@param timestamp -/// -///@returns The new status of the pending command (the command status field is -/// not updated). The caller should set pending->_status to this value -/// after doing any additional processing requiring the pending struct. -/// -static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp) -{ - assert( pending->_status != MIP_STATUS_NONE ); // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here. - assert( !mip_cmd_result_is_finished(pending->_status) ); // Command shouldn't be finished yet - make sure the queue is processed properly. - - if( pending->_status == MIP_STATUS_PENDING ) - { - // Update the timeout to the timestamp of the timeout time. - pending->_timeout_time = timestamp + base_timeout + pending->_extra_timeout; - pending->_status = MIP_STATUS_WAITING; - } - - // ------+------+------+------+------+------+------+------+------+------------------------ - // ... | 0x02 | 0xF1 | cmd1 | nack | 0x02 | 0xF1 | cmd2 | ack | response field ... - // ------+------+------+------+------+------+------+------+------+------------------------ - - if( mip_packet_descriptor_set(packet) == pending->_descriptor_set ) - { - mip_field field = {0}; - while( mip_field_next_in_packet(&field, packet) ) - { - // Not an ack/nack reply field, skip it. - if( mip_field_field_descriptor(&field) != MIP_REPLY_DESC_GLOBAL_ACK_NACK ) - continue; - - // Sanity check payload length before accessing it. - if( mip_field_payload_length(&field) != 2 ) - continue; - - const uint8_t* const payload = mip_field_payload(&field); - - const uint8_t cmd_descriptor = payload[MIP_INDEX_REPLY_DESCRIPTOR]; - const uint8_t ack_code = payload[MIP_INDEX_REPLY_ACK_CODE]; - - // Is this the right command reply? - if( pending->_field_descriptor != cmd_descriptor ) - continue; - - // Descriptor matches! - - uint8_t response_length = 0; - mip_field response_field; - - // If the command was ACK'd, check if response data is expected. - if( pending->_response_descriptor != 0x00 && ack_code == MIP_ACK_OK ) - { - // Look ahead one field for response data. - response_field = mip_field_next_after(&field); - if( mip_field_is_valid(&response_field) ) - { - const uint8_t response_descriptor = mip_field_field_descriptor(&response_field); - - // This is a wildcard to accept any response data descriptor. - // Needed when the response descriptor is not known or is wrong. - if( pending->_response_descriptor == MIP_REPLY_DESC_GLOBAL_ACK_NACK ) - pending->_response_descriptor = response_descriptor; - - // Make sure the response descriptor matches what is expected. - if( response_descriptor == pending->_response_descriptor ) - { - // Update the response_size field to reflect the actual size. - response_length = mip_field_payload_length(&response_field); - - // Skip this field when iterating for next ack/nack reply. - field = response_field; - } - } - } - - // Limit response data size to lesser of buffer size or actual response length. - pending->_response_length = (response_length < pending->_response_buffer_size) ? response_length : pending->_response_buffer_size; - - // Copy response data to the pending buffer (skip if response_field is invalid). - if( pending->_response_length > 0 ) - memcpy(pending->_response_buffer, mip_field_payload(&response_field), pending->_response_length); - - // pending->_ack_code = ack_code; - pending->_reply_time = timestamp; // Completion time - - return (enum mip_cmd_result)ack_code; - } - } - - // No matching reply descriptors in this packet. - - // Check for timeout - if( mip_pending_cmd_check_timeout(pending, timestamp) ) - { - pending->_response_length = 0; - // pending->_ack_code = MIP_NACK_COMMAND_TIMEOUT; - - // Must be last! - return MIP_STATUS_TIMEDOUT; - } - - return pending->_status; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Process an incoming packet and check for replies to pending commands. -/// -/// Call this from the Mip_parser callback, passing the arguments directly. -/// -///@param queue -///@param packet The received MIP packet. Assumed to be valid. -///@param timestamp The time the packet was received -/// -void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp) -{ - // Check if the packet is a command descriptor set. - const uint8_t descriptor_set = mip_packet_descriptor_set(packet); - if( descriptor_set >= 0x80 && descriptor_set < 0xF0 ) - return; - - if( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - - const enum mip_cmd_result status = process_fields_for_pending_cmd(pending, packet, queue->_base_timeout, timestamp); - - if( mip_cmd_result_is_finished(status) ) - { - queue->_first_pending_cmd = queue->_first_pending_cmd->_next; - - // This must be done last b/c it may trigger the thread which queued the command. - // The command could go out of scope or its attributes inspected. - pending->_status = status; - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Clears the command queue. -/// -/// This must be called from the same thread context as the update function. -/// -///@param queue -/// -void mip_cmd_queue_clear(mip_cmd_queue* queue) -{ - while( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - queue->_first_pending_cmd = pending->_next; - - // This may deallocate the pending command in another thread (make sure to fetch the next cmd first). - pending->_status = MIP_STATUS_ERROR; - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Call periodically to make sure commands time out if no packets are -/// received. -/// -/// Call this during the device update if no calls to mip_cmd_queue_process_packet -/// are made (e.g. because no packets were received). It is safe to call this -/// in either case. -/// -///@param queue -///@param now -/// -void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) -{ - if( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - - if( pending->_status == MIP_STATUS_PENDING ) - { - // Update the timeout to the timestamp of the timeout time. - pending->_timeout_time = now + queue->_base_timeout + pending->_extra_timeout; - pending->_status = MIP_STATUS_WAITING; - } - else if( mip_pending_cmd_check_timeout(pending, now) ) - { - queue->_first_pending_cmd = queue->_first_pending_cmd->_next; - - // Clear response length and mark when it timed out. - pending->_response_length = 0; - pending->_reply_time = now; - - // This must be last! - pending->_status = MIP_STATUS_TIMEDOUT; - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Sets the base reply timeout for all commands. -/// -/// THe base reply timeout is the minimum time to wait for a reply. -/// -///@param queue -///@param timeout -/// -void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout) -{ - queue->_base_timeout = timeout; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the base reply timeout for all commands. -/// -///@returns The minimum time to wait for a reply to any command. -/// -timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) -{ - return queue->_base_timeout; -} + +#include "mip_cmdqueue.h" + +#include "mip_field.h" +#include "mip_packet.h" + +#include +#include + + +#define MIP_REPLY_DESC_GLOBAL_ACK_NACK 0xF1 + +#define MIP_INDEX_REPLY_DESCRIPTOR 0 +#define MIP_INDEX_REPLY_ACK_CODE 1 + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending command with no reponse data or additional time. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +/// +void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, 0); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with extra timeout time. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param additional_time +/// Additional time on top of the base reply timeout for this specific command. +/// +void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with expected response data. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param response_descriptor +/// Optional response data descriptor. Use 0x00 if no data is expected. +///@param response_buffer +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Size of the response buffer. The response will be limited to this size. +/// +void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, response_descriptor, response_buffer, response_buffer_size, 0); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with all parameters. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param response_descriptor +/// Optional response data descriptor. Use 0x00 if no data is expected. +///@param response_buffer +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Size of the response buffer. The response will be limited to this size. +///@param additional_time +/// Additional time on top of the base reply timeout for this specific command. +/// +void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time) +{ + cmd->_next = NULL; + cmd->_response_buffer = NULL; + cmd->_extra_timeout = additional_time; + cmd->_descriptor_set = descriptor_set; + cmd->_field_descriptor = field_descriptor; + cmd->_response_descriptor = response_descriptor; + cmd->_response_buffer = response_buffer; + cmd->_response_buffer_size = response_buffer_size; + // cmd->_ack_code = 0xFF; // invalid + cmd->_status = MIP_STATUS_NONE; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the status of the pending command. +/// +///@see mip_cmd_status +/// +enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd) +{ + return cmd->_status; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the response payload pointer. +/// +/// This function may only be called after the command finishes with an ACK. +/// +const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd) +{ + assert(mip_cmd_result_is_finished(cmd->_status)); + + return cmd->_response_buffer; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the length of the response data. +/// +/// This function may only be called after the command finishes. +/// If the command completed with a NACK, or if it timed out, the response +/// length will be zero. +/// +uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) +{ + assert(mip_cmd_result_is_finished(cmd->_status)); + + return cmd->_response_length; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines how much time is remaining before the command times out. +/// +/// For most cases you should instead call mip_pending_cmd_check_timeout() to +/// know if the command has timed out or not. +/// +///@param cmd The command to check. Must be in MIP_STATUS_WAITING state. +///@param now The current timestamp. +/// +///@warning The command must be in the MIP_STATUS_WAITING state, otherwise the +/// result is unspecified. +/// +///@returns The time remaining before the command times out. The value will be +/// negative if the timeout time has passed. +/// +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now) +{ + assert(cmd->_status == MIP_STATUS_WAITING); + + // result <= 0 if timed out. + // Note: this still works with unsigned overflow. + return (int)(now - cmd->_timeout_time); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Checks if the command should time out. +/// +///@param cmd +///@param now Current time +/// +///@returns true if the command should time out. Only possible for MIP_STATUS_WAITING. +///@returns false if the command should not time out. +/// +bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now) +{ + if( cmd->_status == MIP_STATUS_WAITING ) + { + if( mip_pending_cmd_remaining_time(cmd, now) > 0 ) + { + return true; + } + } + + return false; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initializes a command queue. +/// +///@param queue +///@param base_reply_timeout +/// The minimum timeout given to all MIP commands. Additional time may be +/// given to specific commands which take longer. This is intended to be +/// used to accommodate long communication latencies, such as when using +/// a TCP connection. +/// +void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) +{ + queue->_first_pending_cmd = NULL; + queue->_base_timeout = base_reply_timeout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Queue a command to wait for replies. +/// +///@param queue +///@param cmd Listens for replies to this command. +/// +///@warning The command must not be deallocated or go out of scope while the +/// mip_cmd_status_is_finished returns false. +/// +void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd) +{ + // For now only one command can be queued at a time. + if( queue->_first_pending_cmd ) + { + cmd->_status = MIP_STATUS_CANCELLED; + return; + } + + cmd->_status = MIP_STATUS_PENDING; + queue->_first_pending_cmd = cmd; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Removes a pending command from the queue. +/// +///@internal +/// +///@param queue +///@param cmd +/// +void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd) +{ + if( queue->_first_pending_cmd == cmd ) + { + queue->_first_pending_cmd = NULL; + cmd->_status = MIP_STATUS_CANCELLED; + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Iterate over a packet, checking for replies to the pending command. +/// +///@internal +/// +///@param pending +/// Pending command which is awaiting replies +///@param packet +///@param base_timeout +///@param timestamp +/// +///@returns The new status of the pending command (the command status field is +/// not updated). The caller should set pending->_status to this value +/// after doing any additional processing requiring the pending struct. +/// +static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp) +{ + assert( pending->_status != MIP_STATUS_NONE ); // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here. + assert( !mip_cmd_result_is_finished(pending->_status) ); // Command shouldn't be finished yet - make sure the queue is processed properly. + + if( pending->_status == MIP_STATUS_PENDING ) + { + // Update the timeout to the timestamp of the timeout time. + pending->_timeout_time = timestamp + base_timeout + pending->_extra_timeout; + pending->_status = MIP_STATUS_WAITING; + } + + // ------+------+------+------+------+------+------+------+------+------------------------ + // ... | 0x02 | 0xF1 | cmd1 | nack | 0x02 | 0xF1 | cmd2 | ack | response field ... + // ------+------+------+------+------+------+------+------+------+------------------------ + + if( mip_packet_descriptor_set(packet) == pending->_descriptor_set ) + { + mip_field field = {0}; + while( mip_field_next_in_packet(&field, packet) ) + { + // Not an ack/nack reply field, skip it. + if( mip_field_field_descriptor(&field) != MIP_REPLY_DESC_GLOBAL_ACK_NACK ) + continue; + + // Sanity check payload length before accessing it. + if( mip_field_payload_length(&field) != 2 ) + continue; + + const uint8_t* const payload = mip_field_payload(&field); + + const uint8_t cmd_descriptor = payload[MIP_INDEX_REPLY_DESCRIPTOR]; + const uint8_t ack_code = payload[MIP_INDEX_REPLY_ACK_CODE]; + + // Is this the right command reply? + if( pending->_field_descriptor != cmd_descriptor ) + continue; + + // Descriptor matches! + + uint8_t response_length = 0; + mip_field response_field; + + // If the command was ACK'd, check if response data is expected. + if( pending->_response_descriptor != 0x00 && ack_code == MIP_ACK_OK ) + { + // Look ahead one field for response data. + response_field = mip_field_next_after(&field); + if( mip_field_is_valid(&response_field) ) + { + const uint8_t response_descriptor = mip_field_field_descriptor(&response_field); + + // This is a wildcard to accept any response data descriptor. + // Needed when the response descriptor is not known or is wrong. + if( pending->_response_descriptor == MIP_REPLY_DESC_GLOBAL_ACK_NACK ) + pending->_response_descriptor = response_descriptor; + + // Make sure the response descriptor matches what is expected. + if( response_descriptor == pending->_response_descriptor ) + { + // Update the response_size field to reflect the actual size. + response_length = mip_field_payload_length(&response_field); + + // Skip this field when iterating for next ack/nack reply. + field = response_field; + } + } + } + + // Limit response data size to lesser of buffer size or actual response length. + pending->_response_length = (response_length < pending->_response_buffer_size) ? response_length : pending->_response_buffer_size; + + // Copy response data to the pending buffer (skip if response_field is invalid). + if( pending->_response_length > 0 ) + memcpy(pending->_response_buffer, mip_field_payload(&response_field), pending->_response_length); + + // pending->_ack_code = ack_code; + pending->_reply_time = timestamp; // Completion time + + return (enum mip_cmd_result)ack_code; + } + } + + // No matching reply descriptors in this packet. + + // Check for timeout + if( mip_pending_cmd_check_timeout(pending, timestamp) ) + { + pending->_response_length = 0; + // pending->_ack_code = MIP_NACK_COMMAND_TIMEOUT; + + // Must be last! + return MIP_STATUS_TIMEDOUT; + } + + return pending->_status; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Process an incoming packet and check for replies to pending commands. +/// +/// Call this from the Mip_parser callback, passing the arguments directly. +/// +///@param queue +///@param packet The received MIP packet. Assumed to be valid. +///@param timestamp The time the packet was received +/// +void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp) +{ + // Check if the packet is a command descriptor set. + const uint8_t descriptor_set = mip_packet_descriptor_set(packet); + if( descriptor_set >= 0x80 && descriptor_set < 0xF0 ) + return; + + if( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + + const enum mip_cmd_result status = process_fields_for_pending_cmd(pending, packet, queue->_base_timeout, timestamp); + + if( mip_cmd_result_is_finished(status) ) + { + queue->_first_pending_cmd = queue->_first_pending_cmd->_next; + + // This must be done last b/c it may trigger the thread which queued the command. + // The command could go out of scope or its attributes inspected. + pending->_status = status; + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Clears the command queue. +/// +/// This must be called from the same thread context as the update function. +/// +///@param queue +/// +void mip_cmd_queue_clear(mip_cmd_queue* queue) +{ + while( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + queue->_first_pending_cmd = pending->_next; + + // This may deallocate the pending command in another thread (make sure to fetch the next cmd first). + pending->_status = MIP_STATUS_ERROR; + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Call periodically to make sure commands time out if no packets are +/// received. +/// +/// Call this during the device update if no calls to mip_cmd_queue_process_packet +/// are made (e.g. because no packets were received). It is safe to call this +/// in either case. +/// +///@param queue +///@param now +/// +void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) +{ + if( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + + if( pending->_status == MIP_STATUS_PENDING ) + { + // Update the timeout to the timestamp of the timeout time. + pending->_timeout_time = now + queue->_base_timeout + pending->_extra_timeout; + pending->_status = MIP_STATUS_WAITING; + } + else if( mip_pending_cmd_check_timeout(pending, now) ) + { + queue->_first_pending_cmd = queue->_first_pending_cmd->_next; + + // Clear response length and mark when it timed out. + pending->_response_length = 0; + pending->_reply_time = now; + + // This must be last! + pending->_status = MIP_STATUS_TIMEDOUT; + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the base reply timeout for all commands. +/// +/// THe base reply timeout is the minimum time to wait for a reply. +/// +///@param queue +///@param timeout +/// +void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout) +{ + queue->_base_timeout = timeout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the base reply timeout for all commands. +/// +///@returns The minimum time to wait for a reply to any command. +/// +timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) +{ + return queue->_base_timeout; +} From ac5f17a2bf44f92abd29c76d7e2a0e9acb852199 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 3 Nov 2022 13:30:50 -0400 Subject: [PATCH 018/252] Merge develop into definitions/dev (#39) * Generate MIP definitions from branches/dev@53813. (#37) Co-authored-by: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> * Change extern C functions to callbacks for use in dynamic libraries (#34) * Add remaining_time accessor to pending commands. * Refactor mip_interface and MipDevice: - Use callbacks. - Add wait_time parameter to recv_from_device. * Update serial and TCP connections to include wait_time parameter. * Add nonmember callback setters to DeviceInterface. * Update recording connection. * Update examples. * Fix serial port build on Windows. * Removes extern timestamp function from connections Co-authored-by: Rob Fisher * Update line endings (#38) Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> Co-authored-by: Rob Fisher Co-authored-by: Nick DaCosta <25206843+dacuster@users.noreply.github.com> --- CHANGELOG.md | 54 +- LICENSE.txt | 40 +- examples/CV7/CV7_example.c | 11 +- examples/GQ7/GQ7_example.c | 12 +- examples/GX5_45/GX5_45_example.c | 12 +- examples/threading.cpp | 8 +- examples/watch_imu.c | 10 +- src/mip/extras/recording_connection.cpp | 4 +- src/mip/extras/recording_connection.hpp | 2 +- src/mip/mip_cmdqueue.c | 901 ++++++++++++------------ src/mip/mip_cmdqueue.h | 1 + src/mip/mip_device.cpp | 51 +- src/mip/mip_device.hpp | 277 +++++++- src/mip/mip_interface.c | 289 ++++++-- src/mip/mip_interface.h | 107 +-- src/mip/platform/serial_connection.cpp | 9 +- src/mip/platform/serial_connection.hpp | 5 +- src/mip/platform/tcp_connection.cpp | 7 +- src/mip/platform/tcp_connection.hpp | 5 +- src/mip/utils/serial_port.c | 42 +- src/mip/utils/serial_port.h | 2 +- 21 files changed, 1126 insertions(+), 723 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6b32c8ac5..01aaf9dd7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,27 +1,27 @@ - -MIP SDK Change Log -================== - -The version number scheme for the MIP SDK is MAJOR.MINOR.PATCH. - -* The MAJOR number is incremented when breaking changes are made which are not backwards compatible. - This includes public API changes and especially behavioral changes. It is likely that existing code - will not work properly and/or may not compile without changes. -* The MINOR number is incremented when a new feature is added or a current feature is improved. - Minor revisions may incorporate bug fixes and other patches. -* The PATCH number is incremented when a bug is fixed or a small, non-breaking change is made. - Patches will not significantly affect the behavior of existing code, except where such behavior - is unintentional or erroneous. - -Major revisions will specify what caused the non-backwards compatible change. These will be specified like so: -CHANGED - A non-backwards compatible change was made to an existing function/class. -RENAMED - A function/class has been renamed. -REMOVED - A function/class has been removed. - -Forthcoming ------------ -* TBD - -v1.0.0 ------- -* Initial release of the MIP SDK + +MIP SDK Change Log +================== + +The version number scheme for the MIP SDK is MAJOR.MINOR.PATCH. + +* The MAJOR number is incremented when breaking changes are made which are not backwards compatible. + This includes public API changes and especially behavioral changes. It is likely that existing code + will not work properly and/or may not compile without changes. +* The MINOR number is incremented when a new feature is added or a current feature is improved. + Minor revisions may incorporate bug fixes and other patches. +* The PATCH number is incremented when a bug is fixed or a small, non-breaking change is made. + Patches will not significantly affect the behavior of existing code, except where such behavior + is unintentional or erroneous. + +Major revisions will specify what caused the non-backwards compatible change. These will be specified like so: +CHANGED - A non-backwards compatible change was made to an existing function/class. +RENAMED - A function/class has been renamed. +REMOVED - A function/class has been removed. + +Forthcoming +----------- +* TBD + +v1.0.0 +------ +* Initial release of the MIP SDK diff --git a/LICENSE.txt b/LICENSE.txt index c14e75f11..578555ed7 100644 --- a/LICENSE.txt +++ b/LICENSE.txt @@ -1,21 +1,21 @@ -The MIT License (MIT) - -Copyright(c) 2022 Parker Hannifin Corp. All rights reserved. - -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 +The MIT License (MIT) + +Copyright(c) 2022 Parker Hannifin Corp. All rights reserved. + +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. \ No newline at end of file diff --git a/examples/CV7/CV7_example.c b/examples/CV7/CV7_example.c index 30366d804..a36465a5c 100644 --- a/examples/CV7/CV7_example.c +++ b/examples/CV7/CV7_example.c @@ -73,7 +73,7 @@ const uint8_t FILTER_PITCH_EVENT_ACTION_ID = 2; //Required MIP interface user-defined functions timestamp_type get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -125,7 +125,10 @@ int main(int argc, const char* argv[]) //Initialize the MIP interface // - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); // @@ -383,10 +386,10 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, out_length); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); } diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index 9d0f8e01f..f56765a5a 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -80,7 +80,7 @@ bool filter_state_full_nav = false; //Required MIP interface user-defined functions timestamp_type get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -130,7 +130,11 @@ int main(int argc, const char* argv[]) //Initialize the MIP interface // - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + // @@ -393,10 +397,10 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, out_length); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); } diff --git a/examples/GX5_45/GX5_45_example.c b/examples/GX5_45/GX5_45_example.c index c10fd20f2..745f3b645 100644 --- a/examples/GX5_45/GX5_45_example.c +++ b/examples/GX5_45/GX5_45_example.c @@ -79,7 +79,7 @@ bool filter_state_running = false; //Required MIP interface user-defined functions timestamp_type get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -129,7 +129,11 @@ int main(int argc, const char* argv[]) //Initialize the MIP interface // - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + // @@ -362,10 +366,10 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, out_length); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); } diff --git a/examples/threading.cpp b/examples/threading.cpp index 46cf29662..21b4a178d 100644 --- a/examples/threading.cpp +++ b/examples/threading.cpp @@ -50,7 +50,7 @@ void device_thread_loop(mip::DeviceInterface* device) { while(!stop) { - if( !device->update(false) ) + if( !device->update(0) ) { device->cmdQueue().clear(); // Avoid deadlocks if the socket is closed. break; @@ -60,10 +60,10 @@ void device_thread_loop(mip::DeviceInterface* device) } } -bool update_device(mip::DeviceInterface& device, bool blocking) +bool update_device(mip::DeviceInterface& device, mip::Timeout wait_time) { - if( !blocking ) - return device.defaultUpdate(blocking); + if( wait_time > 0 ) + return device.defaultUpdate(wait_time); // Optionally display progress while waiting for command replies. // Displaying it here makes it update more frequently. diff --git a/examples/watch_imu.c b/examples/watch_imu.c index 937b6a53b..4c6d88a5f 100644 --- a/examples/watch_imu.c +++ b/examples/watch_imu.c @@ -95,12 +95,12 @@ timestamp_type get_current_timestamp() } -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) { (void)device; *timestamp_out = get_current_timestamp(); - if( !serial_port_read(&port, buffer, max_length, out_length) ) + if( !serial_port_read(&port, buffer, max_length, wait_time, out_length) ) return false; return true; @@ -142,7 +142,11 @@ int main(int argc, const char* argv[]) if( !open_port(argv[1], baudrate) ) return 1; - mip_interface_init(&device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000); + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + // Record program start time for use with difftime in getTimestamp(). time(&startTime); diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp index 82bd86208..725bdfbf6 100644 --- a/src/mip/extras/recording_connection.cpp +++ b/src/mip/extras/recording_connection.cpp @@ -25,9 +25,9 @@ bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) } ///@copydoc mip::Connection::recvFromDevice -bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* count_out, Timestamp* timestamp_out) +bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* count_out, Timestamp* timestamp_out) { - const bool ok = mConnection->recvFromDevice(buffer, max_length, count_out, timestamp_out); + const bool ok = mConnection->recvFromDevice(buffer, max_length, wait_time, count_out, timestamp_out); if( ok && mRecvFile != nullptr ) mRecvFile->write(reinterpret_cast(buffer), *count_out); return ok; diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index d05ea8069..7446d8324 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -24,7 +24,7 @@ class RecordingConnection : public Connection RecordingConnection(Connection* connection, std::ostream* recvStream=nullptr, std::ostream* sendStream=nullptr); bool sendToDevice(const uint8_t* data, size_t length) final; - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* count_out, Timestamp* timestamp_out) final; + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) final; protected: Connection* mConnection; diff --git a/src/mip/mip_cmdqueue.c b/src/mip/mip_cmdqueue.c index dbb6cdc5e..a2f97d0ab 100644 --- a/src/mip/mip_cmdqueue.c +++ b/src/mip/mip_cmdqueue.c @@ -1,439 +1,462 @@ - -#include "mip_cmdqueue.h" - -#include "mip_field.h" -#include "mip_packet.h" - -#include -#include - - -#define MIP_REPLY_DESC_GLOBAL_ACK_NACK 0xF1 - -#define MIP_INDEX_REPLY_DESCRIPTOR 0 -#define MIP_INDEX_REPLY_ACK_CODE 1 - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending command with no reponse data or additional time. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -/// -void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, 0); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with extra timeout time. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param additional_time -/// Additional time on top of the base reply timeout for this specific command. -/// -void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with expected response data. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param response_descriptor -/// Optional response data descriptor. Use 0x00 if no data is expected. -///@param response_buffer -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Size of the response buffer. The response will be limited to this size. -/// -void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, response_descriptor, response_buffer, response_buffer_size, 0); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with all parameters. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param response_descriptor -/// Optional response data descriptor. Use 0x00 if no data is expected. -///@param response_buffer -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Size of the response buffer. The response will be limited to this size. -///@param additional_time -/// Additional time on top of the base reply timeout for this specific command. -/// -void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time) -{ - cmd->_next = NULL; - cmd->_response_buffer = NULL; - cmd->_extra_timeout = additional_time; - cmd->_descriptor_set = descriptor_set; - cmd->_field_descriptor = field_descriptor; - cmd->_response_descriptor = response_descriptor; - cmd->_response_buffer = response_buffer; - cmd->_response_buffer_size = response_buffer_size; - // cmd->_ack_code = 0xFF; // invalid - cmd->_status = MIP_STATUS_NONE; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the status of the pending command. -/// -///@see mip_cmd_status -/// -enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd) -{ - return cmd->_status; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the response payload pointer. -/// -/// This function may only be called after the command finishes with an ACK. -/// -const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd) -{ - assert(mip_cmd_result_is_finished(cmd->_status)); - - return cmd->_response_buffer; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the length of the response data. -/// -/// This function may only be called after the command finishes. -/// If the command completed with a NACK, or if it timed out, the response -/// length will be zero. -/// -uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) -{ - assert(mip_cmd_result_is_finished(cmd->_status)); - - return cmd->_response_length; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Checks if the command should time out. -/// -///@param cmd -///@param now Current time -/// -///@returns true if the command should time out. Only possible for MIP_STATUS_WAITING. -///@returns false if the command should not time out. -/// -bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now) -{ - if( cmd->_status == MIP_STATUS_WAITING ) - { - if( (int)(now - cmd->_timeout_time) > 0 ) - { - return true; - } - } - - return false; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initializes a command queue. -/// -///@param queue -///@param base_reply_timeout -/// The minimum timeout given to all MIP commands. Additional time may be -/// given to specific commands which take longer. This is intended to be -/// used to accommodate long communication latencies, such as when using -/// a TCP connection. -/// -void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) -{ - queue->_first_pending_cmd = NULL; - queue->_base_timeout = base_reply_timeout; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Queue a command to wait for replies. -/// -///@param queue -///@param cmd Listens for replies to this command. -/// -///@warning The command must not be deallocated or go out of scope while the -/// mip_cmd_status_is_finished returns false. -/// -void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd) -{ - // For now only one command can be queued at a time. - if( queue->_first_pending_cmd ) - { - cmd->_status = MIP_STATUS_CANCELLED; - return; - } - - cmd->_status = MIP_STATUS_PENDING; - queue->_first_pending_cmd = cmd; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Removes a pending command from the queue. -/// -///@internal -/// -///@param queue -///@param cmd -/// -void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd) -{ - if( queue->_first_pending_cmd == cmd ) - { - queue->_first_pending_cmd = NULL; - cmd->_status = MIP_STATUS_CANCELLED; - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Iterate over a packet, checking for replies to the pending command. -/// -///@internal -/// -///@param pending -/// Pending command which is awaiting replies -///@param packet -///@param base_timeout -///@param timestamp -/// -///@returns The new status of the pending command (the command status field is -/// not updated). The caller should set pending->_status to this value -/// after doing any additional processing requiring the pending struct. -/// -static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp) -{ - assert( pending->_status != MIP_STATUS_NONE ); // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here. - assert( !mip_cmd_result_is_finished(pending->_status) ); // Command shouldn't be finished yet - make sure the queue is processed properly. - - if( pending->_status == MIP_STATUS_PENDING ) - { - // Update the timeout to the timestamp of the timeout time. - pending->_timeout_time = timestamp + base_timeout + pending->_extra_timeout; - pending->_status = MIP_STATUS_WAITING; - } - - // ------+------+------+------+------+------+------+------+------+------------------------ - // ... | 0x02 | 0xF1 | cmd1 | nack | 0x02 | 0xF1 | cmd2 | ack | response field ... - // ------+------+------+------+------+------+------+------+------+------------------------ - - if( mip_packet_descriptor_set(packet) == pending->_descriptor_set ) - { - mip_field field = {0}; - while( mip_field_next_in_packet(&field, packet) ) - { - // Not an ack/nack reply field, skip it. - if( mip_field_field_descriptor(&field) != MIP_REPLY_DESC_GLOBAL_ACK_NACK ) - continue; - - // Sanity check payload length before accessing it. - if( mip_field_payload_length(&field) != 2 ) - continue; - - const uint8_t* const payload = mip_field_payload(&field); - - const uint8_t cmd_descriptor = payload[MIP_INDEX_REPLY_DESCRIPTOR]; - const uint8_t ack_code = payload[MIP_INDEX_REPLY_ACK_CODE]; - - // Is this the right command reply? - if( pending->_field_descriptor != cmd_descriptor ) - continue; - - // Descriptor matches! - - uint8_t response_length = 0; - mip_field response_field; - - // If the command was ACK'd, check if response data is expected. - if( pending->_response_descriptor != 0x00 && ack_code == MIP_ACK_OK ) - { - // Look ahead one field for response data. - response_field = mip_field_next_after(&field); - if( mip_field_is_valid(&response_field) ) - { - const uint8_t response_descriptor = mip_field_field_descriptor(&response_field); - - // This is a wildcard to accept any response data descriptor. - // Needed when the response descriptor is not known or is wrong. - if( pending->_response_descriptor == MIP_REPLY_DESC_GLOBAL_ACK_NACK ) - pending->_response_descriptor = response_descriptor; - - // Make sure the response descriptor matches what is expected. - if( response_descriptor == pending->_response_descriptor ) - { - // Update the response_size field to reflect the actual size. - response_length = mip_field_payload_length(&response_field); - - // Skip this field when iterating for next ack/nack reply. - field = response_field; - } - } - } - - // Limit response data size to lesser of buffer size or actual response length. - pending->_response_length = (response_length < pending->_response_buffer_size) ? response_length : pending->_response_buffer_size; - - // Copy response data to the pending buffer (skip if response_field is invalid). - if( pending->_response_length > 0 ) - memcpy(pending->_response_buffer, mip_field_payload(&response_field), pending->_response_length); - - // pending->_ack_code = ack_code; - pending->_reply_time = timestamp; // Completion time - - return (enum mip_cmd_result)ack_code; - } - } - - // No matching reply descriptors in this packet. - - // Check for timeout - if( mip_pending_cmd_check_timeout(pending, timestamp) ) - { - pending->_response_length = 0; - // pending->_ack_code = MIP_NACK_COMMAND_TIMEOUT; - - // Must be last! - return MIP_STATUS_TIMEDOUT; - } - - return pending->_status; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Process an incoming packet and check for replies to pending commands. -/// -/// Call this from the Mip_parser callback, passing the arguments directly. -/// -///@param queue -///@param packet The received MIP packet. Assumed to be valid. -///@param timestamp The time the packet was received -/// -void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp) -{ - // Check if the packet is a command descriptor set. - const uint8_t descriptor_set = mip_packet_descriptor_set(packet); - if( descriptor_set >= 0x80 && descriptor_set < 0xF0 ) - return; - - if( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - - const enum mip_cmd_result status = process_fields_for_pending_cmd(pending, packet, queue->_base_timeout, timestamp); - - if( mip_cmd_result_is_finished(status) ) - { - queue->_first_pending_cmd = queue->_first_pending_cmd->_next; - - // This must be done last b/c it may trigger the thread which queued the command. - // The command could go out of scope or its attributes inspected. - pending->_status = status; - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Clears the command queue. -/// -/// This must be called from the same thread context as the update function. -/// -///@param queue -/// -void mip_cmd_queue_clear(mip_cmd_queue* queue) -{ - while( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - queue->_first_pending_cmd = pending->_next; - - // This may deallocate the pending command in another thread (make sure to fetch the next cmd first). - pending->_status = MIP_STATUS_ERROR; - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Call periodically to make sure commands time out if no packets are -/// received. -/// -/// Call this during the device update if no calls to mip_cmd_queue_process_packet -/// are made (e.g. because no packets were received). It is safe to call this -/// in either case. -/// -///@param queue -///@param now -/// -void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) -{ - if( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - - if( pending->_status == MIP_STATUS_PENDING ) - { - // Update the timeout to the timestamp of the timeout time. - pending->_timeout_time = now + queue->_base_timeout + pending->_extra_timeout; - pending->_status = MIP_STATUS_WAITING; - } - else if( mip_pending_cmd_check_timeout(pending, now) ) - { - queue->_first_pending_cmd = queue->_first_pending_cmd->_next; - - // Clear response length and mark when it timed out. - pending->_response_length = 0; - pending->_reply_time = now; - - // This must be last! - pending->_status = MIP_STATUS_TIMEDOUT; - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Sets the base reply timeout for all commands. -/// -/// THe base reply timeout is the minimum time to wait for a reply. -/// -///@param queue -///@param timeout -/// -void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout) -{ - queue->_base_timeout = timeout; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the base reply timeout for all commands. -/// -///@returns The minimum time to wait for a reply to any command. -/// -timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) -{ - return queue->_base_timeout; -} + +#include "mip_cmdqueue.h" + +#include "mip_field.h" +#include "mip_packet.h" + +#include +#include + + +#define MIP_REPLY_DESC_GLOBAL_ACK_NACK 0xF1 + +#define MIP_INDEX_REPLY_DESCRIPTOR 0 +#define MIP_INDEX_REPLY_ACK_CODE 1 + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending command with no reponse data or additional time. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +/// +void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, 0); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with extra timeout time. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param additional_time +/// Additional time on top of the base reply timeout for this specific command. +/// +void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with expected response data. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param response_descriptor +/// Optional response data descriptor. Use 0x00 if no data is expected. +///@param response_buffer +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Size of the response buffer. The response will be limited to this size. +/// +void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, response_descriptor, response_buffer, response_buffer_size, 0); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with all parameters. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param response_descriptor +/// Optional response data descriptor. Use 0x00 if no data is expected. +///@param response_buffer +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Size of the response buffer. The response will be limited to this size. +///@param additional_time +/// Additional time on top of the base reply timeout for this specific command. +/// +void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time) +{ + cmd->_next = NULL; + cmd->_response_buffer = NULL; + cmd->_extra_timeout = additional_time; + cmd->_descriptor_set = descriptor_set; + cmd->_field_descriptor = field_descriptor; + cmd->_response_descriptor = response_descriptor; + cmd->_response_buffer = response_buffer; + cmd->_response_buffer_size = response_buffer_size; + // cmd->_ack_code = 0xFF; // invalid + cmd->_status = MIP_STATUS_NONE; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the status of the pending command. +/// +///@see mip_cmd_status +/// +enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd) +{ + return cmd->_status; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the response payload pointer. +/// +/// This function may only be called after the command finishes with an ACK. +/// +const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd) +{ + assert(mip_cmd_result_is_finished(cmd->_status)); + + return cmd->_response_buffer; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the length of the response data. +/// +/// This function may only be called after the command finishes. +/// If the command completed with a NACK, or if it timed out, the response +/// length will be zero. +/// +uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) +{ + assert(mip_cmd_result_is_finished(cmd->_status)); + + return cmd->_response_length; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines how much time is remaining before the command times out. +/// +/// For most cases you should instead call mip_pending_cmd_check_timeout() to +/// know if the command has timed out or not. +/// +///@param cmd The command to check. Must be in MIP_STATUS_WAITING state. +///@param now The current timestamp. +/// +///@warning The command must be in the MIP_STATUS_WAITING state, otherwise the +/// result is unspecified. +/// +///@returns The time remaining before the command times out. The value will be +/// negative if the timeout time has passed. +/// +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now) +{ + assert(cmd->_status == MIP_STATUS_WAITING); + + // result <= 0 if timed out. + // Note: this still works with unsigned overflow. + return (int)(now - cmd->_timeout_time); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Checks if the command should time out. +/// +///@param cmd +///@param now Current time +/// +///@returns true if the command should time out. Only possible for MIP_STATUS_WAITING. +///@returns false if the command should not time out. +/// +bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now) +{ + if( cmd->_status == MIP_STATUS_WAITING ) + { + if( mip_pending_cmd_remaining_time(cmd, now) > 0 ) + { + return true; + } + } + + return false; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initializes a command queue. +/// +///@param queue +///@param base_reply_timeout +/// The minimum timeout given to all MIP commands. Additional time may be +/// given to specific commands which take longer. This is intended to be +/// used to accommodate long communication latencies, such as when using +/// a TCP connection. +/// +void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) +{ + queue->_first_pending_cmd = NULL; + queue->_base_timeout = base_reply_timeout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Queue a command to wait for replies. +/// +///@param queue +///@param cmd Listens for replies to this command. +/// +///@warning The command must not be deallocated or go out of scope while the +/// mip_cmd_status_is_finished returns false. +/// +void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd) +{ + // For now only one command can be queued at a time. + if( queue->_first_pending_cmd ) + { + cmd->_status = MIP_STATUS_CANCELLED; + return; + } + + cmd->_status = MIP_STATUS_PENDING; + queue->_first_pending_cmd = cmd; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Removes a pending command from the queue. +/// +///@internal +/// +///@param queue +///@param cmd +/// +void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd) +{ + if( queue->_first_pending_cmd == cmd ) + { + queue->_first_pending_cmd = NULL; + cmd->_status = MIP_STATUS_CANCELLED; + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Iterate over a packet, checking for replies to the pending command. +/// +///@internal +/// +///@param pending +/// Pending command which is awaiting replies +///@param packet +///@param base_timeout +///@param timestamp +/// +///@returns The new status of the pending command (the command status field is +/// not updated). The caller should set pending->_status to this value +/// after doing any additional processing requiring the pending struct. +/// +static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp) +{ + assert( pending->_status != MIP_STATUS_NONE ); // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here. + assert( !mip_cmd_result_is_finished(pending->_status) ); // Command shouldn't be finished yet - make sure the queue is processed properly. + + if( pending->_status == MIP_STATUS_PENDING ) + { + // Update the timeout to the timestamp of the timeout time. + pending->_timeout_time = timestamp + base_timeout + pending->_extra_timeout; + pending->_status = MIP_STATUS_WAITING; + } + + // ------+------+------+------+------+------+------+------+------+------------------------ + // ... | 0x02 | 0xF1 | cmd1 | nack | 0x02 | 0xF1 | cmd2 | ack | response field ... + // ------+------+------+------+------+------+------+------+------+------------------------ + + if( mip_packet_descriptor_set(packet) == pending->_descriptor_set ) + { + mip_field field = {0}; + while( mip_field_next_in_packet(&field, packet) ) + { + // Not an ack/nack reply field, skip it. + if( mip_field_field_descriptor(&field) != MIP_REPLY_DESC_GLOBAL_ACK_NACK ) + continue; + + // Sanity check payload length before accessing it. + if( mip_field_payload_length(&field) != 2 ) + continue; + + const uint8_t* const payload = mip_field_payload(&field); + + const uint8_t cmd_descriptor = payload[MIP_INDEX_REPLY_DESCRIPTOR]; + const uint8_t ack_code = payload[MIP_INDEX_REPLY_ACK_CODE]; + + // Is this the right command reply? + if( pending->_field_descriptor != cmd_descriptor ) + continue; + + // Descriptor matches! + + uint8_t response_length = 0; + mip_field response_field; + + // If the command was ACK'd, check if response data is expected. + if( pending->_response_descriptor != 0x00 && ack_code == MIP_ACK_OK ) + { + // Look ahead one field for response data. + response_field = mip_field_next_after(&field); + if( mip_field_is_valid(&response_field) ) + { + const uint8_t response_descriptor = mip_field_field_descriptor(&response_field); + + // This is a wildcard to accept any response data descriptor. + // Needed when the response descriptor is not known or is wrong. + if( pending->_response_descriptor == MIP_REPLY_DESC_GLOBAL_ACK_NACK ) + pending->_response_descriptor = response_descriptor; + + // Make sure the response descriptor matches what is expected. + if( response_descriptor == pending->_response_descriptor ) + { + // Update the response_size field to reflect the actual size. + response_length = mip_field_payload_length(&response_field); + + // Skip this field when iterating for next ack/nack reply. + field = response_field; + } + } + } + + // Limit response data size to lesser of buffer size or actual response length. + pending->_response_length = (response_length < pending->_response_buffer_size) ? response_length : pending->_response_buffer_size; + + // Copy response data to the pending buffer (skip if response_field is invalid). + if( pending->_response_length > 0 ) + memcpy(pending->_response_buffer, mip_field_payload(&response_field), pending->_response_length); + + // pending->_ack_code = ack_code; + pending->_reply_time = timestamp; // Completion time + + return (enum mip_cmd_result)ack_code; + } + } + + // No matching reply descriptors in this packet. + + // Check for timeout + if( mip_pending_cmd_check_timeout(pending, timestamp) ) + { + pending->_response_length = 0; + // pending->_ack_code = MIP_NACK_COMMAND_TIMEOUT; + + // Must be last! + return MIP_STATUS_TIMEDOUT; + } + + return pending->_status; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Process an incoming packet and check for replies to pending commands. +/// +/// Call this from the Mip_parser callback, passing the arguments directly. +/// +///@param queue +///@param packet The received MIP packet. Assumed to be valid. +///@param timestamp The time the packet was received +/// +void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp) +{ + // Check if the packet is a command descriptor set. + const uint8_t descriptor_set = mip_packet_descriptor_set(packet); + if( descriptor_set >= 0x80 && descriptor_set < 0xF0 ) + return; + + if( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + + const enum mip_cmd_result status = process_fields_for_pending_cmd(pending, packet, queue->_base_timeout, timestamp); + + if( mip_cmd_result_is_finished(status) ) + { + queue->_first_pending_cmd = queue->_first_pending_cmd->_next; + + // This must be done last b/c it may trigger the thread which queued the command. + // The command could go out of scope or its attributes inspected. + pending->_status = status; + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Clears the command queue. +/// +/// This must be called from the same thread context as the update function. +/// +///@param queue +/// +void mip_cmd_queue_clear(mip_cmd_queue* queue) +{ + while( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + queue->_first_pending_cmd = pending->_next; + + // This may deallocate the pending command in another thread (make sure to fetch the next cmd first). + pending->_status = MIP_STATUS_ERROR; + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Call periodically to make sure commands time out if no packets are +/// received. +/// +/// Call this during the device update if no calls to mip_cmd_queue_process_packet +/// are made (e.g. because no packets were received). It is safe to call this +/// in either case. +/// +///@param queue +///@param now +/// +void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) +{ + if( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + + if( pending->_status == MIP_STATUS_PENDING ) + { + // Update the timeout to the timestamp of the timeout time. + pending->_timeout_time = now + queue->_base_timeout + pending->_extra_timeout; + pending->_status = MIP_STATUS_WAITING; + } + else if( mip_pending_cmd_check_timeout(pending, now) ) + { + queue->_first_pending_cmd = queue->_first_pending_cmd->_next; + + // Clear response length and mark when it timed out. + pending->_response_length = 0; + pending->_reply_time = now; + + // This must be last! + pending->_status = MIP_STATUS_TIMEDOUT; + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the base reply timeout for all commands. +/// +/// THe base reply timeout is the minimum time to wait for a reply. +/// +///@param queue +///@param timeout +/// +void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout) +{ + queue->_base_timeout = timeout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the base reply timeout for all commands. +/// +///@returns The minimum time to wait for a reply to any command. +/// +timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) +{ + return queue->_base_timeout; +} diff --git a/src/mip/mip_cmdqueue.h b/src/mip/mip_cmdqueue.h index 4515a1b25..d18a60620 100644 --- a/src/mip/mip_cmdqueue.h +++ b/src/mip/mip_cmdqueue.h @@ -67,6 +67,7 @@ enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd); const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd); uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd); +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now); bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now); ///@} diff --git a/src/mip/mip_device.cpp b/src/mip/mip_device.cpp index f1d36b234..5e68c032d 100644 --- a/src/mip/mip_device.cpp +++ b/src/mip/mip_device.cpp @@ -2,19 +2,50 @@ #include "mip_device.hpp" namespace mip { -namespace C { -extern "C" { -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out) -{ - return static_cast(device)->recvFromDevice(buffer, max_length, out_length, timestamp_out); -} +//////////////////////////////////////////////////////////////////////////////// +///@fn Connection::sendToDevice +/// +///@brief Sends bytes to the device +/// +///@param data The data to send to the device +///@param length Length of data in bytes +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +///@fn Connection::recvFromDevice +/// +///@brief Receives bytes from the device +/// +///@param buffer Buffer to store the received data in +///@param max_length Max number of bytes that can be read. Should be at most the length of buffer. +///@param length_out Number of bytes actually read. +///@param timestamp Timestamp of when the data was received +//////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length) + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets up the mip interface callbacks to point at this object. +/// +///@note This sets the interface's user pointer to this connection object. +/// +///@param device Device to configure. +/// +void Connection::connect_interface(C::mip_interface* device) { - return static_cast(device)->sendToDevice(data, length); + auto send = [](C::mip_interface* device, const uint8_t* data, size_t length)->bool + { + return static_cast(C::mip_interface_user_pointer(device))->sendToDevice(data, length); + }; + auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out)->bool + { + return static_cast(C::mip_interface_user_pointer(device))->recvFromDevice(buffer, max_length, wait_time, length_out, timestamp_out); + }; + + C::mip_interface_set_user_pointer(device, this); + + C::mip_interface_set_send_function(device, send); + C::mip_interface_set_recv_function(device, recv); } -} // extern "C" -} // namespace C } // namespace mip diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index bd8717ec7..1db0813c3 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -135,19 +135,10 @@ template bool startCommand(C::mip_interface& device, C::mip_pending_c class Connection { public: - ///@brief Sends bytes to the device - /// - ///@param data The data to send to the device - ///@param length Length of data in bytes - virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; // Must be implemented by a derived class. + virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; + virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) = 0; - ///@brief Receives bytes from the device - /// - ///@param buffer Buffer to store the received data in - ///@param max_length Max number of bytes that can be read. Should be at most the length of buffer. - ///@param length_out Number of bytes actually read. - ///@param timestamp Timestamp of when the data was received - virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, Timestamp* timestamp) = 0; // Must be implemented by a derived class. + void connect_interface(C::mip_interface* device); }; @@ -161,9 +152,21 @@ class DeviceInterface : public C::mip_interface // Constructors // + ///@copydoc mip::C::mip_interface_init + /// The interface callbacks must be assigned separately (e.g. with Connection::connect_interface()) + DeviceInterface(uint8_t* parseBuffer, size_t parseBufferSize, Timeout parseTimeout, Timeout baseReplyTimeout) + { + C::mip_interface_init(this, parseBuffer, parseBufferSize, parseTimeout, baseReplyTimeout, nullptr, nullptr, &C::mip_interface_default_update, nullptr); + } + ///@copydoc mip::C::mip_interface_init ///@param connection The connection object used to communicate with the device. This object must exist for the life of the DeviceInterface object - DeviceInterface(Connection* connection, uint8_t* parseBuffer, size_t parseBufferSize, Timeout parseTimeout, Timeout baseReplyTimeout) : mConnection(connection) { C::mip_interface_init(this, parseBuffer, parseBufferSize, parseTimeout, baseReplyTimeout); } + DeviceInterface(Connection* connection, uint8_t* parseBuffer, size_t parseBufferSize, Timeout parseTimeout, Timeout baseReplyTimeout) : + DeviceInterface(parseBuffer, parseBufferSize, parseTimeout, baseReplyTimeout) + { + if(connection) + connection->connect_interface(this); + } DeviceInterface(const DeviceInterface&) = delete; DeviceInterface& operator=(const DeviceInterface&) = delete; @@ -171,50 +174,84 @@ class DeviceInterface : public C::mip_interface ~DeviceInterface() = default; // - // Accessors + // Callback functions // - ///@copydoc C::mip_interface_set_update_function + // C function callbacks + + C::mip_send_callback sendFunction() const { return C::mip_interface_send_function(this); } + C::mip_recv_callback recvFunction() const { return C::mip_interface_recv_function(this); } + C::mip_update_callback updateFunction() const { return C::mip_interface_update_function(this); } + + void setSendFunction (C::mip_send_callback callback) { C::mip_interface_set_send_function (this, callback); } + void setRecvFunction (C::mip_recv_callback callback) { C::mip_interface_set_recv_function (this, callback); } void setUpdateFunction(C::mip_update_callback function) { C::mip_interface_set_update_function(this, function); } - template + // free/nonmember function callbacks + + template + void setSendFunction(); + + template + void setRecvFunction(); + + template + void setUpdateFunction(); + + // derived member function callbacks + + template + void setSendFunction(); + + template + void setRecvFunction(); + + template void setUpdateFunction(); + // Separate class object callbacks + + template< + class T, + bool (T::*Send)(const uint8_t*, size_t), + bool (T::*Recv)(uint8_t*, size_t, Timeout, size_t*, Timestamp*), + bool (T::*Update)(Timeout) = nullptr + > + void setCallbacks(T* object); + + // + // General accessors + // + void setMaxPacketsPerPoll(unsigned int maxPackets) { C::mip_interface_set_max_packets_per_update(this, maxPackets); } unsigned int maxPacketsPerPoll() const { return C::mip_interface_max_packets_per_update(this); } Timeout baseReplyTimeout() const { return C::mip_cmd_queue_base_reply_timeout(&cmdQueue()); } void setBaseReplyTimeout(Timeout timeout) { C::mip_cmd_queue_set_base_reply_timeout(&cmdQueue(), timeout); } + Parser& parser() { return *static_cast(C::mip_interface_parser(this)); } CmdQueue& cmdQueue() { return *static_cast(C::mip_interface_cmd_queue(this)); } const Parser& parser() const { return const_cast(this)->parser(); } const CmdQueue& cmdQueue() const { return const_cast(this)->cmdQueue(); } - const Connection* connection() const { return mConnection; } - void setConnection(Connection* connection) { mConnection = connection; } - // // Communications // - RemainingCount receiveBytes(const uint8_t* data, size_t length, Timestamp timestamp) { return C::mip_interface_receive_bytes(this, data, length, timestamp); } - - void receivePacket(const C::mip_packet& packet, Timestamp timestamp) { C::mip_interface_receive_packet(this, &packet, timestamp); } - - bool sendToDevice(const uint8_t* data, size_t length) { return mConnection->sendToDevice(data, length); } + bool sendToDevice(const uint8_t* data, size_t length) { return C::mip_interface_send_to_device(this, data, length); } bool sendToDevice(const C::mip_packet& packet) { return sendToDevice(C::mip_packet_pointer(&packet), C::mip_packet_total_length(&packet)); } - + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp) { return C::mip_interface_recv_from_device(this, buffer, max_length, wait_time, length_out, timestamp); } bool update(bool blocking=false) { return C::mip_interface_update(this, blocking); } - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, Timestamp* timestamp) { return mConnection->recvFromDevice(buffer, max_length, length_out, timestamp); } + bool defaultUpdate(bool blocking=false) { return C::mip_interface_default_update(this, blocking); } + RemainingCount receiveBytes(const uint8_t* data, size_t length, Timestamp timestamp) { return C::mip_interface_receive_bytes(this, data, length, timestamp); } + void receivePacket(const C::mip_packet& packet, Timestamp timestamp) { C::mip_interface_receive_packet(this, &packet, timestamp); } void processUnparsedPackets() { C::mip_interface_process_unparsed_packets(this); } CmdResult waitForReply(C::mip_pending_cmd& cmd) { return C::mip_interface_wait_for_reply(this, &cmd); } - bool defaultUpdate(bool blocking=false) { return C::mip_interface_default_update(this, blocking); } - // // Data Callbacks // @@ -272,36 +309,198 @@ class DeviceInterface : public C::mip_interface // template // bool startCommand(PendingCmd& pending, const Cmd& cmd, uint8_t* responseBuffer, uint8_t responseBufferSize, Timeout additionalTime=0) { return mip::startCommand(pending, cmd, responseBuffer, responseBufferSize, additionalTime); } - -private: - Connection* mConnection; }; + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the send callback function (free function version). +/// +///@tparam Send A compile-time pointer to the callback function. +/// +template +void DeviceInterface::setSendFunction() +{ + setSendFunction([](C::mip_interface* device, const uint8_t* data, size_t length){ + return (*Send)(*static_cast(device), data, length); + }); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the receive callback function (free function version). +/// +///@tparam Send A compile-time pointer to the callback function. +/// +template +void DeviceInterface::setRecvFunction() +{ + setRecvFunction([](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out){ + return (*Recv)(*static_cast(device), buffer, max_length, wait_time, length_out, timestamp_out); + }); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the update callback function (free function version). +/// +///@tparam Send A compile-time pointer to the callback function. +/// +template +void DeviceInterface::setUpdateFunction() +{ + setUpdateFunction([](C::mip_interface* device, C::timeout_type wait_time){ + return (*Update)(*static_cast(device), wait_time); + }); +} + + //////////////////////////////////////////////////////////////////////////////// -///@brief Sets the update function to a function taking a MipDevice reference. +///@brief Sets the send callback function (derived member function version). +/// +///@tparam Derived Derived class type. Must inherit from DeviceInterface. +///@tparam Send Compile-time pointer to member function of Derived. /// ///@code{.cpp} -/// bool updateDevice(DeviceInterface& device, bool blocking) +/// class MyClass : public mip::DeviceInterface /// { -/// return device.defaultUpdate(blocking); -/// } +/// bool sendToDevice(const uint8_t* data, size_t size); +/// bool recvFromDevice(uint8_t* data, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out); +/// }; +/// +/// MyClass instance; +/// instance.setSendFunction(); +/// instance.setRecvFunction(); /// -/// device.setUpdateFunction<&updateDevice>(); +/// instance.setCallbacks(connection); ///@endcode /// -template + +template +void DeviceInterface::setSendFunction() +{ + static_assert(std::is_base_of::value, "Derived must be derived from C::mip_interface."); + + setSendFunction( + [](C::mip_interface* device, const uint8_t* data, size_t length) + { + return (static_cast(device)->*Send)(data, length); + } + ); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the receive callback function (derived member function version). +/// +///@tparam Derived Derived class type. Must inherit from DeviceInterface. +///@tparam Recv Compile-time pointer to member function of Derived. +/// +///@see DeviceInterface::setSendFunction() +/// +template +void DeviceInterface::setRecvFunction() +{ + static_assert(std::is_base_of::value, "Derived must be derived from C::mip_interface."); + + setRecvFunction( + [](C::mip_interface* device, uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) + { + return (static_cast(device)->*Recv)(buffer, max_length, wait_time, length_out, timestamp_out); + } + ); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the update callback function (derived member function version). +/// +///@tparam Derived Derived class type. Must inherit from DeviceInterface. +///@tparam Update Compile-time pointer to member function of Derived. +/// +///@see DeviceInterface::setSendFunction() +/// +template void DeviceInterface::setUpdateFunction() { + static_assert(std::is_base_of::value, "Derived must be derived from C::mip_interface."); + setUpdateFunction( - [](C::mip_interface* device, bool blocking)->bool + [](C::mip_interface* device, C::timeout_type wait_time)->bool { - return Function(*static_cast(device), blocking); + return (static_cast(device)->*Update)(wait_time); } ); } +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the callback functions to a common class object. +/// +///@tparam T +/// A class type. +/// +///@tparam Send +/// A member function pointer for sending bytes to the device. +/// May be NULL. +/// +///@tparam Recv +/// A member function pointer for receiving bytes from the device. +/// May be NULL. +/// +///@tparam Update +/// A member function pointer for updating the device. +/// If both this and Recv are NULL, no update function is set. +/// If Update is NULL but Recv is not, the default update function +/// is used instead. +/// +///@param object +/// An instance of T. The interface's user pointer will be set to this +/// value. All of the callbacks will be invoked on this instance. +/// +///@code{.cpp} +/// class DeviceConnection +/// { +/// bool send(const uint8_t* data, size_t length); +/// bool recv(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out); +/// }; +/// +/// DeviceConnection connection; +/// mip::DeviceInterface interface; +/// +/// interface.setCallbacks(&connection); +///@endcode +/// +template< + class T, + bool (T::*Send)(const uint8_t*, size_t), + bool (T::*Recv)(uint8_t*, size_t, Timeout, size_t*, Timestamp*), + bool (T::*Update)(Timeout) +> +void DeviceInterface::setCallbacks(T* object) +{ + auto send = [](C::mip_interface* device, const uint8_t* data, size_t size) + { + return (static_cast(mip_interface_user_pointer(device))->*Send)(data, size); + }; + auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out) + { + return (static_cast(mip_interface_user_pointer(device))->*Recv)(buffer, max_length, wait_time, length_out, timestamp_out); + }; + auto update = [](C::mip_interface* device, C::timeout_type wait_time) + { + return (static_cast(mip_interface_user_pointer(device))->*Update)(wait_time); + }; + + C::mip_interface_set_user_pointer(this, object); + C::mip_interface_set_send_function(this, Send != nullptr ? send : nullptr); + C::mip_interface_set_recv_function(this, Recv != nullptr ? recv : nullptr); + + if( Update != nullptr ) + C::mip_interface_set_update_function(this, update); + else if( Recv != nullptr ) + C::mip_interface_set_update_function(this, &C::mip_interface_default_update); + else + C::mip_interface_set_update_function(this, nullptr); +} + + //////////////////////////////////////////////////////////////////////////////// ///@brief Registers a packet callback (free function version). /// diff --git a/src/mip/mip_interface.c b/src/mip/mip_interface.c index 98b8a8e3c..8a5e14f9a 100644 --- a/src/mip/mip_interface.c +++ b/src/mip/mip_interface.c @@ -9,21 +9,71 @@ #include + //////////////////////////////////////////////////////////////////////////////// -///@brief Wrapper around mip_interface_receive_packet for use with mip_parser. +///@typedef mip_send_callback /// -///@param device Void pointer to the device. Must be a mip_interface pointer. -///@param packet MIP Packet from the parser. -///@param timestamp timestamp_type of the packet. +///@copydoc mip_interface_send_to_device /// -///@returns True +///@note There are cases where the data will not be a MIP packet. /// -bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp) -{ - mip_interface_receive_packet(device, packet, timestamp); - return true; -} +//////////////////////////////////////////////////////////////////////////////// +///@typedef mip_recv_callback +/// +///@brief Receives new data from the device. Called repeatedly +/// by mip_interface_update() while waiting for command responses. +/// +///@param device The mip interface object +///@param buffer Buffer to fill with data. Should be allocated before +/// calling this function +///@param max_length Max number of bytes that can be read into the buffer. +///@param wait_time Time to wait for data from the device. The actual time +/// waited may be less than wait_time, but it should not +/// significantly exceed this value. +///@param out_length Number of bytes actually read into the buffer. +///@param timestamp_out Timestamp of the data was received. +/// +///@returns True if successful, even if no data is received. +///@returns False if the port cannot be read or some other error occurs. +/// +///@note Except in case of error (i.e. returning false), the timestamp must be +/// set even if no data is received. This is required to allow commands +/// to time out. +/// +///@note Applications may sleep the thread or enter a low-power state while +/// waiting for data. On posix-like (e.g. desktop) systems, applications +/// should call read() with a maximum timeout of wait_time. +/// If the actual wait time is less than the requested duration, this +/// function may be called again by the MIP SDK to wait the remaining time. +/// If the actual wait time exceeds wait_time, command timeouts may take +/// longer than intended. +/// + +//////////////////////////////////////////////////////////////////////////////// +///@typedef mip_update_callback +/// +///@brief Callback function typedef for custom update behavior. +/// +/// This function is called whenever data should be parsed from the port: +///@li While waiting for command responses +///@li To check for new data packets +/// +/// Generally an application should call mip_interface_recv_from_device() from +/// within this callback and pass the data to mip_interface_receive_bytes(). +/// Most applications can set this callback to mip_interface_default_update(). +/// +///@param device +/// The mip_interface object being updated. +///@param timeout +/// Amount of time to wait for data from the device. This will be zero +/// when checking for data and nonzero when waiting for commands. +/// +///@returns True if successful (even if no data is received). +///@returns False if an error occurs and the port cannot be read (e.g. if the +/// port is closed). Returning false will cause any pending commands to +/// fail with a status error code. +/// //////////////////////////////////////////////////////////////////////////////// ///@brief Initialize the mip_interface components. @@ -39,12 +89,19 @@ bool mip_interface_parse_callback(void* device, const mip_packet* packet, timest ///@param base_reply_timeout /// Minimum time for all commands. See mip_cmd_queue_init(). /// -void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, timeout_type parse_timeout, timeout_type base_reply_timeout) +void mip_interface_init( + mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, + timeout_type parse_timeout, timeout_type base_reply_timeout, + mip_send_callback send, mip_recv_callback recv, + mip_update_callback update, void* user_pointer) { mip_parser_init(&device->_parser, parse_buffer, parse_buffer_size, &mip_interface_parse_callback, device, parse_timeout); device->_max_update_pkts = MIPPARSER_UNLIMITED_PACKETS; - device->_update_function = &mip_interface_default_update; + device->_send_callback = send; + device->_recv_callback = recv; + device->_update_callback = update; + device->_user_pointer = user_pointer; mip_cmd_queue_init(&device->_queue, base_reply_timeout); @@ -52,6 +109,66 @@ void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t par } +//////////////////////////////////////////////////////////////////////////////// +// +// Accessors +// +//////////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the send callback function. +/// +///@param device +/// +///@param function +/// Function which sends raw bytes to the device. This can be NULL if no +/// commands will be issued (they would fail). +/// +void mip_interface_set_send_function(mip_interface* device, mip_send_callback callback) +{ + device->_send_callback = callback; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the send function pointer. +/// +///@param device +/// +///@returns The send callback function. May be NULL. +/// +mip_send_callback mip_interface_send_function(const mip_interface* device) +{ + return device->_send_callback; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the receive callback function. +/// +///@param device +/// +///@param function +/// Function which gets data from the device connection. +/// If this is NULL then commands will fail and no data will be received. +/// +void mip_interface_set_recv_function(mip_interface* device, mip_recv_callback callback) +{ + device->_recv_callback = callback; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the receive function pointer. +/// +///@param device +/// +///@returns The receive callback function. May be NULL. +/// +mip_recv_callback mip_interface_recv_function(const mip_interface* device) +{ + return device->_recv_callback; +} + //////////////////////////////////////////////////////////////////////////////// ///@brief Sets the update function. @@ -68,20 +185,21 @@ void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t par /// If this is NULL, then update calls will fail and no data or /// or command replies will be received. /// -void mip_interface_set_update_function(struct mip_interface* device, mip_update_callback function) +void mip_interface_set_update_function(mip_interface* device, mip_update_callback callback) { - device->_update_function = function; + device->_update_callback = callback; } + //////////////////////////////////////////////////////////////////////////////// ///@brief Gets the update function pointer. /// ///@returns The update function. Defaults to mip_interface_default_update. May /// be NULL. /// -mip_update_callback mip_interface_update_function(struct mip_interface* device) +mip_update_callback mip_interface_update_function(const mip_interface* device) { - return device->_update_function; + return device->_update_callback; } @@ -138,6 +256,66 @@ void mip_interface_set_max_packets_per_update(mip_interface* device, unsigned in } +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the MIP parser for the device. +/// +mip_parser* mip_interface_parser(mip_interface* device) +{ + return &device->_parser; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the commmand queue for the device. +/// +mip_cmd_queue* mip_interface_cmd_queue(mip_interface* device) +{ + return &device->_queue; +} + + +//////////////////////////////////////////////////////////////////////////////// +// +// Communications +// +//////////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sends data to the port (i.e. from this library to the physical device). +/// +///@param device The mip_interface object. +///@param data Data to be sent. +///@param length Length of data. +/// +///@returns True if the data was sent successfully. +///@returns False if the send callback is NULL. +///@returns False if some or all data could not be sent. +/// +/// This is called whenever bytes must be sent to the physical device. +/// +bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, size_t length) +{ + return device->_send_callback && device->_send_callback(device, data, length); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Checks for data at the port and reads it into buffer. +/// +///@param device +///@param buffer A place to store the data. +///@param max_length Maximum number of bytes to read into buffer. +///@param wait_time Maximum time to wait for data. May be 0. +///@param length_out The number of bytes successfully read into buffer. +///@param timestamp_out The timestamp of the received data. +/// +///@returns True if successful (even if 0 bytes were read). +///@returns False if the receive callback is NULL. +///@returns False if the receive callback failed (i.e. if it returned false). +/// +bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* length_out, timestamp_type* timestamp_out) +{ + return device->_recv_callback && device->_recv_callback(device, buffer, max_length, wait_time, length_out, timestamp_out); +} //////////////////////////////////////////////////////////////////////////////// ///@brief Call to process data from the device. @@ -145,42 +323,47 @@ void mip_interface_set_max_packets_per_update(mip_interface* device, unsigned in /// This function is also called while waiting for command replies. /// /// Call this periodically to process packets received from the device. It -/// should be called at a suitably-high rate to prevent the connection buffers +/// should be called at a suitably high rate to prevent the connection buffers /// from overflowing. The update rate affects the reception timestamp resolution. /// ///@param device /// -///@param blocking -/// This is true when called from within a blocking command function. -/// Applications should generally set this to false, e.g. when calling -/// this to process incoming data packets. +///@param wait_time +/// Time to wait for data from the device. This will be nonzero when +/// waiting for command replies. Applications calling this function +/// can pass 0 to avoid blocking when checking for new data. /// ///@returns true if operation should continue, or false if the device cannot be /// updated (e.g. if the serial port is not open). /// -bool mip_interface_update(struct mip_interface* device, bool blocking) +bool mip_interface_update(struct mip_interface* device, timeout_type wait_time) { - if( !device->_update_function ) + if( !device->_update_callback ) return false; - return device->_update_function(device, blocking); + return device->_update_callback(device, wait_time); } + //////////////////////////////////////////////////////////////////////////////// ///@brief Polls the port for new data or command replies. /// /// This is the default choice for the user update function. It ignores the /// blocking flag and always reads data from the device. /// -///@param device The mip_interface object. -///@param blocking Ignored. +///@param device +/// +///@param wait_time +/// Time to wait for data to be received. Passed directly to +/// mip_interface_recv_from_device(). /// ///@returns The value returned by mip_interface_user_recv_from_device. /// -bool mip_interface_default_update(struct mip_interface* device, bool blocking) +bool mip_interface_default_update(struct mip_interface* device, timeout_type wait_time) { - (void)blocking; + if( !device->_recv_callback ) + return false; uint8_t* ptr; mip_parser* parser = mip_interface_parser(device); @@ -188,37 +371,20 @@ bool mip_interface_default_update(struct mip_interface* device, bool blocking) size_t count = 0; timestamp_type timestamp = 0; - if ( !mip_interface_user_recv_from_device(device, ptr, max_count, &count, ×tamp) ) + if ( !mip_interface_recv_from_device(device, ptr, max_count, wait_time, &count, ×tamp) ) return false; assert(count <= max_count); mip_parser_process_written(parser, count, timestamp, 0); mip_cmd_queue_update(mip_interface_cmd_queue(device), timestamp); - return true; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Sends data to the port (i.e. from this library to the physical device). -/// -///@param device The mip_interface object. -///@param data Data to be sent. -///@param length Length of data. -/// -///@returns True if the data was sent successfully, false if some or all data -/// could not be sent. -/// -/// This is called whenever bytes must be sent to the physical device. -/// -bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, size_t length) -{ - return mip_interface_user_send_to_device(device, data, length); + return true; } //////////////////////////////////////////////////////////////////////////////// -///@brief Receive data from the port (i.e. the physical device) into the parser. +///@brief Passes data from the device into the parser. /// ///@param device /// @@ -237,7 +403,6 @@ remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t return mip_parser_parse(&device->_parser, data, length, timestamp, device->_max_update_pkts); } - //////////////////////////////////////////////////////////////////////////////// ///@brief Process more packets from the internal buffer. /// @@ -271,22 +436,28 @@ void mip_interface_receive_packet(mip_interface* device, const mip_packet* packe mip_dispatcher_dispatch_packet(&device->_dispatcher, packet, timestamp); } - //////////////////////////////////////////////////////////////////////////////// -///@brief Returns the MIP parser for the device. +///@brief Wrapper around mip_interface_receive_packet for use with mip_parser. /// -mip_parser* mip_interface_parser(mip_interface* device) +///@param device Void pointer to the device. Must be a mip_interface pointer. +///@param packet MIP Packet from the parser. +///@param timestamp timestamp_type of the packet. +/// +///@returns True +/// +bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp) { - return &device->_parser; + mip_interface_receive_packet(device, packet, timestamp); + + return true; } + +//////////////////////////////////////////////////////////////////////////////// +// +// Command and data processing +// //////////////////////////////////////////////////////////////////////////////// -///@brief Returns the commmand queue for the device. -/// -mip_cmd_queue* mip_interface_cmd_queue(mip_interface* device) -{ - return &device->_queue; -} //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/mip_interface.h b/src/mip/mip_interface.h index fafdb5f37..7784fbcc4 100644 --- a/src/mip/mip_interface.h +++ b/src/mip/mip_interface.h @@ -32,45 +32,46 @@ extern "C" { struct mip_interface; -//////////////////////////////////////////////////////////////////////////////// -///@brief Callback function typedef for custom update behavior. -/// -///@param device The mip_interface object being updated. -///@param blocking True if called from within a blocking command function. -/// -///@returns False if an error occurs and the port cannot be read (e.g. if the -/// port is closed). Returning false will cause any pending commands to -/// fail with a status error code. -///@returns True if successful (even if no data is received). -/// -typedef bool (*mip_update_callback)(struct mip_interface* device, bool blocking); +// Documentation is in source file. +typedef bool (*mip_send_callback)(struct mip_interface* device, const uint8_t* data, size_t length); +typedef bool (*mip_recv_callback)(struct mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +typedef bool (*mip_update_callback)(struct mip_interface* device, timeout_type timeout); + //////////////////////////////////////////////////////////////////////////////// ///@brief State of the interface for communicating with a MIP device. /// typedef struct mip_interface { - mip_parser _parser; ///<@private MIP Parser for incoming MIP packets. - mip_cmd_queue _queue; ///<@private Queue for checking command replies. - mip_dispatcher _dispatcher; ///<@private Dispatcher for data callbacks. - unsigned int _max_update_pkts; ///<@private Max number of MIP packets to parse at once. - mip_update_callback _update_function; ///<@private Optional function to call during updates. - void* _user_pointer; ///<@private Optional user-specified data pointer. + mip_parser _parser; ///<@private MIP Parser for incoming MIP packets. + mip_cmd_queue _queue; ///<@private Queue for checking command replies. + mip_dispatcher _dispatcher; ///<@private Dispatcher for data callbacks. + unsigned int _max_update_pkts; ///<@private Max number of MIP packets to parse at once. + mip_send_callback _send_callback; ///<@private Optional function which is called to send raw bytes to the device. + mip_recv_callback _recv_callback; ///<@private Optional function which is called to receive raw bytes from the device. + mip_update_callback _update_callback; ///<@private Optional function to call during updates. + void* _user_pointer; ///<@private Optional user-specified data pointer. } mip_interface; -void mip_interface_init(mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, timeout_type parse_timeout, timeout_type base_reply_timeout); + +void mip_interface_init( + mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, + timeout_type parse_timeout, timeout_type base_reply_timeout, + mip_send_callback send, mip_recv_callback recv, + mip_update_callback update, void* user_pointer +); // // Communications // -remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, timestamp_type timestamp); -void mip_interface_process_unparsed_packets(mip_interface* device); -bool mip_interface_update(mip_interface* device, bool blocking); -bool mip_interface_default_update(mip_interface* device, bool blocking); - bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, size_t length); +bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type timeout, size_t* length_out, timestamp_type* now); +bool mip_interface_update(mip_interface* device, timeout_type wait_time); +bool mip_interface_default_update(mip_interface* device, timeout_type wait_time); +remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, timestamp_type timestamp); +void mip_interface_process_unparsed_packets(mip_interface* device); bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp); void mip_interface_receive_packet(mip_interface* device, const mip_packet* packet, timestamp_type timestamp); @@ -97,66 +98,24 @@ void mip_interface_register_extractor(mip_interface* device, mip_dispatch_handle // Accessors // +void mip_interface_set_recv_function(mip_interface* device, mip_recv_callback function); +void mip_interface_set_send_function(mip_interface* device, mip_send_callback function); void mip_interface_set_update_function(mip_interface* device, mip_update_callback function); void mip_interface_set_user_pointer(mip_interface* device, void* pointer); + void mip_interface_set_max_packets_per_update(mip_interface* device, unsigned int max_packets); unsigned int mip_interface_max_packets_per_update(const mip_interface* device); -mip_update_callback mip_interface_update_function(mip_interface* device); -void* mip_interface_user_pointer(const mip_interface* device); +mip_recv_callback mip_interface_recv_function(const mip_interface* device); +mip_send_callback mip_interface_send_function(const mip_interface* device); +mip_update_callback mip_interface_update_function(const mip_interface* device); +void* mip_interface_user_pointer(const mip_interface* device); + mip_parser* mip_interface_parser(mip_interface* device); mip_cmd_queue* mip_interface_cmd_queue(mip_interface* device); ///@} ///@} -//////////////////////////////////////////////////////////////////////////////// -///@defgroup user_callbacks User callback functions [C/CPP] -/// -///@{ - -//////////////////////////////////////////////////////////////////////////////// -///@brief Receives new data from the device. Called repeatedly -/// by mip_interface_update() while waiting for command responses. -/// -///@param device The mip interface object -///@param buffer Buffer to fill with data. Should be allocated before -/// calling this function -///@param max_length Max number of bytes that can be read into the buffer. -///@param out_length Number of bytes actually read into the buffer. -///@param timestamp_out Timestamp of the data was received. -/// -///@returns true if operation should continue, or false if the device cannot be -/// updated (e.g. if the serial port is not open). -/// -///@note Except in case of error (i.e. returning false), the timestamp must be -/// set even if no data is received. This is required to allow commands -/// to time out. -/// -///@note On systems where it makes sense, this is a good place to call sleep -/// or enter a low-power state until data arrives at the port. Typically -/// this function will wait a few milliseconds before returning. -/// -///@warning Do not block indefinitely as this will stall the system beyond the -/// normal command timeout. Use a sensible timeout (i.e. 1/10th of the -/// base reply timeout) or only sleep for a minimal amount of time. -/// -extern bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, size_t* out_length, timestamp_type* timestamp_out); - - -//////////////////////////////////////////////////////////////////////////////// -///@copydoc mip_interface_send_to_device -/// -///@note This is a good place to put logging code for debugging device -/// communications at the byte level. -/// -///@note There are cases where the data will not be a MIP packet. -/// -extern bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); - - -///@} -//////////////////////////////////////////////////////////////////////////////// - #ifdef __cplusplus } // namespace mip diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index f31722e06..3bc10794e 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -12,7 +12,7 @@ namespace platform ///@brief Creates a SerialConnection that will communicate with a device over serial /// ///@param portName Path to the port to connect to. On Windows, this usually looks like "COM", on linux, "/dev/tty" -///@param baudrate Baud rate to open the device at. Note that the device needs to be configured to +///@param baudrate Baud rate to open the device at. Note that the device needs to be configured to SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrate) { if (!serial_port_open(&mPort, portName.c_str(), baudrate)) @@ -26,10 +26,11 @@ SerialConnection::~SerialConnection() } ///@copydoc mip::Connection::recvFromDevice -bool SerialConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) +bool SerialConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) { - *timestamp = getCurrentTimestamp(); - return serial_port_read(&mPort, buffer, max_length, length_out); + *timestamp = std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + + return serial_port_read(&mPort, buffer, max_length, wait_time, length_out); } ///@copydoc mip::Connection::sendToDevice diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index 1e0e9f510..85adfc925 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -5,9 +5,6 @@ #include - -extern mip::Timestamp getCurrentTimestamp(); - namespace mip { namespace platform @@ -27,7 +24,7 @@ class SerialConnection : public mip::Connection SerialConnection(const std::string& portName, uint32_t baudrate); ~SerialConnection(); - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) final; + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; private: diff --git a/src/mip/platform/tcp_connection.cpp b/src/mip/platform/tcp_connection.cpp index 9d63a51fb..12780e4c2 100644 --- a/src/mip/platform/tcp_connection.cpp +++ b/src/mip/platform/tcp_connection.cpp @@ -27,9 +27,12 @@ TcpConnection::~TcpConnection() } ///@copydoc mip::Connection::sendToDevice -bool TcpConnection::recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) +bool TcpConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) { - *timestamp = getCurrentTimestamp(); + (void)wait_time; // Not used, timeout is always fixed + + *timestamp = std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + return tcp_socket_recv(&mSocket, buffer, max_length, length_out); } diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index 426cb40a4..85823bb01 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -6,9 +6,6 @@ #include - -extern mip::Timestamp getCurrentTimestamp(); - namespace mip { namespace platform @@ -28,7 +25,7 @@ class TcpConnection : public mip::Connection TcpConnection(const std::string& hostname, uint16_t port); ~TcpConnection(); - bool recvFromDevice(uint8_t* buffer, size_t max_length, size_t* length_out, mip::Timestamp* timestamp) final; + bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; private: diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 37c339ccb..bcd047c1f 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -63,7 +63,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Connect to the provided com port port->handle = CreateFile(port_str, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); - + //Check for an invalid handle if(port->handle == INVALID_HANDLE_VALUE) { @@ -74,7 +74,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Setup the com port buffer sizes if(SetupComm(port->handle, COM_PORT_BUFFER_SIZE, COM_PORT_BUFFER_SIZE) == 0) return false; - + //Set the timeouts COMMTIMEOUTS timeouts; GetCommTimeouts(port->handle, &timeouts); @@ -90,7 +90,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Setup the com port parameters ready = GetCommState(port->handle, &dcb); - + //Close the serial port, mutex, and exit if(!ready) { @@ -98,7 +98,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) return false; } - dcb.BaudRate = baudrate; //Baudrate is typically 115200 + dcb.BaudRate = baudrate; //Baudrate is typically 115200 dcb.ByteSize = 8; //Charsize is 8, default for MicroStrain dcb.Parity = NOPARITY; //Parity is none, default for MicroStrain dcb.StopBits = ONESTOPBIT; //Stopbits is 1, default for MicroStrain @@ -106,14 +106,14 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) dcb.fDtrControl = DTR_CONTROL_ENABLE; ready = SetCommState(port->handle, &dcb); - + //Close the serial port and exit if(!ready) { CloseHandle(port->handle); return false; } - + #else //Linux port->handle = open(port_str, O_RDWR | O_NOCTTY | O_SYNC); @@ -144,7 +144,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) // Persist the settings if(tcsetattr(port->handle, TCSANOW, &serial_port_settings) < 0) return false; - + // Flush any waiting data tcflush(port->handle, TCIOFLUSH); @@ -175,13 +175,13 @@ bool serial_port_close(serial_port *port) bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, size_t *bytes_written) { - + *bytes_written = 0; //Check for a valid port handle if(!port->is_open) return false; - + #ifdef WIN32 //Windows DWORD local_bytes_written; @@ -189,7 +189,7 @@ bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, if(WriteFile(port->handle, buffer, num_bytes, &local_bytes_written, NULL)) { *bytes_written = local_bytes_written; - + if(*bytes_written == num_bytes) return true; } @@ -199,23 +199,29 @@ bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, if(*bytes_written == num_bytes) return true; - + #endif return false; } -bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, size_t *bytes_read) +bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wait_time, size_t *bytes_read) { - //Set the bytes read to zero *bytes_read = 0; //Check for a valid port handle if(!port->is_open) return false; - + #ifdef WIN32 //Windows + + if( wait_time <= 0 ) + { + if( serial_port_read_count(port) == 0 ) + return true; + } + DWORD local_bytes_read; //Call the windows read function @@ -226,7 +232,7 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, size_t #else //Linux // Poll the device before attempting to read any data, so we will only block for 10ms if there is no data available struct pollfd poll_fd = { .fd = port->handle, .events = POLLIN }; - int poll_status = poll(&poll_fd, 1, 10); + int poll_status = poll(&poll_fd, 1, wait_time); // Keep reading and polling while there is still data available if (poll_status > 0 && poll_fd.revents & POLLIN) @@ -249,17 +255,17 @@ uint32_t serial_port_read_count(serial_port *port) //Check for a valid port handle if(!port->is_open) return 0; - + #ifdef WIN32 //Windows COMSTAT com_status; DWORD errors; - + //This function gets the current com status if(ClearCommError(port->handle, &errors, &com_status)) { return com_status.cbInQue; } - + #else //Linux int bytes_available; ioctl(port->handle, FIONREAD, &bytes_available); diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index a61a65ab9..8354974cd 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -46,7 +46,7 @@ typedef struct serial_port bool serial_port_open(serial_port *port, const char *port_str, int baudrate); bool serial_port_close(serial_port *port); bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, size_t *bytes_written); -bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, size_t *bytes_read); +bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wait_time, size_t *bytes_read); uint32_t serial_port_read_count(serial_port *port); bool serial_port_is_open(serial_port *port); From 3d51bf4cad10560e7cbd9221e9ba31ddd4233d7a Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 3 Nov 2022 15:23:42 -0400 Subject: [PATCH 019/252] Add diagnostic counters to mip parser. (#36) * Add diagnostic counters to mip parser. * Remove unnecessary #ifdef. * Add MIP_DIAG_ZERO macro to zero diagnostic counters. * Fix CMake not passing diagnostic flags to compiler. * Fix compiler errors related to diagnostics. * Add missing stats increment in mip_parser_process_written. * Print statistics in WatchImu example. * Add diagnostic printout to WatchImuC example. * Add diagnostic counters to command queue. * Change MIP_ENABLE_DIAGNOSTIC_COUNTERS -> MIP_ENABLE_DIAGNOSTICS. * Add MIP_ENABLE_DIAGNOSTICS to readme. * Merge line endings. * Removes semicolon warnings with void cast Co-authored-by: Rob Fisher --- CMakeLists.txt | 11 ++++- README.md | 1 + examples/watch_imu.c | 35 +++++++++++++ examples/watch_imu.cpp | 84 +++++++++++++++++++++++--------- src/mip/mip_cmdqueue.c | 94 ++++++++++++++++++++++++++++++++++- src/mip/mip_cmdqueue.h | 20 ++++++++ src/mip/mip_parser.c | 108 +++++++++++++++++++++++++++++++++++++---- src/mip/mip_parser.h | 27 +++++++++++ src/mip/mip_types.h | 18 +++++++ 9 files changed, 364 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ff3b16e8..8bd2697b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,6 +73,7 @@ configure_file(${VERSION_IN_FILE} ${VERSION_OUT_FILE}) option(MIP_USE_SERIAL "Build serial connection support into the library and examples" ON) option(MIP_USE_TCP "Build TCP connection support into the library and exampels" ON) option(MIP_USE_EXTRAS "Build extra support into the library including some things that might work at a higher level and use dynamically allocated memory" ON) +option(MIP_ENABLE_DIAGNOSTICS "Enable various diagnostic counters in the mip library for debugging." OFF) set(MIP_TIMESTAMP_TYPE "uint64_t" CACHE STRING "Override the type used for received data timestamps and timeouts (must be unsigned or at least 64 bits).") @@ -199,8 +200,16 @@ endif() add_library(mip ${ALL_MIP_SOURCES}) +# +# Preprocessor definitions +# + if(${MIP_TIMESTAMP_TYPE}) - add_compile_definitions("MIP_TIMESTAMP_TYPE=${MIP_TIMESTAMP_TYPE}") + target_compile_definitions(mip PUBLIC "MIP_TIMESTAMP_TYPE=${MIP_TIMESTAMP_TYPE}") +endif() + +if(${MIP_ENABLE_DIAGNOSTICS}) + target_compile_definitions(mip PUBLIC "MIP_ENABLE_DIAGNOSTICS") endif() # Disable windows defined min/max diff --git a/README.md b/README.md index a94297823..76cf18e65 100644 --- a/README.md +++ b/README.md @@ -88,6 +88,7 @@ The following options may be specified when configuring the build with CMake (e. * MIP_USE_SERIAL - Builds the included serial port library (default enabled). * MIP_USE_TCP - Builds the included socket library (default enabled). * MIP_USE_EXTRAS - Builds some higher level utility classes and functions that may use dynamic memory. +* MIP_ENABLE_DIAGNOSTICS - Adds some counters to various entities which can serve as a debugging aid. * BUILD_EXAMPLES - If enabled (`-DBUILD_EXAMPLES=ON`), the example projects will be built (default disabled). * BUILD_TESTING - If enabled (`-DBUILD_TESTING=ON`), the test programs in the /test directory will be compiled and linked. Run the tests with `ctest`. * BUILD_DOCUMENTATION - If enabled, the documentation will be built with doxygen. You must have doxygen installed. diff --git a/examples/watch_imu.c b/examples/watch_imu.c index 4c6d88a5f..1774a02a0 100644 --- a/examples/watch_imu.c +++ b/examples/watch_imu.c @@ -17,6 +17,7 @@ #include #include #include +#include #ifdef WIN32 #else @@ -223,6 +224,40 @@ int main(int argc, const char* argv[]) done: +#ifdef MIP_ENABLE_DIAGNOSTICS + printf( + "\nDiagnostics:\n" + "\n" + "Commands:\n" + " Sent: %" PRIu16 "\n" + " Acks: %" PRIu16 "\n" + " Nacks: %" PRIu16 "\n" + " Timeouts: %" PRIu16 "\n" + " Errors: %" PRIu16 "\n" + "\n" + "Parser:\n" + " Valid packets: %" PRIu32 "\n" + " Invalid packets: %" PRIu32 "\n" + " Timeouts: %" PRIu32 "\n" + "\n" + " Bytes read: %" PRIu32 "\n" + " Valid bytes: %" PRIu32 "\n" + " Unparsed bytes: %" PRIu32 "\n", + mip_cmd_queue_diagnostic_cmds_queued(mip_interface_cmd_queue(&device)), + mip_cmd_queue_diagnostic_cmd_acks(mip_interface_cmd_queue(&device)), + mip_cmd_queue_diagnostic_cmd_nacks(mip_interface_cmd_queue(&device)), + mip_cmd_queue_diagnostic_cmd_timeouts(mip_interface_cmd_queue(&device)), + mip_cmd_queue_diagnostic_cmd_errors(mip_interface_cmd_queue(&device)), + + mip_parser_diagnostic_valid_packets(mip_interface_parser(&device)), + mip_parser_diagnostic_invalid_packets(mip_interface_parser(&device)), + mip_parser_diagnostic_timeouts(mip_interface_parser(&device)), + mip_parser_diagnostic_bytes_read(mip_interface_parser(&device)), + mip_parser_diagnostic_packet_bytes(mip_interface_parser(&device)), + mip_parser_diagnostic_bytes_skipped(mip_interface_parser(&device)) + ); +#endif // MIP_ENABLE_DIAGNOSTICS + serial_port_close(&port); return result == MIP_ACK_OK ? 0 : 2; } diff --git a/examples/watch_imu.cpp b/examples/watch_imu.cpp index 83c9cef80..51beba8b9 100644 --- a/examples/watch_imu.cpp +++ b/examples/watch_imu.cpp @@ -14,6 +14,9 @@ #include #include +#define __STDC_FORMAT_MACROS 1 +#include + mip::data_sensor::ScaledAccel scaled_accel; void handlePacket(void*, const mip::Packet& packet, mip::Timestamp timestamp) @@ -56,19 +59,14 @@ void handleMag(void*, const mip::data_sensor::ScaledMag& data, mip::Timestamp ti } -int main(int argc, const char* argv[]) +int run(mip::DeviceInterface& device) { - try - { - std::unique_ptr utils = handleCommonArgs(argc, argv); - std::unique_ptr& device = utils->device; - mip::CmdResult result; // Get the base rate. uint16_t base_rate; - result = mip::commands_3dm::getBaseRate(*device, mip::data_sensor::DESCRIPTOR_SET, &base_rate); + result = mip::commands_3dm::getBaseRate(device, mip::data_sensor::DESCRIPTOR_SET, &base_rate); if( result != mip::CmdResult::ACK_OK ) return fprintf(stderr, "Failed to get base rate: %s (%d)\n", result.name(), result.value), 1; @@ -84,13 +82,13 @@ int main(int argc, const char* argv[]) { mip::data_sensor::DATA_MAG_SCALED, decimation }, }}; - result = mip::commands_3dm::writeMessageFormat(*device, mip::data_sensor::DESCRIPTOR_SET, descriptors.size(), descriptors.data()); + result = mip::commands_3dm::writeMessageFormat(device, mip::data_sensor::DESCRIPTOR_SET, descriptors.size(), descriptors.data()); if( result == mip::CmdResult::NACK_COMMAND_FAILED ) { // Failed to set message format - maybe this device doesn't have a magnetometer. // Try again without the last descriptor (scaled mag). - result = mip::commands_3dm::writeMessageFormat(*device, mip::data_sensor::DESCRIPTOR_SET, descriptors.size()-1, descriptors.data()); + result = mip::commands_3dm::writeMessageFormat(device, mip::data_sensor::DESCRIPTOR_SET, descriptors.size()-1, descriptors.data()); } if( result != mip::CmdResult::ACK_OK ) return fprintf(stderr, "Failed to set message format: %s (%d)\n", result.name(), result.value), 1; @@ -98,23 +96,23 @@ int main(int argc, const char* argv[]) // Register some callbacks. mip::DispatchHandler packetHandler; - device->registerPacketCallback<&handlePacket>(packetHandler, mip::C::MIP_DISPATCH_ANY_DATA_SET, false); + device.registerPacketCallback<&handlePacket>(packetHandler, mip::C::MIP_DISPATCH_ANY_DATA_SET, false); mip::DispatchHandler dataHandlers[4]; - device->registerFieldCallback<&handleAccel>(dataHandlers[0], mip::data_sensor::DESCRIPTOR_SET, mip::data_sensor::DATA_ACCEL_SCALED); - device->registerDataCallback(dataHandlers[1]); - device->registerDataCallback(dataHandlers[2]); - device->registerExtractor(dataHandlers[3], &scaled_accel); + device.registerFieldCallback<&handleAccel>(dataHandlers[0], mip::data_sensor::DESCRIPTOR_SET, mip::data_sensor::DATA_ACCEL_SCALED); + device.registerDataCallback(dataHandlers[1]); + device.registerDataCallback(dataHandlers[2]); + device.registerExtractor(dataHandlers[3], &scaled_accel); // Enable the data stream and resume the device. - result = mip::commands_3dm::writeDatastreamControl(*device, mip::data_sensor::DESCRIPTOR_SET, true); + result = mip::commands_3dm::writeDatastreamControl(device, mip::data_sensor::DESCRIPTOR_SET, true); if( result != mip::CmdResult::ACK_OK ) return fprintf(stderr, "Failed to enable datastream: %s (%d)\n", result.name(), result.value), 1; // Resume the device to ensure it's streaming. - result = mip::commands_base::resume(*device); + result = mip::commands_base::resume(device); if( result != mip::CmdResult::ACK_OK ) return fprintf(stderr, "Failed to resume device: %s (%d)\n", result.name(), result.value), 1; @@ -122,19 +120,24 @@ int main(int argc, const char* argv[]) const mip::Timestamp start_time = getCurrentTimestamp(); do { - device->update(); + device.update(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } while( getCurrentTimestamp() - start_time < 3000 ); - result = mip::commands_base::setIdle(*device); + result = mip::commands_base::setIdle(device); if( result != mip::CmdResult::ACK_OK ) return fprintf(stderr, "Failed to idle device: %s (%d)\n", result.name(), result.value), 1; - // mip::TdmCommands::EventControl ctrl; - // ctrl.mode = mip::TdmCommands::EventControl::Mode::ENABLED; + return 0; +} - return 0; +int main(int argc, const char* argv[]) +{ + std::unique_ptr utils; + try + { + utils = handleCommonArgs(argc, argv); } catch(const std::underflow_error& ex) { @@ -145,5 +148,42 @@ int main(int argc, const char* argv[]) fprintf(stderr, "Error: %s\n", ex.what()); return 1; } - return 0; + + const int result = run(*utils->device); + +#ifdef MIP_ENABLE_DIAGNOSTICS + printf( + "\nDiagnostics:\n" + "\n" + "Commands:\n" + " Sent: %" PRIu16 "\n" + " Acks: %" PRIu16 "\n" + " Nacks: %" PRIu16 "\n" + " Timeouts: %" PRIu16 "\n" + " Errors: %" PRIu16 "\n" + "\n" + "Parser:\n" + " Valid packets: %" PRIu32 "\n" + " Invalid packets: %" PRIu32 "\n" + " Timeouts: %" PRIu32 "\n" + "\n" + " Bytes read: %" PRIu32 "\n" + " Valid bytes: %" PRIu32 "\n" + " Unparsed bytes: %" PRIu32 "\n", + mip_cmd_queue_diagnostic_cmds_queued(&utils->device->cmdQueue()), + mip_cmd_queue_diagnostic_cmd_acks(&utils->device->cmdQueue()), + mip_cmd_queue_diagnostic_cmd_nacks(&utils->device->cmdQueue()), + mip_cmd_queue_diagnostic_cmd_timeouts(&utils->device->cmdQueue()), + mip_cmd_queue_diagnostic_cmd_errors(&utils->device->cmdQueue()), + + mip_parser_diagnostic_valid_packets(&utils->device->parser()), + mip_parser_diagnostic_invalid_packets(&utils->device->parser()), + mip_parser_diagnostic_timeouts(&utils->device->parser()), + mip_parser_diagnostic_bytes_read(&utils->device->parser()), + mip_parser_diagnostic_packet_bytes(&utils->device->parser()), + mip_parser_diagnostic_bytes_skipped(&utils->device->parser()) + ); +#endif // MIP_ENABLE_DIAGNOSTICS + + return result; } diff --git a/src/mip/mip_cmdqueue.c b/src/mip/mip_cmdqueue.c index a2f97d0ab..82f911b57 100644 --- a/src/mip/mip_cmdqueue.c +++ b/src/mip/mip_cmdqueue.c @@ -195,6 +195,12 @@ void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) { queue->_first_pending_cmd = NULL; queue->_base_timeout = base_reply_timeout; + + MIP_DIAG_ZERO(queue->_diag_cmds_queued); + MIP_DIAG_ZERO(queue->_diag_cmds_acked); + MIP_DIAG_ZERO(queue->_diag_cmds_nacked); + MIP_DIAG_ZERO(queue->_diag_cmds_timedout); + MIP_DIAG_ZERO(queue->_diag_cmds_failed); } //////////////////////////////////////////////////////////////////////////////// @@ -215,6 +221,8 @@ void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd) return; } + MIP_DIAG_INC(queue->_diag_cmds_queued, 1); + cmd->_status = MIP_STATUS_PENDING; queue->_first_pending_cmd = cmd; } @@ -378,6 +386,17 @@ void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet // This must be done last b/c it may trigger the thread which queued the command. // The command could go out of scope or its attributes inspected. pending->_status = status; + +#ifdef MIP_ENABLE_DIAGNOSTICS + if( mip_cmd_result_is_ack(status) ) + MIP_DIAG_INC(queue->_diag_cmds_acked, 1); + else if( mip_cmd_result_is_reply(status) ) + MIP_DIAG_INC(queue->_diag_cmds_nacked, 1); + else if( status == MIP_STATUS_TIMEDOUT ) + MIP_DIAG_INC(queue->_diag_cmds_timedout, 1); + else + MIP_DIAG_INC(queue->_diag_cmds_failed, 1); +#endif // MIP_ENABLE_DIAGNOSTICS } } } @@ -397,7 +416,7 @@ void mip_cmd_queue_clear(mip_cmd_queue* queue) queue->_first_pending_cmd = pending->_next; // This may deallocate the pending command in another thread (make sure to fetch the next cmd first). - pending->_status = MIP_STATUS_ERROR; + pending->_status = MIP_STATUS_CANCELLED; } } @@ -434,6 +453,8 @@ void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) // This must be last! pending->_status = MIP_STATUS_TIMEDOUT; + + MIP_DIAG_INC(queue->_diag_cmds_timedout, 1); } } } @@ -441,7 +462,8 @@ void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) //////////////////////////////////////////////////////////////////////////////// ///@brief Sets the base reply timeout for all commands. /// -/// THe base reply timeout is the minimum time to wait for a reply. +/// The base reply timeout is the minimum time to wait for a reply. +/// Takes effect for any commands queued after this function call. /// ///@param queue ///@param timeout @@ -460,3 +482,71 @@ timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) { return queue->_base_timeout; } + + +#ifdef MIP_ENABLE_DIAGNOSTICS + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands ever put into the queue. +/// +/// In most cases this is the number of commands sent to the device. +/// +uint16_t mip_cmd_queue_diagnostic_cmds_queued(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_queued; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands that have failed for any reason. +/// +uint16_t mip_cmd_queue_diagnostic_cmds_failed(const mip_cmd_queue* queue) +{ + return (uint16_t)queue->_diag_cmds_nacked + queue->_diag_cmds_failed + queue->_diag_cmds_timedout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of successful commands. +/// +/// Same as mip_cmd_queue_diagnostic_cmd_acks(). +/// +uint16_t mip_cmd_queue_diagnostic_cmds_successful(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_acked; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of successful commands. +/// +/// Same as mip_cmd_queue_diagnostic_cmds_successful(). +/// +uint16_t mip_cmd_queue_diagnostic_cmd_acks(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_acked; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands nack'd by the device. +/// +uint16_t mip_cmd_queue_diagnostic_cmd_nacks(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_nacked; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands that did not receive a reply within the +/// time limit. +/// +uint16_t mip_cmd_queue_diagnostic_cmd_timeouts(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_timedout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of command errors not caused by the device. +/// +uint16_t mip_cmd_queue_diagnostic_cmd_errors(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_failed; +} + +#endif // MIP_ENABLE_DIAGNOSTICS diff --git a/src/mip/mip_cmdqueue.h b/src/mip/mip_cmdqueue.h index d18a60620..99ff096a1 100644 --- a/src/mip/mip_cmdqueue.h +++ b/src/mip/mip_cmdqueue.h @@ -94,6 +94,15 @@ typedef struct mip_cmd_queue { mip_pending_cmd* _first_pending_cmd; timeout_type _base_timeout; + +#ifdef MIP_ENABLE_DIAGNOSTICS + uint16_t _diag_cmds_queued; ///<@private Number of queued commands. + uint16_t _diag_cmds_acked; ///<@private Number of successful commands. + uint8_t _diag_cmds_nacked; ///<@private Number of commands failed by the device. + uint8_t _diag_cmds_timedout; ///<@private Number of commands that have timed out. + uint8_t _diag_cmds_failed; ///<@private Number of commands failed due to errors not from the device. +#endif // MIP_ENABLE_DIAGNOSTICS + } mip_cmd_queue; void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout); @@ -110,6 +119,17 @@ timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue); void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp); +#ifdef MIP_ENABLE_DIAGNOSTICS +uint16_t mip_cmd_queue_diagnostic_cmds_queued(const mip_cmd_queue* queue); +uint16_t mip_cmd_queue_diagnostic_cmds_failed(const mip_cmd_queue* queue); +uint16_t mip_cmd_queue_diagnostic_cmds_successful(const mip_cmd_queue* queue); + +uint16_t mip_cmd_queue_diagnostic_cmd_acks(const mip_cmd_queue* queue); +uint16_t mip_cmd_queue_diagnostic_cmd_nacks(const mip_cmd_queue* queue); +uint16_t mip_cmd_queue_diagnostic_cmd_timeouts(const mip_cmd_queue* queue); +uint16_t mip_cmd_queue_diagnostic_cmd_errors(const mip_cmd_queue* queue); +#endif // MIP_ENABLE_DIAGNOSTICS + ///@} ///@} ///@} diff --git a/src/mip/mip_parser.c b/src/mip/mip_parser.c index dc4889c67..9e7848d37 100644 --- a/src/mip/mip_parser.c +++ b/src/mip/mip_parser.c @@ -31,14 +31,11 @@ /// void mip_parser_init(mip_parser* parser, uint8_t* buffer, size_t buffer_size, mip_packet_callback callback, void* callback_object, timestamp_type timeout) { - parser->_start_time = 0; - parser->_timeout = timeout; - - parser->_result_buffer[0] = 0; - byte_ring_init(&parser->_ring, buffer, buffer_size); - parser->_expected_length = MIPPARSER_RESET_LENGTH; + parser->_timeout = timeout; + + mip_parser_reset(parser); parser->_callback = callback; parser->_callback_object = callback_object; @@ -59,6 +56,12 @@ void mip_parser_reset(mip_parser* parser) parser->_result_buffer[0] = 0; parser->_start_time = 0; byte_ring_clear(&parser->_ring); + + MIP_DIAG_ZERO(parser->_diag_bytes_read); + MIP_DIAG_ZERO(parser->_diag_packet_bytes); + MIP_DIAG_ZERO(parser->_diag_valid_packets); + MIP_DIAG_ZERO(parser->_diag_invalid_packets); + MIP_DIAG_ZERO(parser->_diag_timeouts); } //////////////////////////////////////////////////////////////////////////////// @@ -115,14 +118,19 @@ remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer { if( byte_ring_count(&parser->_ring) > 0 ) byte_ring_pop(&parser->_ring, 1); + parser->_expected_length = MIPPARSER_RESET_LENGTH; + + MIP_DIAG_INC(parser->_diag_timeouts, 1); } unsigned int num_packets = 0; do { // Copy as much data as will fit in the ring buffer. - byte_ring_copy_from_and_update(&parser->_ring, &input_buffer, &input_count); + size_t count = byte_ring_copy_from_and_update(&parser->_ring, &input_buffer, &input_count); + + MIP_DIAG_INC(parser->_diag_bytes_read, count); mip_packet packet; while( mip_parser_parse_one_packet_from_ring(parser, &packet, timestamp) ) @@ -136,7 +144,9 @@ remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer if( stop ) { // Pull more data from the input buffer if possible. - byte_ring_copy_from_and_update(&parser->_ring, &input_buffer, &input_count); + size_t count = byte_ring_copy_from_and_update(&parser->_ring, &input_buffer, &input_count); + + MIP_DIAG_INC(parser->_diag_bytes_read, count); return -(remaining_count)input_count; } @@ -200,7 +210,7 @@ bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packe uint_least16_t packet_length = parser->_expected_length; parser->_expected_length = MIPPARSER_RESET_LENGTH; // Reset parsing state - byte_ring_copy_to(&parser->_ring, parser->_result_buffer, packet_length); + byte_ring_copy_to(&parser->_ring, parser->_result_buffer, packet_length); mip_packet_from_buffer(packet_out, parser->_result_buffer, packet_length); @@ -208,12 +218,17 @@ bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packe { // Invalid packet, drop just the first sync byte and restart. byte_ring_pop(&parser->_ring, 1); + + MIP_DIAG_INC(parser->_diag_invalid_packets, 1); } else // Checksum is valid { // Discard the packet bytes from the ring buffer since a copy was made. byte_ring_pop(&parser->_ring, packet_length); + MIP_DIAG_INC(parser->_diag_valid_packets, 1); + MIP_DIAG_INC(parser->_diag_packet_bytes, packet_length); + // Successfully parsed a packet. return true; } @@ -356,11 +371,86 @@ size_t mip_parser_get_write_ptr(mip_parser* parser, uint8_t** const ptr_out) /// void mip_parser_process_written(mip_parser* parser, size_t count, timestamp_type timestamp, unsigned int max_packets) { + MIP_DIAG_INC(parser->_diag_bytes_read, count); + byte_ring_notify_written(&parser->_ring, count); mip_parser_parse(parser, NULL, 0, timestamp, max_packets); } +#ifdef MIP_ENABLE_DIAGNOSTICS + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the total number of bytes read from the user input buffer. +/// +/// This includes data read into the internal ring buffer but not yet seen by +/// the parser. Ensure all packets have been processed by the parser before +/// comparing against the packet_bytes counter. +/// +uint32_t mip_parser_diagnostic_bytes_read(const mip_parser* parser) +{ + return parser->_diag_bytes_read; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets total the number of bytes that have been parsed into valid +/// packets. +/// +/// This is a summation of the total length of every valid mip packet emitted by +/// the parser. +/// +uint32_t mip_parser_diagnostic_packet_bytes(const mip_parser* parser) +{ + return parser->_diag_packet_bytes; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the total number of bytes which weren't part of a valid packet. +/// +/// This is the difference between the "packet bytes" and "bytes read" counters. +/// +uint32_t mip_parser_diagnostic_bytes_skipped(const mip_parser* parser) +{ + return parser->_diag_bytes_read - parser->_diag_packet_bytes; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the total number of valid packets emitted by the parser. +/// +uint32_t mip_parser_diagnostic_valid_packets(const mip_parser* parser) +{ + return parser->_diag_valid_packets; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the total number of packets that failed the checksum check. +/// +/// These invalid packets are not emitted by the parser and are not included in +/// the "valid packets" or "packet bytes" counters. +/// +uint16_t mip_parser_diagnostic_invalid_packets(const mip_parser* parser) +{ + return parser->_diag_invalid_packets; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the total number of times a packet timed out waiting for more +/// data. +/// +/// Packets may time out under the following conditions: +///@li The connection is interrupted +///@li The length byte is corrupted to make the packet look longer +///@li The connection bandwidth and/or latency is too low +/// +uint16_t mip_parser_diagnostic_timeouts(const mip_parser* parser) +{ + return parser->_diag_timeouts; +} + +#endif // MIP_ENABLE_DIAGNOSTICS + + //////////////////////////////////////////////////////////////////////////////// ///@brief Computes an appropriate packet timeout for a given serial baud rate. /// diff --git a/src/mip/mip_parser.h b/src/mip/mip_parser.h index 29fa20d36..74595ee4b 100644 --- a/src/mip/mip_parser.h +++ b/src/mip/mip_parser.h @@ -61,6 +61,15 @@ typedef struct mip_parser byte_ring_state _ring; ///<@private Ring buffer which holds data being parsed. User-specified backing buffer and size. mip_packet_callback _callback; ///<@private Callback called when a valid packet is parsed. Can be NULL. void* _callback_object; ///<@private User-specified pointer passed to the callback function. + +#ifdef MIP_ENABLE_DIAGNOSTICS + uint32_t _diag_bytes_read; ///<@private Counts bytes read from the user input buffer. + uint32_t _diag_packet_bytes; ///<@private Counts bytes parsed into valid packets. + uint32_t _diag_valid_packets; ///<@private Counts packets successfully parsed. + uint16_t _diag_invalid_packets; ///<@private Counts invalid packets encountered (bad checksums). + uint16_t _diag_timeouts; ///<@private Counts packet timeouts. +#endif // MIP_ENABLE_DIAGNOSTICS + } mip_parser; @@ -91,6 +100,24 @@ void* mip_parser_callback_object(const mip_parser* parser); timestamp_type mip_parser_last_packet_timestamp(const mip_parser* parser); + +// +// Diagnostics +// + +#ifdef MIP_ENABLE_DIAGNOSTICS + +uint32_t mip_parser_diagnostic_bytes_read(const mip_parser* parser); +uint32_t mip_parser_diagnostic_packet_bytes(const mip_parser* parser); +uint32_t mip_parser_diagnostic_bytes_skipped(const mip_parser* parser); + +uint32_t mip_parser_diagnostic_valid_packets(const mip_parser* parser); +uint16_t mip_parser_diagnostic_invalid_packets(const mip_parser* parser); +uint16_t mip_parser_diagnostic_timeouts(const mip_parser* parser); + +#endif // MIP_ENABLE_DIAGNOSTICS + + // // Misc // diff --git a/src/mip/mip_types.h b/src/mip/mip_types.h index a32b6def8..4bcc4528c 100644 --- a/src/mip/mip_types.h +++ b/src/mip/mip_types.h @@ -34,6 +34,24 @@ typedef int_least16_t remaining_count; typedef timestamp_type timeout_type; + +#ifdef MIP_ENABLE_DIAGNOSTICS + +// Saturating addition +#define MIP_DIAG_INC(counter, amount) do { if (counter + amount < counter) counter = -1; else counter += amount; } while(false) + +#define MIP_DIAG_ZERO(counter) counter = 0 + +#else // MIP_ENABLE_DIAGNOSTICS + +// Do nothing if diagnostic counters diabled. Cast amount to void to avoid "unused local variable" warnings. +#define MIP_DIAG_INC(counter, amount) (void)amount + +#define MIP_DIAG_ZERO(counter) (void)0 + +#endif // MIP_ENABLE_DIAGNOSTICS + + #ifdef __cplusplus } // extern "C" From b1e62780dfdfe6a9166480e9b05a8fdc051e6dd9 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Thu, 3 Nov 2022 16:24:29 -0400 Subject: [PATCH 020/252] MIP SDK Logger (#27) * Checkin * checkin * Adds logging functionality * Reverts spaces for mip_interface * Updates README * Updates based on PR comments * Changes noop from do while to void cast * Updates files to mip_logging, and adds MIP_LOG_INIT macro * Removes mip_logger.c * Renames LoggingLevel to LogLevel and removes test log * Removes C++, and adds ability to set max log level at compile time * Removes namespaces and moves enums to defines in logging * Checkin? * Updates based on PR feedback * Reverts some pointless changes --- CMakeLists.txt | 22 +++++- README.md | 2 + examples/CMakeLists.txt | 2 +- examples/example_utils.cpp | 33 +++++++++ examples/watch_imu.c | 21 ++++++ src/mip/mip_logging.c | 80 ++++++++++++++++++++ src/mip/mip_logging.h | 141 ++++++++++++++++++++++++++++++++++++ src/mip/utils/serial_port.c | 31 +++++++- src/mip/utils/serial_port.h | 1 + 9 files changed, 326 insertions(+), 7 deletions(-) create mode 100644 src/mip/mip_logging.c create mode 100644 src/mip/mip_logging.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 8bd2697b0..79cefcd3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,6 +75,9 @@ option(MIP_USE_TCP "Build TCP connection support into the library and exampel option(MIP_USE_EXTRAS "Build extra support into the library including some things that might work at a higher level and use dynamically allocated memory" ON) option(MIP_ENABLE_DIAGNOSTICS "Enable various diagnostic counters in the mip library for debugging." OFF) +option(MIP_ENABLE_LOGGING "Build with logging functions enabled" ON) +set(MIP_LOGGING_MAX_LEVEL "" CACHE STRING "Max log level the SDK is allowed to log. If this is defined, any log level logged at a higher level than this will result in a noop regardless of runtime configuration.") + set(MIP_TIMESTAMP_TYPE "uint64_t" CACHE STRING "Override the type used for received data timestamps and timeouts (must be unsigned or at least 64 bits).") option(BUILD_PACKAGE "Whether to build a package from the resulting binaries" OFF) @@ -182,6 +185,13 @@ if(MIP_USE_EXTRAS) ) endif() +# Logging is a little weird since we need to install the header no matter what +list(APPEND MIP_SOURCES "${MIP_DIR}/mip_logging.h") +if(MIP_ENABLE_LOGGING) + list(APPEND MIP_SOURCES "${MIP_DIR}/mip_logging.c") + list(APPEND MIP_DEFINES "MIP_ENABLE_LOGGING") +endif() + set(ALL_MIP_SOURCES ${MIPDEF_SOURCES} ${MIPDEF_CPP_SOURCES} @@ -204,17 +214,21 @@ add_library(mip ${ALL_MIP_SOURCES}) # Preprocessor definitions # +if(${MIP_LOGGING_MAX_LEVEL}) + list(APPEND MIP_DEFINES "MIP_LOGGING_MAX_LEVEL=${MIP_LOGGING_MAX_LEVEL}") +endif() + if(${MIP_TIMESTAMP_TYPE}) - target_compile_definitions(mip PUBLIC "MIP_TIMESTAMP_TYPE=${MIP_TIMESTAMP_TYPE}") + list(APPEND MIP_DEFINES "MIP_TIMESTAMP_TYPE=${MIP_TIMESTAMP_TYPE}") endif() if(${MIP_ENABLE_DIAGNOSTICS}) - target_compile_definitions(mip PUBLIC "MIP_ENABLE_DIAGNOSTICS") + list(APPEND MIP_DEFINES "MIP_ENABLE_DIAGNOSTICS") endif() # Disable windows defined min/max if(WIN32) - target_compile_definitions(mip PUBLIC "NOMINMAX=1") + list(APPEND MIP_DEFINES "NOMINMAX=1") endif() if(MSVC) @@ -223,6 +237,8 @@ else() target_compile_options(mip PRIVATE -Wall -Wextra) endif() +target_compile_definitions(mip PUBLIC "${MIP_DEFINES}") + # # TESTING # diff --git a/README.md b/README.md index 76cf18e65..011fde1c0 100644 --- a/README.md +++ b/README.md @@ -88,6 +88,8 @@ The following options may be specified when configuring the build with CMake (e. * MIP_USE_SERIAL - Builds the included serial port library (default enabled). * MIP_USE_TCP - Builds the included socket library (default enabled). * MIP_USE_EXTRAS - Builds some higher level utility classes and functions that may use dynamic memory. +* MIP_ENABLE_LOGGING - Builds logging functionality into the library. The user is responsible for configuring a logging callback (default enabled) +* MIP_LOGGING_MAX_LEVEL - Max log level the SDK is allowed to log. If this is defined, any log level logged at a higher level than this will result in a noop regardless of runtime configuration. Useful if you want some logs, but do not want the overhead of the higher level functions compiled into the code * MIP_ENABLE_DIAGNOSTICS - Adds some counters to various entities which can serve as a debugging aid. * BUILD_EXAMPLES - If enabled (`-DBUILD_EXAMPLES=ON`), the example projects will be built (default disabled). * BUILD_TESTING - If enabled (`-DBUILD_TESTING=ON`), the test programs in the /test directory will be compiled and linked. Run the tests with `ctest`. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 7a29e5ec9..0e38cf416 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -19,7 +19,7 @@ if(MIP_USE_EXTRAS) set(EXTRAS_DEFS "MIP_USE_EXTRAS") endif() -set(MIP_EXAMPLE_DEFS ${SERIAL_DEFS} ${TCP_DEFS} ${EXTRAS_DEFS}) +set(MIP_EXAMPLE_DEFS ${MIP_DEFINES} ${SERIAL_DEFS} ${TCP_DEFS} ${EXTRAS_DEFS}) # C++ examples need either serial or TCP support if(MIP_USE_SERIAL OR MIP_USE_TCP) diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index 53f5771de..e0d010cdc 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -1,6 +1,10 @@ #include #include +#include + +#include + #include "example_utils.hpp" @@ -16,6 +20,32 @@ mip::Timestamp getCurrentTimestamp() return duration_cast( steady_clock::now().time_since_epoch() ).count(); } +void customLog(void* user, mip_log_level level, const char* fmt, va_list args) +{ + // Convert the varargs into a string + std::string log; + va_list args_copy; + va_copy(args_copy, args); + const int required_len = vsnprintf(nullptr, 0, fmt, args_copy); + if (required_len >= 0) + { + log.resize(required_len); + vsnprintf(&log[0], required_len + 1, fmt, args); + } + va_end(args_copy); + + // Print to the proper stream + switch (level) + { + case MIP_LOG_LEVEL_FATAL: + case MIP_LOG_LEVEL_ERROR: + std::cerr << log; + break; + default: + std::cout << log; + break; + } +} std::unique_ptr openFromArgs(const std::string& port_or_hostname, const std::string& baud_or_port, const std::string& binary_file_path) { @@ -79,6 +109,9 @@ std::unique_ptr openFromArgs(const std::string& port_or_hostname, std::unique_ptr handleCommonArgs(int argc, const char* argv[], int maxArgs) { + // Setup the logger for the MIP SDK + MIP_LOG_INIT(&customLog, MIP_LOG_LEVEL_DEBUG, nullptr); + if( argc < 3 || argc > maxArgs ) { throw std::underflow_error("Usage error"); diff --git a/examples/watch_imu.c b/examples/watch_imu.c index 1774a02a0..048c1ccf5 100644 --- a/examples/watch_imu.c +++ b/examples/watch_imu.c @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -13,10 +14,13 @@ #include +#include + #include #include #include #include +#include #include #ifdef WIN32 @@ -32,6 +36,20 @@ uint8_t parse_buffer[1024]; mip_interface device; mip_sensor_scaled_accel_data scaled_accel; +void customLog(void* user, mip_log_level level, const char* fmt, va_list args) +{ + switch (level) + { + case MIP_LOG_LEVEL_FATAL: + case MIP_LOG_LEVEL_ERROR: + vfprintf(stderr, fmt, args); + break; + default: + vprintf(fmt, args); + break; + } +} + void handlePacket(void* unused, const mip_packet* packet, timestamp_type timestamp) { (void)unused; @@ -140,6 +158,9 @@ int main(int argc, const char* argv[]) if( baudrate == 0 ) return usage(argv[0]); + // Initialize the MIP logger before opening the port so we can print errors if they occur + MIP_LOG_INIT(&customLog, MIP_LOG_LEVEL_INFO, NULL); + if( !open_port(argv[1], baudrate) ) return 1; diff --git a/src/mip/mip_logging.c b/src/mip/mip_logging.c new file mode 100644 index 000000000..23592293a --- /dev/null +++ b/src/mip/mip_logging.c @@ -0,0 +1,80 @@ + +#include "mip_logging.h" + +#include + +//////////////////////////////////////////////////////////////////////////////// +///@brief Global logging callback. Do not access directly +mip_log_callback mip_log_callback_ = NULL; + +//////////////////////////////////////////////////////////////////////////////// +///@brief Global logging level. Do not access directly +mip_log_level mip_log_level_ = MIP_LOG_LEVEL_OFF; + +//////////////////////////////////////////////////////////////////////////////// +///@brief Global logging user data. Do not access directly +void* mip_log_user_data_ = NULL; + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initializes the logger with a callback and user data. +/// Call MIP_LOG_INIT instead of using this function directly. +/// This function does not have to be called unless the user wants logging +/// +///@param callback The callback to execute when there is data to log +///@param level The level that the MIP SDK should log at +///@param user User data that will be passed to the callback every time it is excuted +/// +void mip_logging_init(mip_log_callback callback, mip_log_level level, void* user) +{ + mip_log_callback_ = callback; + mip_log_level_ = level; + mip_log_user_data_ = user; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the currently active logging callback +/// +///@return The currently active logging callback +/// +mip_log_callback mip_logging_callback() +{ + return mip_log_callback_; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the currently active logging level +/// +///@return The currently active logging level +/// +mip_log_level mip_logging_level() +{ + return mip_log_level_; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the currently active logging user data +/// +///@return The currently active logging user data +/// +void* mip_logging_user_data() +{ + return mip_log_user_data_; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Internal log function called by logging macros. +/// Call MIP_LOG_* macros instead of using this function directly +///@copydetails mip::C::mip_log_callback +/// +void mip_logging_log(mip_log_level level, const char* fmt, ...) +{ + const mip_log_callback logging_callback = mip_logging_callback(); + const mip_log_level logging_level = mip_logging_level(); + if (logging_callback != NULL && logging_level >= level) + { + va_list args; + va_start(args, fmt); + logging_callback(mip_logging_user_data(), level, fmt, args); + va_end(args); + } +} \ No newline at end of file diff --git a/src/mip/mip_logging.h b/src/mip/mip_logging.h new file mode 100644 index 000000000..5182d4810 --- /dev/null +++ b/src/mip/mip_logging.h @@ -0,0 +1,141 @@ +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_c +///@{ +//////////////////////////////////////////////////////////////////////////////// +///@defgroup mip_logging Mip Logging [C] +/// +///@brief High-level C functions for logging information from within the MIP SDK +/// +/// This module contains functions that allow the MIP SDK to log information +/// and allows users to override the logging functions +/// +///@{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief Logging level enum +/// +typedef uint8_t mip_log_level; +#define MIP_LOG_LEVEL_OFF 0 ///< Signifies that the log is turned off +#define MIP_LOG_LEVEL_FATAL 1 ///< Fatal logs are logged when an unrecoverable error occurs +#define MIP_LOG_LEVEL_ERROR 2 ///< Error logs are logged when an error occurs +#define MIP_LOG_LEVEL_WARN 3 ///< Warning logs are logged when something concerning happens that may or not be a mistake +#define MIP_LOG_LEVEL_INFO 4 ///< Info logs are logged when some general info needs to be conveyed to the user +#define MIP_LOG_LEVEL_DEBUG 5 ///< Debug logs are logged for debug purposes. +#define MIP_LOG_LEVEL_TRACE 6 ///< Trace logs are logged in similar cases to debug logs but can be logged in tight loops + +//////////////////////////////////////////////////////////////////////////////// +///@brief Callback function typedef for custom logging behavior. +/// +///@param level The log level that this log should be logged at +///@param fmt printf style format string +///@param ... Variadic args used to populate the fmt string +/// +typedef void (*mip_log_callback)(void* user, mip_log_level level, const char* fmt, va_list args); + +void mip_logging_init(mip_log_callback callback, mip_log_level level, void* user); + +mip_log_callback mip_logging_callback(); +mip_log_level mip_logging_level(); +void* mip_logging_user_data(); + +void mip_logging_log(mip_log_level level, const char* fmt, ...); + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to initialize the MIP logger. +/// This function does not need to be called unless the user wants logging +/// +///@param callback The callback to execute when there is data to log +///@param level The level that the MIP SDK should log at +///@param user User data that will be passed to the callback every time it is excuted +/// +#ifdef MIP_ENABLE_LOGGING +#define MIP_LOG_INIT(callback, level, user) mip_logging_init(callback, level, user) +#else +#define MIP_LOG_INIT(callback, level, user) (void)0 +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to log data inside the MIP SDK. Prefer specific +/// log level functions like MIP_LOG_INFO, etc. when possible. +///@copydetails mip::C::mip_log_callback +/// +#ifdef MIP_ENABLE_LOGGING +#define MIP_LOG_LOG(level, fmt, ...) mip_logging_log(level, fmt, __VA_ARGS__) +#else +#define MIP_LOG_LOG(level, fmt, ...) (void)0 +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to log data inside the MIP SDK at fatal level +/// +///@param context Context of what called this function. Could be a MIP device, serial connection, etc. +///@param fmt printf style format string +///@param ... Variadic args used to populate the fmt string +/// +#if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_FATAL +#define MIP_LOG_FATAL(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_FATAL, fmt, __VA_ARGS__) +#else +#define MIP_LOG_FATAL(fmt, ...) (void)0 +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to log data inside the MIP SDK at error level +///@copydetails mip::C::MIP_LOG_FATAL +#if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_ERROR +#define MIP_LOG_ERROR(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_ERROR, fmt, __VA_ARGS__) +#else +#define MIP_LOG_ERROR(fmt, ...) (void)0 +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to log data inside the MIP SDK at warn level +///@copydetails mip::C::MIP_LOG_FATAL +#if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_WARN +#define MIP_LOG_WARN(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_WARN, fmt, __VA_ARGS__) +#else +#define MIP_LOG_WARN(fmt, ...) (void)0 +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to log data inside the MIP SDK at info level +///@copydetails mip::C::MIP_LOG_FATAL +#if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_INFO +#define MIP_LOG_INFO(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_INFO, fmt, __VA_ARGS__) +#else +#define MIP_LOG_INFO(fmt, ...) (void)0 +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to log data inside the MIP SDK at debug level +///@copydetails mip::C::MIP_LOG_FATAL +#if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_DEBUG +#define MIP_LOG_DEBUG(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_DEBUG, fmt, __VA_ARGS__) +#else +#define MIP_LOG_DEBUG(fmt, ...) (void)0 +#endif + +//////////////////////////////////////////////////////////////////////////////// +///@brief Helper macro used to log data inside the MIP SDK at trace level +///@copydetails mip::C::MIP_LOG_FATAL +#if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_TRACE +#define MIP_LOG_TRACE(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_TRACE, fmt, __VA_ARGS__) +#else +#define MIP_LOG_TRACE(fmt, ...) (void)0 +#endif + +///@} +///@} +//////////////////////////////////////////////////////////////////////////////// + +#ifdef __cplusplus +} // extern "C" +#endif \ No newline at end of file diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index bcd047c1f..5da24d3a4 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -1,4 +1,6 @@ +#include "mip/mip_logging.h" + #include "serial_port.h" #define COM_PORT_BUFFER_SIZE 0x200 @@ -57,6 +59,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) if(port_str == NULL) return false; + MIP_LOG_DEBUG("Opening serial port %s at %d\n", port_str, baudrate); #ifdef WIN32 BOOL ready; DCB dcb; @@ -67,13 +70,16 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Check for an invalid handle if(port->handle == INVALID_HANDLE_VALUE) { - printf( "\nError: Unable to open com port (%d)\n", GetLastError( ) ); + MIP_LOG_ERROR("Unable to open com port (%d)\n", GetLastError()); return false; } //Setup the com port buffer sizes if(SetupComm(port->handle, COM_PORT_BUFFER_SIZE, COM_PORT_BUFFER_SIZE) == 0) + { + MIP_LOG_ERROR("Unable to setup com port buffer size (%d)\n", GetLastError()); return false; + } //Set the timeouts COMMTIMEOUTS timeouts; @@ -94,6 +100,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Close the serial port, mutex, and exit if(!ready) { + MIP_LOG_ERROR("Unable to get com state\n"); CloseHandle(port->handle); return false; } @@ -110,6 +117,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) //Close the serial port and exit if(!ready) { + MIP_LOG_ERROR("Unable to set com state\n"); CloseHandle(port->handle); return false; } @@ -120,16 +128,23 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) if (port->handle < 0) { + MIP_LOG_ERROR("Unable to open port (%d): %s\n", errno, strerror(errno)); return false; } // Set up baud rate and other serial device options struct termios serial_port_settings; if (tcgetattr(port->handle, &serial_port_settings) < 0) + { + MIP_LOG_ERROR("Unable to get serial port settings (%d): %s\n", errno, strerror(errno)); return false; + } if (cfsetispeed(&serial_port_settings, baud_rate_to_speed(baudrate)) < 0 || cfsetospeed(&serial_port_settings, baud_rate_to_speed(baudrate)) < 0) + { + MIP_LOG_ERROR("Unable to set baud rate (%d): %s\n", errno, strerror(errno)); return false; + } // Other serial settings to match MSCL serial_port_settings.c_cflag |= (tcflag_t)(CLOCAL | CREAD); @@ -143,8 +158,11 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) // Persist the settings if(tcsetattr(port->handle, TCSANOW, &serial_port_settings) < 0) + { + MIP_LOG_ERROR("Unable to save serial port settings (%d): %s\n", errno, strerror(errno)); return false; - + } + // Flush any waiting data tcflush(port->handle, TCIOFLUSH); @@ -199,7 +217,9 @@ bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, if(*bytes_written == num_bytes) return true; - + else if(*bytes_written == (size_t)-1) + MIP_LOG_ERROR("Failed to write serial data (%d): %s\n", errno, strerror(errno)); + #endif return false; @@ -240,9 +260,14 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai ssize_t local_bytes_read = read(port->handle, buffer, num_bytes); if(local_bytes_read == (ssize_t)-1 && errno != EAGAIN) + { + MIP_LOG_ERROR("Failed to read serial data (%d): %s\n", errno, strerror(errno)); return false; + } if(local_bytes_read >= 0) + { *bytes_read = local_bytes_read; + } } #endif diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index 8354974cd..d8850b78d 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include From 5a60e4c2b5274803760ce63a1160934d1f9a55bc Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 8 Nov 2022 11:04:18 -0500 Subject: [PATCH 021/252] Generate MIP definitions from branches/fix_tuple@53851. (#41) --- src/mip/definitions/data_filter.hpp | 66 ++++++++++++++--------------- src/mip/definitions/data_gnss.hpp | 16 +++---- src/mip/definitions/data_sensor.hpp | 28 ++++++------ 3 files changed, 55 insertions(+), 55 deletions(-) diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index d6a9b1599..c47f70cb0 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -418,7 +418,7 @@ struct AttitudeQuaternion auto as_tuple() const { - return std::make_tuple(q,valid_flags); + return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND @@ -455,7 +455,7 @@ struct AttitudeDcm auto as_tuple() const { - return std::make_tuple(dcm,valid_flags); + return std::make_tuple(dcm[0],dcm[1],dcm[2],dcm[3],dcm[4],dcm[5],dcm[6],dcm[7],dcm[8],valid_flags); } float dcm[9] = {0}; ///< Matrix elements in row-major order. @@ -512,7 +512,7 @@ struct GyroBias auto as_tuple() const { - return std::make_tuple(bias,valid_flags); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } float bias[3] = {0}; ///< (x, y, z) [radians/second] @@ -539,7 +539,7 @@ struct AccelBias auto as_tuple() const { - return std::make_tuple(bias,valid_flags); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } float bias[3] = {0}; ///< (x, y, z) [meters/second^2] @@ -654,7 +654,7 @@ struct GyroBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert,valid_flags); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } float bias_uncert[3] = {0}; ///< (x,y,z) [radians/sec] @@ -681,7 +681,7 @@ struct AccelBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert,valid_flags); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } float bias_uncert[3] = {0}; ///< (x,y,z) [meters/second^2] @@ -771,7 +771,7 @@ struct LinearAccel auto as_tuple() const { - return std::make_tuple(accel,valid_flags); + return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } float accel[3] = {0}; ///< (x,y,z) [meters/second^2] @@ -798,7 +798,7 @@ struct GravityVector auto as_tuple() const { - return std::make_tuple(gravity,valid_flags); + return std::make_tuple(gravity[0],gravity[1],gravity[2],valid_flags); } float gravity[3] = {0}; ///< (x, y, z) [meters/second^2] @@ -825,7 +825,7 @@ struct CompAccel auto as_tuple() const { - return std::make_tuple(accel,valid_flags); + return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } float accel[3] = {0}; ///< (x,y,z) [meters/second^2] @@ -852,7 +852,7 @@ struct CompAngularRate auto as_tuple() const { - return std::make_tuple(gyro,valid_flags); + return std::make_tuple(gyro[0],gyro[1],gyro[2],valid_flags); } float gyro[3] = {0}; ///< (x, y, z) [radians/second] @@ -879,7 +879,7 @@ struct QuaternionAttitudeUncertainty auto as_tuple() const { - return std::make_tuple(q,valid_flags); + return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } float q[4] = {0}; ///< [dimensionless] @@ -1006,7 +1006,7 @@ struct AccelScaleFactor auto as_tuple() const { - return std::make_tuple(scale_factor,valid_flags); + return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1033,7 +1033,7 @@ struct AccelScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(scale_factor_uncert,valid_flags); + return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1060,7 +1060,7 @@ struct GyroScaleFactor auto as_tuple() const { - return std::make_tuple(scale_factor,valid_flags); + return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1087,7 +1087,7 @@ struct GyroScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(scale_factor_uncert,valid_flags); + return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1114,7 +1114,7 @@ struct MagBias auto as_tuple() const { - return std::make_tuple(bias,valid_flags); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } float bias[3] = {0}; ///< (x,y,z) [Gauss] @@ -1141,7 +1141,7 @@ struct MagBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert,valid_flags); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } float bias_uncert[3] = {0}; ///< (x,y,z) [Gauss] @@ -1260,7 +1260,7 @@ struct AntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(offset,valid_flags); + return std::make_tuple(offset[0],offset[1],offset[2],valid_flags); } float offset[3] = {0}; ///< (x,y,z) [meters] @@ -1287,7 +1287,7 @@ struct AntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(offset_uncert,valid_flags); + return std::make_tuple(offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } float offset_uncert[3] = {0}; ///< (x,y,z) [meters] @@ -1316,7 +1316,7 @@ struct MultiAntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(receiver_id,offset,valid_flags); + return std::make_tuple(receiver_id,offset[0],offset[1],offset[2],valid_flags); } uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached @@ -1344,7 +1344,7 @@ struct MultiAntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(receiver_id,offset_uncert,valid_flags); + return std::make_tuple(receiver_id,offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached @@ -1374,7 +1374,7 @@ struct MagnetometerOffset auto as_tuple() const { - return std::make_tuple(hard_iron,valid_flags); + return std::make_tuple(hard_iron[0],hard_iron[1],hard_iron[2],valid_flags); } float hard_iron[3] = {0}; ///< (x,y,z) [Gauss] @@ -1403,7 +1403,7 @@ struct MagnetometerMatrix auto as_tuple() const { - return std::make_tuple(soft_iron,valid_flags); + return std::make_tuple(soft_iron[0],soft_iron[1],soft_iron[2],soft_iron[3],soft_iron[4],soft_iron[5],soft_iron[6],soft_iron[7],soft_iron[8],valid_flags); } float soft_iron[9] = {0}; ///< Row-major [dimensionless] @@ -1430,7 +1430,7 @@ struct MagnetometerOffsetUncertainty auto as_tuple() const { - return std::make_tuple(hard_iron_uncertainty,valid_flags); + return std::make_tuple(hard_iron_uncertainty[0],hard_iron_uncertainty[1],hard_iron_uncertainty[2],valid_flags); } float hard_iron_uncertainty[3] = {0}; ///< (x,y,z) [Gauss] @@ -1457,7 +1457,7 @@ struct MagnetometerMatrixUncertainty auto as_tuple() const { - return std::make_tuple(soft_iron_uncertainty,valid_flags); + return std::make_tuple(soft_iron_uncertainty[0],soft_iron_uncertainty[1],soft_iron_uncertainty[2],soft_iron_uncertainty[3],soft_iron_uncertainty[4],soft_iron_uncertainty[5],soft_iron_uncertainty[6],soft_iron_uncertainty[7],soft_iron_uncertainty[8],valid_flags); } float soft_iron_uncertainty[9] = {0}; ///< Row-major [dimensionless] @@ -1483,7 +1483,7 @@ struct MagnetometerCovarianceMatrix auto as_tuple() const { - return std::make_tuple(covariance,valid_flags); + return std::make_tuple(covariance[0],covariance[1],covariance[2],covariance[3],covariance[4],covariance[5],covariance[6],covariance[7],covariance[8],valid_flags); } float covariance[9] = {0}; @@ -1510,7 +1510,7 @@ struct MagnetometerResidualVector auto as_tuple() const { - return std::make_tuple(residual,valid_flags); + return std::make_tuple(residual[0],residual[1],residual[2],valid_flags); } float residual[3] = {0}; ///< (x,y,z) [Gauss] @@ -1652,7 +1652,7 @@ struct HeadAidStatus auto as_tuple() const { - return std::make_tuple(time_of_week,type,reserved); + return std::make_tuple(time_of_week,type,reserved[0],reserved[1]); } enum class HeadingAidType : uint8_t @@ -1686,7 +1686,7 @@ struct RelPosNed auto as_tuple() const { - return std::make_tuple(relative_position,valid_flags); + return std::make_tuple(relative_position[0],relative_position[1],relative_position[2],valid_flags); } double relative_position[3] = {0}; ///< [meters, NED] @@ -1713,7 +1713,7 @@ struct EcefPos auto as_tuple() const { - return std::make_tuple(position_ecef,valid_flags); + return std::make_tuple(position_ecef[0],position_ecef[1],position_ecef[2],valid_flags); } double position_ecef[3] = {0}; ///< [meters, ECEF] @@ -1740,7 +1740,7 @@ struct EcefVel auto as_tuple() const { - return std::make_tuple(velocity_ecef,valid_flags); + return std::make_tuple(velocity_ecef[0],velocity_ecef[1],velocity_ecef[2],valid_flags); } float velocity_ecef[3] = {0}; ///< [meters/second, ECEF] @@ -1767,7 +1767,7 @@ struct EcefPosUncertainty auto as_tuple() const { - return std::make_tuple(pos_uncertainty,valid_flags); + return std::make_tuple(pos_uncertainty[0],pos_uncertainty[1],pos_uncertainty[2],valid_flags); } float pos_uncertainty[3] = {0}; ///< [meters] @@ -1794,7 +1794,7 @@ struct EcefVelUncertainty auto as_tuple() const { - return std::make_tuple(vel_uncertainty,valid_flags); + return std::make_tuple(vel_uncertainty[0],vel_uncertainty[1],vel_uncertainty[2],valid_flags); } float vel_uncertainty[3] = {0}; ///< [meters/second] diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index a8a465bd4..8dbff4186 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -250,7 +250,7 @@ struct PosEcef auto as_tuple() const { - return std::make_tuple(x,x_accuracy,valid_flags); + return std::make_tuple(x[0],x[1],x[2],x_accuracy,valid_flags); } struct ValidFlags : Bitfield @@ -305,7 +305,7 @@ struct VelNed auto as_tuple() const { - return std::make_tuple(v,speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); + return std::make_tuple(v[0],v[1],v[2],speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); } struct ValidFlags : Bitfield @@ -376,7 +376,7 @@ struct VelEcef auto as_tuple() const { - return std::make_tuple(v,v_accuracy,valid_flags); + return std::make_tuple(v[0],v[1],v[2],v_accuracy,valid_flags); } struct ValidFlags : Bitfield @@ -1480,7 +1480,7 @@ struct BaseStationInfo auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,ecef_pos,height,station_id,indicators,valid_flags); + return std::make_tuple(time_of_week,week_number,ecef_pos[0],ecef_pos[1],ecef_pos[2],height,station_id,indicators,valid_flags); } struct IndicatorFlags : Bitfield @@ -1595,7 +1595,7 @@ struct RtkCorrectionsStatus auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved,valid_flags); + return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved[0],reserved[1],reserved[2],reserved[3],valid_flags); } struct ValidFlags : Bitfield @@ -2092,7 +2092,7 @@ struct GloEphemeris auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x,v,a,health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x[0],x[1],x[2],v[0],v[1],v[2],a[0],a[1],a[2],health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); } struct ValidFlags : Bitfield @@ -2166,7 +2166,7 @@ struct GpsIonoCorr auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,alpha,beta,valid_flags); + return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],alpha[3],beta[0],beta[1],beta[2],beta[3],valid_flags); } struct ValidFlags : Bitfield @@ -2229,7 +2229,7 @@ struct GalileoIonoCorr auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,alpha,disturbance_flags,valid_flags); + return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],disturbance_flags,valid_flags); } struct ValidFlags : Bitfield diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 0621df768..b5868dc2e 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -85,7 +85,7 @@ struct RawAccel auto as_tuple() const { - return std::make_tuple(raw_accel); + return std::make_tuple(raw_accel[0],raw_accel[1],raw_accel[2]); } float raw_accel[3] = {0}; ///< Native sensor counts @@ -112,7 +112,7 @@ struct RawGyro auto as_tuple() const { - return std::make_tuple(raw_gyro); + return std::make_tuple(raw_gyro[0],raw_gyro[1],raw_gyro[2]); } float raw_gyro[3] = {0}; ///< Native sensor counts @@ -139,7 +139,7 @@ struct RawMag auto as_tuple() const { - return std::make_tuple(raw_mag); + return std::make_tuple(raw_mag[0],raw_mag[1],raw_mag[2]); } float raw_mag[3] = {0}; ///< Native sensor counts @@ -193,7 +193,7 @@ struct ScaledAccel auto as_tuple() const { - return std::make_tuple(scaled_accel); + return std::make_tuple(scaled_accel[0],scaled_accel[1],scaled_accel[2]); } float scaled_accel[3] = {0}; ///< (x, y, z)[g] @@ -220,7 +220,7 @@ struct ScaledGyro auto as_tuple() const { - return std::make_tuple(scaled_gyro); + return std::make_tuple(scaled_gyro[0],scaled_gyro[1],scaled_gyro[2]); } float scaled_gyro[3] = {0}; ///< (x, y, z) [radians/second] @@ -247,7 +247,7 @@ struct ScaledMag auto as_tuple() const { - return std::make_tuple(scaled_mag); + return std::make_tuple(scaled_mag[0],scaled_mag[1],scaled_mag[2]); } float scaled_mag[3] = {0}; ///< (x, y, z) [Gauss] @@ -300,7 +300,7 @@ struct DeltaTheta auto as_tuple() const { - return std::make_tuple(delta_theta); + return std::make_tuple(delta_theta[0],delta_theta[1],delta_theta[2]); } float delta_theta[3] = {0}; ///< (x, y, z) [radians] @@ -327,7 +327,7 @@ struct DeltaVelocity auto as_tuple() const { - return std::make_tuple(delta_velocity); + return std::make_tuple(delta_velocity[0],delta_velocity[1],delta_velocity[2]); } float delta_velocity[3] = {0}; ///< (x, y, z) [g*sec] @@ -363,7 +363,7 @@ struct CompOrientationMatrix auto as_tuple() const { - return std::make_tuple(m); + return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } float m[9] = {0}; ///< Matrix elements in row-major order. @@ -397,7 +397,7 @@ struct CompQuaternion auto as_tuple() const { - return std::make_tuple(q); + return std::make_tuple(q[0],q[1],q[2],q[3]); } float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND @@ -452,7 +452,7 @@ struct CompOrientationUpdateMatrix auto as_tuple() const { - return std::make_tuple(m); + return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } float m[9] = {0}; @@ -478,7 +478,7 @@ struct OrientationRawTemp auto as_tuple() const { - return std::make_tuple(raw_temp); + return std::make_tuple(raw_temp[0],raw_temp[1],raw_temp[2],raw_temp[3]); } uint16_t raw_temp[4] = {0}; @@ -662,7 +662,7 @@ struct UpVector auto as_tuple() const { - return std::make_tuple(up); + return std::make_tuple(up[0],up[1],up[2]); } float up[3] = {0}; ///< [Gs] @@ -691,7 +691,7 @@ struct NorthVector auto as_tuple() const { - return std::make_tuple(north); + return std::make_tuple(north[0],north[1],north[2]); } float north[3] = {0}; ///< [Gauss] From 3468b79c8d637977f29733bb7707f383da95cdb4 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 9 Nov 2022 11:19:36 -0500 Subject: [PATCH 022/252] Generate MIP definitions from branches/dev@53881. --- src/mip/definitions/commands_3dm.h | 18 +-- src/mip/definitions/commands_3dm.hpp | 18 +-- src/mip/definitions/commands_gnss.c | 213 ++++++++++++++++++++++++++ src/mip/definitions/commands_gnss.cpp | 202 ++++++++++++++++++++++++ src/mip/definitions/commands_gnss.h | 51 ++++++ src/mip/definitions/commands_gnss.hpp | 61 ++++++++ src/mip/definitions/commands_rtk.h | 2 +- src/mip/definitions/commands_rtk.hpp | 6 +- 8 files changed, 549 insertions(+), 22 deletions(-) diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 3d96ab5ed..5ff9a401e 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -147,27 +147,27 @@ enum typedef uint8_t mip_nmea_message_message_id; static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GGA = 1; ///< GPS System Fix Data. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GLL = 2; ///< Geographic Position Lat/Lon. Source can be the Filter or GNSS1/2 datasets. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GSV = 3; ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID is ignored (talker depends on the satellite). +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GSV = 3; ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID must be set to IGNORED. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_RMC = 4; ///< Recommended Minimum Specific GNSS Data. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_VTG = 5; ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_HDT = 6; ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_ZDA = 7; ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKA = 129; ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID is ignored. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKR = 130; ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID is ignored. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PKRA = 129; ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PKRR = 130; ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. typedef uint8_t mip_nmea_message_talker_id; -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_RESERVED = 0; ///< -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GNSS = 1; ///< NMEA message will be produced with talker id "GN" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GPS = 2; ///< NMEA message will be produced with talker id "GP" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GALILEO = 3; ///< NMEA message will be produced with talker id "GA" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GLONASS = 4; ///< NMEA message will be produced with talker id "GL" +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_IGNORED = 0; ///< Talker ID cannot be changed. +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GNSS = 1; ///< NMEA message will be produced with talker id "GN". +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GPS = 2; ///< NMEA message will be produced with talker id "GP". +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GALILEO = 3; ///< NMEA message will be produced with talker id "GA". +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GLONASS = 4; ///< NMEA message will be produced with talker id "GL". struct mip_nmea_message { mip_nmea_message_message_id message_id; ///< NMEA sentence type. mip_nmea_message_talker_id talker_id; ///< NMEA talker ID. Ignored for proprietary sentences. uint8_t source_desc_set; ///< Data descriptor set where the data will be sourced. Available options depend on the sentence. - uint16_t decimation; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. + uint16_t decimation; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. Must be 0 when polling. }; typedef struct mip_nmea_message mip_nmea_message; diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 989d08120..523318ed7 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -149,28 +149,28 @@ struct NmeaMessage { GGA = 1, ///< GPS System Fix Data. Source can be the Filter or GNSS1/2 datasets. GLL = 2, ///< Geographic Position Lat/Lon. Source can be the Filter or GNSS1/2 datasets. - GSV = 3, ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID is ignored (talker depends on the satellite). + GSV = 3, ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID must be set to IGNORED. RMC = 4, ///< Recommended Minimum Specific GNSS Data. Source can be the Filter or GNSS1/2 datasets. VTG = 5, ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. HDT = 6, ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. ZDA = 7, ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. - PRKA = 129, ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID is ignored. - PRKR = 130, ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID is ignored. + PKRA = 129, ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. + PKRR = 130, ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. }; enum class TalkerID : uint8_t { - RESERVED = 0, ///< - GNSS = 1, ///< NMEA message will be produced with talker id "GN" - GPS = 2, ///< NMEA message will be produced with talker id "GP" - GALILEO = 3, ///< NMEA message will be produced with talker id "GA" - GLONASS = 4, ///< NMEA message will be produced with talker id "GL" + IGNORED = 0, ///< Talker ID cannot be changed. + GNSS = 1, ///< NMEA message will be produced with talker id "GN". + GPS = 2, ///< NMEA message will be produced with talker id "GP". + GALILEO = 3, ///< NMEA message will be produced with talker id "GA". + GLONASS = 4, ///< NMEA message will be produced with talker id "GL". }; MessageID message_id = static_cast(0); ///< NMEA sentence type. TalkerID talker_id = static_cast(0); ///< NMEA talker ID. Ignored for proprietary sentences. uint8_t source_desc_set = 0; ///< Data descriptor set where the data will be sourced. Available options depend on the sentence. - uint16_t decimation = 0; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. + uint16_t decimation = 0; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. Must be 0 when polling. }; void insert(Serializer& serializer, const NmeaMessage& self); diff --git a/src/mip/definitions/commands_gnss.c b/src/mip/definitions/commands_gnss.c index cb41c03a3..a0e0672a6 100644 --- a/src/mip/definitions/commands_gnss.c +++ b/src/mip/definitions/commands_gnss.c @@ -239,6 +239,219 @@ mip_cmd_result mip_gnss_default_signal_configuration(struct mip_interface* devic return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_gnss_mip_gnss_spartn_configuration_command(mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->enable); + + insert_u8(serializer, self->type); + + insert_u32(serializer, self->current_key_tow); + + insert_u16(serializer, self->current_key_week); + + for(unsigned int i=0; i < 32; i++) + insert_u8(serializer, self->current_key[i]); + + insert_u32(serializer, self->next_key_tow); + + insert_u16(serializer, self->next_key_week); + + for(unsigned int i=0; i < 32; i++) + insert_u8(serializer, self->next_key[i]); + + } +} +void extract_mip_gnss_mip_gnss_spartn_configuration_command(mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->enable); + + extract_u8(serializer, &self->type); + + extract_u32(serializer, &self->current_key_tow); + + extract_u16(serializer, &self->current_key_week); + + for(unsigned int i=0; i < 32; i++) + extract_u8(serializer, &self->current_key[i]); + + extract_u32(serializer, &self->next_key_tow); + + extract_u16(serializer, &self->next_key_week); + + for(unsigned int i=0; i < 32; i++) + extract_u8(serializer, &self->next_key[i]); + + } +} + +void insert_mip_gnss_mip_gnss_spartn_configuration_response(mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_response* self) +{ + insert_u8(serializer, self->enable); + + insert_u8(serializer, self->type); + + insert_u32(serializer, self->current_key_tow); + + insert_u16(serializer, self->current_key_week); + + for(unsigned int i=0; i < 32; i++) + insert_u8(serializer, self->current_key[i]); + + insert_u32(serializer, self->next_key_tow); + + insert_u16(serializer, self->next_key_week); + + for(unsigned int i=0; i < 32; i++) + insert_u8(serializer, self->next_key[i]); + +} +void extract_mip_gnss_mip_gnss_spartn_configuration_response(mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_response* self) +{ + extract_u8(serializer, &self->enable); + + extract_u8(serializer, &self->type); + + extract_u32(serializer, &self->current_key_tow); + + extract_u16(serializer, &self->current_key_week); + + for(unsigned int i=0; i < 32; i++) + extract_u8(serializer, &self->current_key[i]); + + extract_u32(serializer, &self->next_key_tow); + + extract_u16(serializer, &self->next_key_week); + + for(unsigned int i=0; i < 32; i++) + extract_u8(serializer, &self->next_key[i]); + +} + +mip_cmd_result mip_gnss_write_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t enable, uint8_t type, uint32_t current_key_tow, uint16_t current_key_week, const uint8_t* current_key, uint32_t next_key_tow, uint16_t next_key_week, const uint8_t* next_key) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, enable); + + insert_u8(&serializer, type); + + insert_u32(&serializer, current_key_tow); + + insert_u16(&serializer, current_key_week); + + assert(current_key || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + insert_u8(&serializer, current_key[i]); + + insert_u32(&serializer, next_key_tow); + + insert_u16(&serializer, next_key_week); + + assert(next_key || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + insert_u8(&serializer, next_key[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_gnss_read_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t* enable_out, uint8_t* type_out, uint32_t* current_key_tow_out, uint16_t* current_key_week_out, uint8_t* current_key_out, uint32_t* next_key_tow_out, uint16_t* next_key_week_out, uint8_t* next_key_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_GNSS_SPARTN_CONFIGURATION, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_u8(&deserializer, enable_out); + + assert(type_out); + extract_u8(&deserializer, type_out); + + assert(current_key_tow_out); + extract_u32(&deserializer, current_key_tow_out); + + assert(current_key_week_out); + extract_u16(&deserializer, current_key_week_out); + + assert(current_key_out || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + extract_u8(&deserializer, ¤t_key_out[i]); + + assert(next_key_tow_out); + extract_u32(&deserializer, next_key_tow_out); + + assert(next_key_week_out); + extract_u16(&deserializer, next_key_week_out); + + assert(next_key_out || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + extract_u8(&deserializer, &next_key_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_gnss_save_mip_gnss_spartn_configuration(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_gnss_load_mip_gnss_spartn_configuration(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_gnss_default_mip_gnss_spartn_configuration(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_gnss_rtk_dongle_configuration_command(mip_serializer* serializer, const mip_gnss_rtk_dongle_configuration_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_gnss.cpp b/src/mip/definitions/commands_gnss.cpp index 00bb9e59c..0cb2da4df 100644 --- a/src/mip/definitions/commands_gnss.cpp +++ b/src/mip/definitions/commands_gnss.cpp @@ -254,6 +254,208 @@ CmdResult defaultSignalConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const MipGnssSpartnConfiguration& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.enable); + + insert(serializer, self.type); + + insert(serializer, self.current_key_tow); + + insert(serializer, self.current_key_week); + + for(unsigned int i=0; i < 32; i++) + insert(serializer, self.current_key[i]); + + insert(serializer, self.next_key_tow); + + insert(serializer, self.next_key_week); + + for(unsigned int i=0; i < 32; i++) + insert(serializer, self.next_key[i]); + + } +} +void extract(Serializer& serializer, MipGnssSpartnConfiguration& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.enable); + + extract(serializer, self.type); + + extract(serializer, self.current_key_tow); + + extract(serializer, self.current_key_week); + + for(unsigned int i=0; i < 32; i++) + extract(serializer, self.current_key[i]); + + extract(serializer, self.next_key_tow); + + extract(serializer, self.next_key_week); + + for(unsigned int i=0; i < 32; i++) + extract(serializer, self.next_key[i]); + + } +} + +void insert(Serializer& serializer, const MipGnssSpartnConfiguration::Response& self) +{ + insert(serializer, self.enable); + + insert(serializer, self.type); + + insert(serializer, self.current_key_tow); + + insert(serializer, self.current_key_week); + + for(unsigned int i=0; i < 32; i++) + insert(serializer, self.current_key[i]); + + insert(serializer, self.next_key_tow); + + insert(serializer, self.next_key_week); + + for(unsigned int i=0; i < 32; i++) + insert(serializer, self.next_key[i]); + +} +void extract(Serializer& serializer, MipGnssSpartnConfiguration::Response& self) +{ + extract(serializer, self.enable); + + extract(serializer, self.type); + + extract(serializer, self.current_key_tow); + + extract(serializer, self.current_key_week); + + for(unsigned int i=0; i < 32; i++) + extract(serializer, self.current_key[i]); + + extract(serializer, self.next_key_tow); + + extract(serializer, self.next_key_week); + + for(unsigned int i=0; i < 32; i++) + extract(serializer, self.next_key[i]); + +} + +CmdResult writeMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t enable, uint8_t type, uint32_t currentKeyTow, uint16_t currentKeyWeek, const uint8_t* currentKey, uint32_t nextKeyTow, uint16_t nextKeyWeek, const uint8_t* nextKey) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, enable); + + insert(serializer, type); + + insert(serializer, currentKeyTow); + + insert(serializer, currentKeyWeek); + + assert(currentKey || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + insert(serializer, currentKey[i]); + + insert(serializer, nextKeyTow); + + insert(serializer, nextKeyWeek); + + assert(nextKey || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + insert(serializer, nextKey[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* typeOut, uint32_t* currentKeyTowOut, uint16_t* currentKeyWeekOut, uint8_t* currentKeyOut, uint32_t* nextKeyTowOut, uint16_t* nextKeyWeekOut, uint8_t* nextKeyOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPARTN_CONFIGURATION, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(typeOut); + extract(deserializer, *typeOut); + + assert(currentKeyTowOut); + extract(deserializer, *currentKeyTowOut); + + assert(currentKeyWeekOut); + extract(deserializer, *currentKeyWeekOut); + + assert(currentKeyOut || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + extract(deserializer, currentKeyOut[i]); + + assert(nextKeyTowOut); + extract(deserializer, *nextKeyTowOut); + + assert(nextKeyWeekOut); + extract(deserializer, *nextKeyWeekOut); + + assert(nextKeyOut || (32 == 0)); + for(unsigned int i=0; i < 32; i++) + extract(deserializer, nextKeyOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMipGnssSpartnConfiguration(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMipGnssSpartnConfiguration(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMipGnssSpartnConfiguration(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const RtkDongleConfiguration& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_gnss.h b/src/mip/definitions/commands_gnss.h index 28959ca40..6501a0ccb 100644 --- a/src/mip/definitions/commands_gnss.h +++ b/src/mip/definitions/commands_gnss.h @@ -35,10 +35,12 @@ enum MIP_CMD_DESC_GNSS_LIST_RECEIVERS = 0x01, MIP_CMD_DESC_GNSS_SIGNAL_CONFIGURATION = 0x02, MIP_CMD_DESC_GNSS_RTK_DONGLE_CONFIGURATION = 0x10, + MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION = 0x20, MIP_REPLY_DESC_GNSS_LIST_RECEIVERS = 0x81, MIP_REPLY_DESC_GNSS_SIGNAL_CONFIGURATION = 0x82, MIP_REPLY_DESC_GNSS_RTK_DONGLE_CONFIGURATION = 0x90, + MIP_REPLY_DESC_GNSS_SPARTN_CONFIGURATION = 0xA0, }; //////////////////////////////////////////////////////////////////////////////// @@ -131,6 +133,55 @@ mip_cmd_result mip_gnss_default_signal_configuration(struct mip_interface* devic ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_gnss_mip_gnss_spartn_configuration (0x0E,0x20) Mip Gnss Spartn Configuration [C] +/// Configure the SPARTN corrections service parameters. +/// Notes:
+/// - Enable and type settings will only update after a power cycle
+/// - Key information can be updated while running +/// +///@{ + +struct mip_gnss_mip_gnss_spartn_configuration_command +{ + mip_function_selector function; + uint8_t enable; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) + uint8_t type; ///< Connection type (1 = Network, 2 = L-Band) + uint32_t current_key_tow; ///< The GPS time of week the current key is valid until + uint16_t current_key_week; ///< The GPS week number the current key is valid until + uint8_t current_key[32]; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) + uint32_t next_key_tow; ///< The GPS time of week the next key is valid until + uint16_t next_key_week; ///< The GPS week number the next key is valid until + uint8_t next_key[32]; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) + +}; +typedef struct mip_gnss_mip_gnss_spartn_configuration_command mip_gnss_mip_gnss_spartn_configuration_command; +void insert_mip_gnss_mip_gnss_spartn_configuration_command(struct mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_command* self); +void extract_mip_gnss_mip_gnss_spartn_configuration_command(struct mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_command* self); + +struct mip_gnss_mip_gnss_spartn_configuration_response +{ + uint8_t enable; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) + uint8_t type; ///< Connection type (1 = Network, 2 = L-Band) + uint32_t current_key_tow; ///< The GPS time of week the current key is valid until + uint16_t current_key_week; ///< The GPS week number the current key is valid until + uint8_t current_key[32]; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) + uint32_t next_key_tow; ///< The GPS time of week the next key is valid until + uint16_t next_key_week; ///< The GPS week number the next key is valid until + uint8_t next_key[32]; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) + +}; +typedef struct mip_gnss_mip_gnss_spartn_configuration_response mip_gnss_mip_gnss_spartn_configuration_response; +void insert_mip_gnss_mip_gnss_spartn_configuration_response(struct mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_response* self); +void extract_mip_gnss_mip_gnss_spartn_configuration_response(struct mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_response* self); + +mip_cmd_result mip_gnss_write_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t enable, uint8_t type, uint32_t current_key_tow, uint16_t current_key_week, const uint8_t* current_key, uint32_t next_key_tow, uint16_t next_key_week, const uint8_t* next_key); +mip_cmd_result mip_gnss_read_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t* enable_out, uint8_t* type_out, uint32_t* current_key_tow_out, uint16_t* current_key_week_out, uint8_t* current_key_out, uint32_t* next_key_tow_out, uint16_t* next_key_week_out, uint8_t* next_key_out); +mip_cmd_result mip_gnss_save_mip_gnss_spartn_configuration(struct mip_interface* device); +mip_cmd_result mip_gnss_load_mip_gnss_spartn_configuration(struct mip_interface* device); +mip_cmd_result mip_gnss_default_mip_gnss_spartn_configuration(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_gnss_rtk_dongle_configuration (0x0E,0x10) Rtk Dongle Configuration [C] /// Configure the communications with the RTK Dongle connected to the device. /// diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 1ecefbd15..3d8d783bf 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -34,10 +34,12 @@ enum CMD_LIST_RECEIVERS = 0x01, CMD_SIGNAL_CONFIGURATION = 0x02, CMD_RTK_DONGLE_CONFIGURATION = 0x10, + CMD_SPARTN_CONFIGURATION = 0x20, REPLY_LIST_RECEIVERS = 0x81, REPLY_SIGNAL_CONFIGURATION = 0x82, REPLY_RTK_DONGLE_CONFIGURATION = 0x90, + REPLY_SPARTN_CONFIGURATION = 0xA0, }; //////////////////////////////////////////////////////////////////////////////// @@ -153,6 +155,65 @@ CmdResult defaultSignalConfiguration(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_gnss_mip_gnss_spartn_configuration (0x0E,0x20) Mip Gnss Spartn Configuration [CPP] +/// Configure the SPARTN corrections service parameters. +/// Notes:
+/// - Enable and type settings will only update after a power cycle
+/// - Key information can be updated while running +/// +///@{ + +struct MipGnssSpartnConfiguration +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SPARTN_CONFIGURATION; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) + uint8_t type = 0; ///< Connection type (1 = Network, 2 = L-Band) + uint32_t current_key_tow = 0; ///< The GPS time of week the current key is valid until + uint16_t current_key_week = 0; ///< The GPS week number the current key is valid until + uint8_t current_key[32] = {0}; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) + uint32_t next_key_tow = 0; ///< The GPS time of week the next key is valid until + uint16_t next_key_week = 0; ///< The GPS week number the next key is valid until + uint8_t next_key[32] = {0}; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SPARTN_CONFIGURATION; + + uint8_t enable = 0; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) + uint8_t type = 0; ///< Connection type (1 = Network, 2 = L-Band) + uint32_t current_key_tow = 0; ///< The GPS time of week the current key is valid until + uint16_t current_key_week = 0; ///< The GPS week number the current key is valid until + uint8_t current_key[32] = {0}; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) + uint32_t next_key_tow = 0; ///< The GPS time of week the next key is valid until + uint16_t next_key_week = 0; ///< The GPS week number the next key is valid until + uint8_t next_key[32] = {0}; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) + + }; +}; +void insert(Serializer& serializer, const MipGnssSpartnConfiguration& self); +void extract(Serializer& serializer, MipGnssSpartnConfiguration& self); + +void insert(Serializer& serializer, const MipGnssSpartnConfiguration::Response& self); +void extract(Serializer& serializer, MipGnssSpartnConfiguration::Response& self); + +CmdResult writeMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t enable, uint8_t type, uint32_t currentKeyTow, uint16_t currentKeyWeek, const uint8_t* currentKey, uint32_t nextKeyTow, uint16_t nextKeyWeek, const uint8_t* nextKey); +CmdResult readMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* typeOut, uint32_t* currentKeyTowOut, uint16_t* currentKeyWeekOut, uint8_t* currentKeyOut, uint32_t* nextKeyTowOut, uint16_t* nextKeyWeekOut, uint8_t* nextKeyOut); +CmdResult saveMipGnssSpartnConfiguration(C::mip_interface& device); +CmdResult loadMipGnssSpartnConfiguration(C::mip_interface& device); +CmdResult defaultMipGnssSpartnConfiguration(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_gnss_rtk_dongle_configuration (0x0E,0x10) Rtk Dongle Configuration [CPP] /// Configure the communications with the RTK Dongle connected to the device. /// diff --git a/src/mip/definitions/commands_rtk.h b/src/mip/definitions/commands_rtk.h index 028ca7c0f..f68eebc06 100644 --- a/src/mip/definitions/commands_rtk.h +++ b/src/mip/definitions/commands_rtk.h @@ -108,7 +108,7 @@ static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FL static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_TOWER_CHANGE_INDICATOR = 0x00F00000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_NMEA_TIMEOUT = 0x01000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_SERVER_TIMEOUT = 0x02000000; ///< -static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_RTCM_TIMEOUT = 0x04000000; ///< +static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_CORRECTIONS_TIMEOUT = 0x04000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_DEVICE_OUT_OF_RANGE = 0x08000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_CORRECTIONS_UNAVAILABLE = 0x10000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_RESERVED = 0x20000000; ///< diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 8cdf9b891..6992b1051 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -152,7 +152,7 @@ struct GetStatusFlags TOWER_CHANGE_INDICATOR = 0x00F00000, ///< NMEA_TIMEOUT = 0x01000000, ///< SERVER_TIMEOUT = 0x02000000, ///< - RTCM_TIMEOUT = 0x04000000, ///< + CORRECTIONS_TIMEOUT = 0x04000000, ///< DEVICE_OUT_OF_RANGE = 0x08000000, ///< CORRECTIONS_UNAVAILABLE = 0x10000000, ///< RESERVED = 0x20000000, ///< @@ -182,8 +182,8 @@ struct GetStatusFlags void nmeaTimeout(bool val) { if(val) value |= NMEA_TIMEOUT; else value &= ~NMEA_TIMEOUT; } bool serverTimeout() const { return (value & SERVER_TIMEOUT) > 0; } void serverTimeout(bool val) { if(val) value |= SERVER_TIMEOUT; else value &= ~SERVER_TIMEOUT; } - bool rtcmTimeout() const { return (value & RTCM_TIMEOUT) > 0; } - void rtcmTimeout(bool val) { if(val) value |= RTCM_TIMEOUT; else value &= ~RTCM_TIMEOUT; } + bool correctionsTimeout() const { return (value & CORRECTIONS_TIMEOUT) > 0; } + void correctionsTimeout(bool val) { if(val) value |= CORRECTIONS_TIMEOUT; else value &= ~CORRECTIONS_TIMEOUT; } bool deviceOutOfRange() const { return (value & DEVICE_OUT_OF_RANGE) > 0; } void deviceOutOfRange(bool val) { if(val) value |= DEVICE_OUT_OF_RANGE; else value &= ~DEVICE_OUT_OF_RANGE; } bool correctionsUnavailable() const { return (value & CORRECTIONS_UNAVAILABLE) > 0; } From d6dcf5f10b84ff13bbe146ee00db14f68abb8c53 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 9 Nov 2022 11:34:21 -0500 Subject: [PATCH 023/252] Generate MIP definitions from branches/dev@53882. --- src/mip/definitions/commands_3dm.c | 18 +++++++++--------- src/mip/definitions/commands_3dm.cpp | 18 +++++++++--------- src/mip/definitions/commands_3dm.h | 28 ++++++++++++++-------------- src/mip/definitions/commands_3dm.hpp | 22 +++++++++++----------- 4 files changed, 43 insertions(+), 43 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index caef68235..2e39a0fd1 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -4761,7 +4761,7 @@ mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mi } return result; } -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_command* self) +void insert_mip_3dm_lowpass_filter_command(mip_serializer* serializer, const mip_3dm_lowpass_filter_command* self) { insert_mip_function_selector(serializer, self->function); @@ -4779,7 +4779,7 @@ void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serialize } } -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_command* self) +void extract_mip_3dm_lowpass_filter_command(mip_serializer* serializer, mip_3dm_lowpass_filter_command* self) { extract_mip_function_selector(serializer, &self->function); @@ -4798,7 +4798,7 @@ void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializ } } -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_response* self) +void insert_mip_3dm_lowpass_filter_response(mip_serializer* serializer, const mip_3dm_lowpass_filter_response* self) { insert_u8(serializer, self->desc_set); @@ -4811,7 +4811,7 @@ void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializ insert_float(serializer, self->frequency); } -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_response* self) +void extract_mip_3dm_lowpass_filter_response(mip_serializer* serializer, mip_3dm_lowpass_filter_response* self) { extract_u8(serializer, &self->desc_set); @@ -4825,7 +4825,7 @@ void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* seriali } -mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency) +mip_cmd_result mip_3dm_write_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4847,7 +4847,7 @@ mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* de return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out) +mip_cmd_result mip_3dm_read_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4887,7 +4887,7 @@ mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* dev } return result; } -mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +mip_cmd_result mip_3dm_save_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4903,7 +4903,7 @@ mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* dev return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +mip_cmd_result mip_3dm_load_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4919,7 +4919,7 @@ mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* dev return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_default_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +mip_cmd_result mip_3dm_default_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index fd5471d53..c727face8 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -4227,7 +4227,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso } return result; } -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self) +void insert(Serializer& serializer, const LowpassFilter& self) { insert(serializer, self.function); @@ -4245,7 +4245,7 @@ void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self) } } -void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self) +void extract(Serializer& serializer, LowpassFilter& self) { extract(serializer, self.function); @@ -4264,7 +4264,7 @@ void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self) } } -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self) +void insert(Serializer& serializer, const LowpassFilter::Response& self) { insert(serializer, self.desc_set); @@ -4277,7 +4277,7 @@ void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self insert(serializer, self.frequency); } -void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self) +void extract(Serializer& serializer, LowpassFilter::Response& self) { extract(serializer, self.desc_set); @@ -4291,7 +4291,7 @@ void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self) } -CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) +CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4311,7 +4311,7 @@ CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) +CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4348,7 +4348,7 @@ CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, } return result; } -CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4362,7 +4362,7 @@ CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4376,7 +4376,7 @@ CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 5ff9a401e..649f4a397 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -2042,7 +2042,7 @@ mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mi ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_3dm_mip_cmd_3dm_lowpass_filter (0x0C,0x54) Mip Cmd 3Dm Lowpass Filter [C] +///@defgroup c_3dm_lowpass_filter (0x0C,0x54) Lowpass Filter [C] /// This command controls the low-pass anti-aliasing filter supported data quantities. /// /// See the device user manual for data quantities which support the anti-aliasing filter. @@ -2060,7 +2060,7 @@ mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mi /// ///@{ -struct mip_3dm_mip_cmd_3dm_lowpass_filter_command +struct mip_3dm_lowpass_filter_command { mip_function_selector function; uint8_t desc_set; ///< Descriptor set of the quantity to be filtered. @@ -2070,11 +2070,11 @@ struct mip_3dm_mip_cmd_3dm_lowpass_filter_command float frequency; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. }; -typedef struct mip_3dm_mip_cmd_3dm_lowpass_filter_command mip_3dm_mip_cmd_3dm_lowpass_filter_command; -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(struct mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_command* self); -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(struct mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_command* self); +typedef struct mip_3dm_lowpass_filter_command mip_3dm_lowpass_filter_command; +void insert_mip_3dm_lowpass_filter_command(struct mip_serializer* serializer, const mip_3dm_lowpass_filter_command* self); +void extract_mip_3dm_lowpass_filter_command(struct mip_serializer* serializer, mip_3dm_lowpass_filter_command* self); -struct mip_3dm_mip_cmd_3dm_lowpass_filter_response +struct mip_3dm_lowpass_filter_response { uint8_t desc_set; ///< Descriptor set of the quantity to be filtered. uint8_t field_desc; ///< Field descriptor of the quantity to be filtered. @@ -2083,15 +2083,15 @@ struct mip_3dm_mip_cmd_3dm_lowpass_filter_response float frequency; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. }; -typedef struct mip_3dm_mip_cmd_3dm_lowpass_filter_response mip_3dm_mip_cmd_3dm_lowpass_filter_response; -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(struct mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_response* self); -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(struct mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_response* self); +typedef struct mip_3dm_lowpass_filter_response mip_3dm_lowpass_filter_response; +void insert_mip_3dm_lowpass_filter_response(struct mip_serializer* serializer, const mip_3dm_lowpass_filter_response* self); +void extract_mip_3dm_lowpass_filter_response(struct mip_serializer* serializer, mip_3dm_lowpass_filter_response* self); -mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency); -mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out); -mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); -mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); -mip_cmd_result mip_3dm_default_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_write_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency); +mip_cmd_result mip_3dm_read_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out); +mip_cmd_result mip_3dm_save_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_load_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_default_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); ///@} /// diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 523318ed7..1cccd0c26 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -2450,7 +2450,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_3dm_mip_cmd_3dm_lowpass_filter (0x0C,0x54) Mip Cmd 3Dm Lowpass Filter [CPP] +///@defgroup cpp_3dm_lowpass_filter (0x0C,0x54) Lowpass Filter [CPP] /// This command controls the low-pass anti-aliasing filter supported data quantities. /// /// See the device user manual for data quantities which support the anti-aliasing filter. @@ -2468,7 +2468,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso /// ///@{ -struct MipCmd3dmLowpassFilter +struct LowpassFilter { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; @@ -2499,17 +2499,17 @@ struct MipCmd3dmLowpassFilter }; }; -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self); -void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self); +void insert(Serializer& serializer, const LowpassFilter& self); +void extract(Serializer& serializer, LowpassFilter& self); -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self); -void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self); +void insert(Serializer& serializer, const LowpassFilter::Response& self); +void extract(Serializer& serializer, LowpassFilter::Response& self); -CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); -CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); -CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult defaultMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); +CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); +CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); ///@} /// From 55ee1084969447c23bc36421212a3d2dd4e9d978 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Mon, 14 Nov 2022 13:30:47 -0500 Subject: [PATCH 024/252] Does not cause a build error if the version returned from git is not in the semantic version format (#43) * Does not cause a build error if the version returned from git is not in the semantic version format * Removes debug print --- CMakeLists.txt | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 79cefcd3e..29a18a661 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,9 +57,18 @@ endif() # Massage the version number a little so we can use it in a couple places string(REGEX REPLACE "^v?([0-9]+)\\.([0-9]+)\\.([0-9]+).*" "\\1.\\2.\\3" MIP_GIT_VERSION_CLEAN ${MIP_GIT_VERSION}) string(REPLACE "." ";" MIP_GIT_VERSION_LIST ${MIP_GIT_VERSION_CLEAN}) -list(GET MIP_GIT_VERSION_LIST 0 MIP_GIT_VERSION_MAJOR) -list(GET MIP_GIT_VERSION_LIST 1 MIP_GIT_VERSION_MINOR) -list(GET MIP_GIT_VERSION_LIST 2 MIP_GIT_VERSION_PATCH) +list(LENGTH MIP_GIT_VERSION_LIST MIP_GIT_VERSION_LIST_LENGTH) +if (MIP_GIT_VERSION_LIST_LENGTH GREATER_EQUAL 3) + list(GET MIP_GIT_VERSION_LIST 0 MIP_GIT_VERSION_MAJOR) + list(GET MIP_GIT_VERSION_LIST 1 MIP_GIT_VERSION_MINOR) + list(GET MIP_GIT_VERSION_LIST 2 MIP_GIT_VERSION_PATCH) +else() + message(WARNING "MIP version cannot be parsed into a semantic version string.\nPlease run 'git fetch --tags' to get a properly tagged build") + set(MIP_GIT_VERSION_CLEAN "0.0.0") + set(MIP_GIT_VERSION_MAJOR 0) + set(MIP_GIT_VERSION_MINOR 0) + set(MIP_GIT_VERSION_PATCH 0) +endif() # Generate the version header file set(VERSION_IN_FILE "${MIP_CMAKE_DIR}/mip_version.h.in") From 0fc55860a8db3530b5193865194380ad237052fc Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Wed, 16 Nov 2022 16:49:06 -0500 Subject: [PATCH 025/252] Replaces only the file extension in .cpp and .hpp files (#44) --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 29a18a661..a76ff7b13 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -163,8 +163,8 @@ set(MIPDEF_SOURCES ${INTDEF_SOURCES} ) -string(REPLACE ".h" ".hpp" MIPDEF_HPP_SOURCES "${MIPDEF_SOURCES}") -string(REPLACE ".c" ".cpp" MIPDEF_CPP_SOURCES "${MIPDEF_HPP_SOURCES}") +string(REGEX REPLACE "\.h(;|$)" ".hpp\\1" MIPDEF_HPP_SOURCES "${MIPDEF_SOURCES}") +string(REGEX REPLACE "\.c(;|$)" ".cpp\\1" MIPDEF_CPP_SOURCES "${MIPDEF_HPP_SOURCES}") if(MIP_USE_SERIAL) list(APPEND UTILS_SOURCES From 32c5f169a3abd4b0da00ca08296ccdf7a94e3f82 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 13 Dec 2022 16:47:31 -0500 Subject: [PATCH 026/252] Generate MIP definitions from branches/dev@54061. --- src/mip/definitions/commands_base.h | 1 + src/mip/definitions/commands_base.hpp | 1 + src/mip/definitions/commands_gnss.h | 5 +++-- src/mip/definitions/commands_gnss.hpp | 5 +++-- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/mip/definitions/commands_base.h b/src/mip/definitions/commands_base.h index f94559e66..a63e33403 100644 --- a/src/mip/definitions/commands_base.h +++ b/src/mip/definitions/commands_base.h @@ -310,6 +310,7 @@ mip_cmd_result mip_base_default_comm_speed(struct mip_interface* device, uint8_t /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_base_gps_time_update (0x01,0x72) Gps Time Update [C] +/// Set device internal GPS time /// When combined with a PPS input signal applied to the I/O connector, this command enables complete synchronization of data outputs /// with an external time base, such as GPS system time. Since the hardware PPS synchronization can only detect the fractional number of seconds when pulses arrive, /// complete synchronization requires that the user provide the whole number of seconds via this command. After achieving PPS synchronization, this command should be sent twice: once to set the time-of-week and once to set the week number. PPS synchronization can be verified by monitoring the time sync status message (0xA0, 0x02) or the valid flags of any shared external timestamp (0x--, D7) data field. diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 9bb661ce2..ae2c775f3 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -489,6 +489,7 @@ CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_base_gps_time_update (0x01,0x72) Gps Time Update [CPP] +/// Set device internal GPS time /// When combined with a PPS input signal applied to the I/O connector, this command enables complete synchronization of data outputs /// with an external time base, such as GPS system time. Since the hardware PPS synchronization can only detect the fractional number of seconds when pulses arrive, /// complete synchronization requires that the user provide the whole number of seconds via this command. After achieving PPS synchronization, this command should be sent twice: once to set the time-of-week and once to set the week number. PPS synchronization can be verified by monitoring the time sync status message (0xA0, 0x02) or the valid flags of any shared external timestamp (0x--, D7) data field. diff --git a/src/mip/definitions/commands_gnss.h b/src/mip/definitions/commands_gnss.h index 6501a0ccb..0de8cfd6a 100644 --- a/src/mip/definitions/commands_gnss.h +++ b/src/mip/definitions/commands_gnss.h @@ -137,6 +137,7 @@ mip_cmd_result mip_gnss_default_signal_configuration(struct mip_interface* devic /// Configure the SPARTN corrections service parameters. /// Notes:
/// - Enable and type settings will only update after a power cycle
+/// - Type settings will only take effect after a power cycle
/// - Key information can be updated while running /// ///@{ @@ -145,7 +146,7 @@ struct mip_gnss_mip_gnss_spartn_configuration_command { mip_function_selector function; uint8_t enable; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type; ///< Connection type (1 = Network, 2 = L-Band) + uint8_t type; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) uint32_t current_key_tow; ///< The GPS time of week the current key is valid until uint16_t current_key_week; ///< The GPS week number the current key is valid until uint8_t current_key[32]; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) @@ -161,7 +162,7 @@ void extract_mip_gnss_mip_gnss_spartn_configuration_command(struct mip_serialize struct mip_gnss_mip_gnss_spartn_configuration_response { uint8_t enable; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type; ///< Connection type (1 = Network, 2 = L-Band) + uint8_t type; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) uint32_t current_key_tow; ///< The GPS time of week the current key is valid until uint16_t current_key_week; ///< The GPS week number the current key is valid until uint8_t current_key[32]; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 3d8d783bf..63c518c37 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -159,6 +159,7 @@ CmdResult defaultSignalConfiguration(C::mip_interface& device); /// Configure the SPARTN corrections service parameters. /// Notes:
/// - Enable and type settings will only update after a power cycle
+/// - Type settings will only take effect after a power cycle
/// - Key information can be updated while running /// ///@{ @@ -176,7 +177,7 @@ struct MipGnssSpartnConfiguration FunctionSelector function = static_cast(0); uint8_t enable = 0; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type = 0; ///< Connection type (1 = Network, 2 = L-Band) + uint8_t type = 0; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) uint32_t current_key_tow = 0; ///< The GPS time of week the current key is valid until uint16_t current_key_week = 0; ///< The GPS week number the current key is valid until uint8_t current_key[32] = {0}; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) @@ -190,7 +191,7 @@ struct MipGnssSpartnConfiguration static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SPARTN_CONFIGURATION; uint8_t enable = 0; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type = 0; ///< Connection type (1 = Network, 2 = L-Band) + uint8_t type = 0; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) uint32_t current_key_tow = 0; ///< The GPS time of week the current key is valid until uint16_t current_key_week = 0; ///< The GPS week number the current key is valid until uint8_t current_key[32] = {0}; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) From 1b724ca771705ebf119b55f3fb79920bab00ea67 Mon Sep 17 00:00:00 2001 From: ianjmoore Date: Fri, 6 Jan 2023 15:27:29 -0500 Subject: [PATCH 027/252] Added CX5-15 example in c and cpp. Found bug in CX5_15_ExampleC in device time calculation. --- examples/CMakeLists.txt | 11 +- examples/CX5_15/CX5_15_example.c | 415 +++++++++++++++++++++++++++++ examples/CX5_15/CX5_15_example.cpp | 341 ++++++++++++++++++++++++ 3 files changed, 766 insertions(+), 1 deletion(-) create mode 100644 examples/CX5_15/CX5_15_example.c create mode 100644 examples/CX5_15/CX5_15_example.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 0e38cf416..3c9c9ee23 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -51,6 +51,11 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") + add_executable(CX5_15_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CX5_15/CX5_15_example.cpp" ${DEVICE_SOURCES}) + target_link_libraries(CX5_15_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") + target_compile_definitions(CX5_15_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + + endif() endif() @@ -68,4 +73,8 @@ if(MIP_USE_SERIAL) add_executable(GX5_45_ExampleC "${EXAMPLE_DIR}/GX5_45/GX5_45_example.c") target_link_libraries(GX5_45_ExampleC mip) -endif() \ No newline at end of file + + add_executable(CX5_15_ExampleC "${EXAMPLE_DIR}/CX5_15/CX5_15_example.c") + target_link_libraries(CX5_15_ExampleC mip) + +endif() diff --git a/examples/CX5_15/CX5_15_example.c b/examples/CX5_15/CX5_15_example.c new file mode 100644 index 000000000..7b2d797c9 --- /dev/null +++ b/examples/CX5_15/CX5_15_example.c @@ -0,0 +1,415 @@ + +///////////////////////////////////////////////////////////////////////////// +// +// CX5_15_Example.c +// +// C Example set-up program for the CX5-15 +// +// This example shows a typical setup for the CX5-15 sensor in a wheeled-vehicle application using using C. +// It is not an exhaustive example of all CX5-15 settings. +// If your specific setup needs are not met by this example, please consult +// the MSCL-embedded API documentation for the proper commands. +// +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include +#include + + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + +serial_port device_port; +clock_t start_time; + +int port = -1; +uint8_t parse_buffer[1024]; +mip_interface device; + +//Sensor-to-vehicle frame rotation (Euler Angles) +float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; + +//GNSS antenna offset +float gnss_antenna_offset_meters[3] = {-0.25, 0.0, 0.0}; + +//Device data stores +mip_sensor_gps_timestamp_data sensor_gps_time; +mip_sensor_scaled_accel_data sensor_accel; +mip_sensor_scaled_gyro_data sensor_gyro; +mip_sensor_scaled_mag_data sensor_mag; + +mip_gnss_fix_info_data gnss_fix_info; + +bool gnss_fix_info_valid = false; + +mip_filter_timestamp_data filter_gps_time; +mip_filter_status_data filter_status; +mip_filter_position_llh_data filter_position_llh; +mip_filter_velocity_ned_data filter_velocity_ned; +mip_filter_euler_angles_data filter_euler_angles; +mip_filter_comp_angular_rate_data filter_comp_angular_rate; +mip_filter_comp_accel_data filter_comp_accel; + +bool filter_state_running = false; + + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + + +//Required MIP interface user-defined functions +timestamp_type get_current_timestamp(); + +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); + +int usage(const char* argv0); + +void exit_gracefully(const char *message); +bool should_exit(); + + +//////////////////////////////////////////////////////////////////////////////// +// Main Function +//////////////////////////////////////////////////////////////////////////////// + + +int main(int argc, const char* argv[]) +{ + + // + //Process arguments + // + + if(argc != 3) + return usage(argv[0]); + + const char* port_name = argv[1]; + uint32_t baudrate = atoi(argv[2]); + + if(baudrate == 0) + return usage(argv[0]); + + // + //Get the program start time + // + + start_time = clock(); + + printf("Connecting to and configuring sensor.\n"); + + // + //Open the device port + // + + if(!serial_port_open(&device_port, port_name, baudrate)) + exit_gracefully("ERROR: Could not open device port!"); + + + // + //Initialize the MIP interface + // + + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(mip_base_ping(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + + // + //Idle the device (note: this is good to do during setup) + // + + if(mip_base_set_idle(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + + if(mip_3dm_default_device_settings(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + const uint16_t sample_time = 15000; + mip_cmd_queue* queue = mip_interface_cmd_queue(&device); + const int16_t old_mip_sdk_timeout = mip_cmd_queue_base_reply_timeout(queue); + printf("Capturing gyro bias. This will take %d seconds. \n", sample_time/1000); + mip_cmd_queue_set_base_reply_timeout(queue, sample_time * 2); + float gyro_bias[3] = {0, 0, 0}; + + if(mip_3dm_capture_gyro_bias(&device, sample_time, gyro_bias) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + mip_cmd_queue_set_base_reply_timeout(queue, old_mip_sdk_timeout); + + // + //Setup Sensor data format to 100 Hz + // + + uint16_t sensor_base_rate; + + //Note: Querying the device base rate is only one way to calculate the descriptor decimation. + //We could have also set it directly with information from the datasheet (shown in GNSS setup). + + if(mip_3dm_imu_get_base_rate(&device, &sensor_base_rate) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not get sensor base rate format!"); + + const uint16_t sensor_sample_rate = 100; // Hz + const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; + + const mip_descriptor_rate sensor_descriptors[4] = { + { MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, sensor_decimation }, + { MIP_DATA_DESC_SENSOR_ACCEL_SCALED, sensor_decimation }, + { MIP_DATA_DESC_SENSOR_GYRO_SCALED, sensor_decimation }, + { MIP_DATA_DESC_SENSOR_MAG_SCALED, sensor_decimation }, + }; + + if(mip_3dm_write_imu_message_format(&device, 4, sensor_descriptors) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set sensor message format!"); + + + // + //Setup GNSS data format to 4 Hz (decimation of 1) + // + + const mip_descriptor_rate gnss_descriptors[1] = { + { MIP_DATA_DESC_GNSS_FIX_INFO, 1 } + }; + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + + if(mip_3dm_filter_get_base_rate(&device, &filter_base_rate) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + const mip_descriptor_rate filter_descriptors[5] = { + { MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, filter_decimation }, + { MIP_DATA_DESC_FILTER_FILTER_STATUS, filter_decimation }, + { MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, filter_decimation }, + { MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, filter_decimation }, + { MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, filter_decimation }, + }; + + if(mip_3dm_write_filter_message_format(&device, 5, filter_descriptors) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + // + //Setup the sensor to vehicle rotation + // + + if(mip_filter_write_sensor_to_vehicle_rotation_euler(&device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); + + // + //Enable filter auto-initialization + // + + if(mip_filter_write_auto_init_control(&device, 1) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set filter autoinit control!"); + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(mip_filter_reset(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Sensor Data + mip_dispatch_handler sensor_data_handlers[4]; + + mip_interface_register_extractor(&device, &sensor_data_handlers[0], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, extract_mip_sensor_gps_timestamp_data_from_field, &sensor_gps_time); + mip_interface_register_extractor(&device, &sensor_data_handlers[1], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_ACCEL_SCALED, extract_mip_sensor_scaled_accel_data_from_field, &sensor_accel); + mip_interface_register_extractor(&device, &sensor_data_handlers[2], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_GYRO_SCALED, extract_mip_sensor_scaled_gyro_data_from_field, &sensor_gyro); + mip_interface_register_extractor(&device, &sensor_data_handlers[3], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_MAG_SCALED, extract_mip_sensor_scaled_mag_data_from_field, &sensor_mag); + + //GNSS Data + mip_dispatch_handler gnss_data_handlers[1]; + + mip_interface_register_extractor(&device, &gnss_data_handlers[0], MIP_GNSS1_DATA_DESC_SET, MIP_DATA_DESC_GNSS_FIX_INFO, extract_mip_gnss_fix_info_data_from_field, &gnss_fix_info); + + //Filter Data + mip_dispatch_handler filter_data_handlers[5]; + + mip_interface_register_extractor(&device, &filter_data_handlers[0], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, extract_mip_filter_timestamp_data_from_field, &filter_gps_time); + mip_interface_register_extractor(&device, &filter_data_handlers[1], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_STATUS, extract_mip_filter_status_data_from_field, &filter_status); + mip_interface_register_extractor(&device, &filter_data_handlers[2], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_LLH, extract_mip_filter_position_llh_data_from_field, &filter_position_llh); + mip_interface_register_extractor(&device, &filter_data_handlers[3], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_NED, extract_mip_filter_velocity_ned_data_from_field, &filter_velocity_ned); + mip_interface_register_extractor(&device, &filter_data_handlers[4], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, extract_mip_filter_euler_angles_data_from_field, &filter_euler_angles); + mip_interface_register_extractor(&device, &filter_data_handlers[5], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, extract_mip_filter_comp_angular_rate_data_from_field, &filter_comp_angular_rate); + mip_interface_register_extractor(&device, &filter_data_handlers[6], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, extract_mip_filter_comp_accel_data_from_field, &filter_comp_accel); + + + // + //Resume the device + // + + if(mip_base_resume(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + + // + //Main Loop: Update the interface and process data + // + + bool running = true; + timestamp_type prev_print_timestamp = 0; + + printf("Sensor is configured... waiting for filter to enter running mode.\n"); + + while(running) + { + mip_interface_update(&device, false); + + //Check Filter State + if((!filter_state_running) && ((filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_VALID))) + { + printf("NOTE: Filter has entered running mode.\n"); + filter_state_running = true; + } + + //Once in running mode, print out data at 1 Hz + if(filter_state_running) + { + + timestamp_type curr_time = get_current_timestamp(); + printf("%lu \n", curr_time-prev_print_timestamp); + + if(curr_time - prev_print_timestamp >= 1000) + { + printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", + filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, + filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], + filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); + + prev_print_timestamp = curr_time; + } + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface Time Access Function +//////////////////////////////////////////////////////////////////////////////// + +timestamp_type get_current_timestamp() +{ + clock_t curr_time; + curr_time = clock(); + + return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface User Recv Data Function +//////////////////////////////////////////////////////////////////////////////// + +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +{ + *timestamp_out = get_current_timestamp(); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface User Send Data Function +//////////////////////////////////////////////////////////////////////////////// + +bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length) +{ + size_t bytes_written; + + return serial_port_write(&device_port, data, length, &bytes_written); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + + //Close com port + if(serial_port_is_open(&device_port)) + serial_port_close(&device_port); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; + +} + diff --git a/examples/CX5_15/CX5_15_example.cpp b/examples/CX5_15/CX5_15_example.cpp new file mode 100644 index 000000000..91747a3bb --- /dev/null +++ b/examples/CX5_15/CX5_15_example.cpp @@ -0,0 +1,341 @@ + +///////////////////////////////////////////////////////////////////////////// +// +// CX5_15_Example.cpp +// +// C++ Example set-up program for the CX5-15 +// +// This example shows a typical setup for the CX5-15 sensor in a wheeled-vehicle application using using C++. +// It is not an exhaustive example of all CX5-15 settings. +// If your specific setup needs are not met by this example, please consult +// the MSCL-embedded API documentation for the proper commands. +// +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include "../example_utils.hpp" + +using namespace mip; + + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + + +//Sensor-to-vehicle frame rotation (Euler Angles) +float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; + +//GNSS antenna offset +float gnss_antenna_offset_meters[3] = {-0.25, 0.0, 0.0}; + +//Device data stores +data_sensor::GpsTimestamp sensor_gps_time; +data_sensor::ScaledAccel sensor_accel; +data_sensor::ScaledGyro sensor_gyro; +data_sensor::ScaledMag sensor_mag; + +data_gnss::FixInfo gnss_fix_info; + +bool gnss_fix_info_valid = false; + +data_filter::Timestamp filter_gps_time; +data_filter::Status filter_status; +data_filter::PositionLlh filter_position_llh; +data_filter::VelocityNed filter_velocity_ned; +data_filter::EulerAngles filter_euler_angles; +data_filter::CompAngularRate filter_comp_angular_rate; +data_filter::CompAccel filter_comp_accel; + +bool filter_state_running = false; + + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0); + +void exit_gracefully(const char *message); +bool should_exit(); + + +//////////////////////////////////////////////////////////////////////////////// +// Main Function +//////////////////////////////////////////////////////////////////////////////// + + +int main(int argc, const char* argv[]) +{ + + std::unique_ptr utils = handleCommonArgs(argc, argv); + std::unique_ptr& device = utils->device; + + printf("Connecting to and configuring sensor.\n"); + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(commands_base::ping(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + + // + //Idle the device (note: this is good to do during setup) + // + + if(commands_base::setIdle(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + + if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + + float gyro_bias[3] = {0, 0, 0}; + + const uint32_t sampling_time = 15000; + const int32_t old_mip_sdk_timeout = device->baseReplyTimeout(); + printf("Capturing gyro bias. This will take %d seconds \n", sampling_time/1000); + device->setBaseReplyTimeout(sampling_time * 2); + + if(commands_3dm::captureGyroBias(*device, sampling_time, gyro_bias) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not capture gyro bias!"); + + if(commands_3dm::saveGyroBias(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not save gyro bias!"); + + const uint8_t fn_selector = 1; + const uint8_t device_selector = 3; + const uint8_t enable_flag = 1; + if(commands_3dm::writeDatastreamControl(*device, device_selector, enable_flag) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not enable device data stream!"); + + // Reset the timeout + device->setBaseReplyTimeout(old_mip_sdk_timeout); + + printf("Gyro bias captured with sampling time: %d, and gyro bias captured as: %f %f %f.\n", sampling_time, gyro_bias[0], gyro_bias[1], gyro_bias[3]); + + + // + //Setup Sensor data format to 100 Hz + // + + uint16_t sensor_base_rate; + + //Note: Querying the device base rate is only one way to calculate the descriptor decimation. + //We could have also set it directly with information from the datasheet (shown in GNSS setup). + + if(commands_3dm::imuGetBaseRate(*device, &sensor_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get sensor base rate format!"); + + const uint16_t sensor_sample_rate = 100; // Hz + const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; + + std::array sensor_descriptors = {{ + { data_sensor::DATA_TIME_STAMP_GPS, sensor_decimation }, + { data_sensor::DATA_ACCEL_SCALED, sensor_decimation }, + { data_sensor::DATA_GYRO_SCALED, sensor_decimation }, + { data_sensor::DATA_MAG_SCALED, sensor_decimation }, + }}; + + if(commands_3dm::writeImuMessageFormat(*device, sensor_descriptors.size(), sensor_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set sensor message format!"); + + + // + //Setup GNSS data format to 4 Hz (decimation of 1) + // + + std::array gnss_descriptors = {{ + { data_gnss::DATA_FIX_INFO, 1 } + }}; + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + + if(commands_3dm::filterGetBaseRate(*device, &filter_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + std::array filter_descriptors = {{ + { data_filter::DATA_FILTER_TIMESTAMP, filter_decimation }, + { data_filter::DATA_FILTER_STATUS, filter_decimation }, + { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, + { data_filter::DATA_COMPENSATED_ANGULAR_RATE, filter_decimation }, + { data_filter::DATA_COMPENSATED_ACCELERATION, filter_decimation }, + }}; + + if(commands_3dm::writeFilterMessageFormat(*device, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + // + //Setup the sensor to vehicle rotation + // + + if(commands_filter::writeSensorToVehicleRotationEuler(*device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); + + // + //Enable filter auto-initialization + // + + if(commands_filter::writeAutoInitControl(*device, 1) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter autoinit control!"); + + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(commands_filter::reset(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Sensor Data + DispatchHandler sensor_data_handlers[4]; + + device->registerExtractor(sensor_data_handlers[0], &sensor_gps_time); + device->registerExtractor(sensor_data_handlers[1], &sensor_accel); + device->registerExtractor(sensor_data_handlers[2], &sensor_gyro); + device->registerExtractor(sensor_data_handlers[3], &sensor_mag); + + //GNSS Data + DispatchHandler gnss_data_handlers[1]; + + device->registerExtractor(gnss_data_handlers[0], &gnss_fix_info); + + //Filter Data + DispatchHandler filter_data_handlers[7]; + + device->registerExtractor(filter_data_handlers[0], &filter_gps_time); + device->registerExtractor(filter_data_handlers[1], &filter_status); + device->registerExtractor(filter_data_handlers[2], &filter_position_llh); + device->registerExtractor(filter_data_handlers[3], &filter_velocity_ned); + device->registerExtractor(filter_data_handlers[4], &filter_euler_angles); + device->registerExtractor(filter_data_handlers[5], &filter_comp_angular_rate); + device->registerExtractor(filter_data_handlers[6], &filter_comp_accel); + + + // + //Resume the device + // + + if(commands_base::resume(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + + // + //Main Loop: Update the interface and process data + // + + bool running = true; + mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); + + printf("Sensor is configured... waiting for filter to enter running mode.\n"); + + while(running) + { + device->update(); + + //Check Filter State + if((!filter_state_running) && ((filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_VALID))) + { + printf("NOTE: Filter has entered running mode.\n"); + filter_state_running = true; + } + + //Once in running mode, print out data at 1 Hz + if(filter_state_running) + { + mip::Timestamp curr_timestamp = getCurrentTimestamp(); + + if(curr_timestamp - prev_print_timestamp >= 1000) + { + printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", + filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, + filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], + filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); + + prev_print_timestamp = curr_timestamp; + } + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; + +} + From d5bc3af1f0d8e61f6ef00ba500f05b40630e811f Mon Sep 17 00:00:00 2001 From: Rob Fisher Date: Fri, 6 Jan 2023 15:38:44 -0500 Subject: [PATCH 028/252] Updates based on test with GX5 15 --- examples/CX5_15/CX5_15_example.c | 6 ++---- examples/CX5_15/CX5_15_example.cpp | 3 +-- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/examples/CX5_15/CX5_15_example.c b/examples/CX5_15/CX5_15_example.c index 7b2d797c9..f9c2e6e6c 100644 --- a/examples/CX5_15/CX5_15_example.c +++ b/examples/CX5_15/CX5_15_example.c @@ -186,14 +186,13 @@ int main(int argc, const char* argv[]) const uint16_t sensor_sample_rate = 100; // Hz const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; - const mip_descriptor_rate sensor_descriptors[4] = { + const mip_descriptor_rate sensor_descriptors[3] = { { MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, sensor_decimation }, { MIP_DATA_DESC_SENSOR_ACCEL_SCALED, sensor_decimation }, { MIP_DATA_DESC_SENSOR_GYRO_SCALED, sensor_decimation }, - { MIP_DATA_DESC_SENSOR_MAG_SCALED, sensor_decimation }, }; - if(mip_3dm_write_imu_message_format(&device, 4, sensor_descriptors) != MIP_ACK_OK) + if(mip_3dm_write_imu_message_format(&device, 3, sensor_descriptors) != MIP_ACK_OK) exit_gracefully("ERROR: Could not set sensor message format!"); @@ -315,7 +314,6 @@ int main(int argc, const char* argv[]) { timestamp_type curr_time = get_current_timestamp(); - printf("%lu \n", curr_time-prev_print_timestamp); if(curr_time - prev_print_timestamp >= 1000) { diff --git a/examples/CX5_15/CX5_15_example.cpp b/examples/CX5_15/CX5_15_example.cpp index 91747a3bb..529c2e673 100644 --- a/examples/CX5_15/CX5_15_example.cpp +++ b/examples/CX5_15/CX5_15_example.cpp @@ -153,11 +153,10 @@ int main(int argc, const char* argv[]) const uint16_t sensor_sample_rate = 100; // Hz const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; - std::array sensor_descriptors = {{ + std::array sensor_descriptors = {{ { data_sensor::DATA_TIME_STAMP_GPS, sensor_decimation }, { data_sensor::DATA_ACCEL_SCALED, sensor_decimation }, { data_sensor::DATA_GYRO_SCALED, sensor_decimation }, - { data_sensor::DATA_MAG_SCALED, sensor_decimation }, }}; if(commands_3dm::writeImuMessageFormat(*device, sensor_descriptors.size(), sensor_descriptors.data()) != CmdResult::ACK_OK) From 3d75de6ef3f0c8a076cef01428ea82392434c679 Mon Sep 17 00:00:00 2001 From: ianjmoore Date: Fri, 6 Jan 2023 16:29:08 -0500 Subject: [PATCH 029/252] added c and cpp examples for CV5-15 and GX5-15. Tested on CV5-AHRS only. --- examples/CMakeLists.txt | 14 ++ examples/CV5_15/CV5_15_example.c | 390 +++++++++++++++++++++++++++++ examples/CV5_15/CV5_15_example.cpp | 317 +++++++++++++++++++++++ examples/CX5_15/CX5_15_example.c | 35 +-- examples/CX5_15/CX5_15_example.cpp | 29 +-- examples/GX5_15/GX5_15_example.c | 390 +++++++++++++++++++++++++++++ examples/GX5_15/GX5_15_example.cpp | 317 +++++++++++++++++++++++ 7 files changed, 1437 insertions(+), 55 deletions(-) create mode 100644 examples/CV5_15/CV5_15_example.c create mode 100644 examples/CV5_15/CV5_15_example.cpp create mode 100644 examples/GX5_15/GX5_15_example.c create mode 100644 examples/GX5_15/GX5_15_example.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 3c9c9ee23..04be70279 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -51,10 +51,18 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") + add_executable(GX5_15_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GX5_15/GX5_15_example.cpp" ${DEVICE_SOURCES}) + target_link_libraries(GX5_15_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") + target_compile_definitions(GX5_15_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + add_executable(CX5_15_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CX5_15/CX5_15_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(CX5_15_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CX5_15_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + add_executable(CV5_15_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV5_15/CV5_15_example.cpp" ${DEVICE_SOURCES}) + target_link_libraries(CV5_15_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") + target_compile_definitions(CV5_15_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + endif() @@ -74,7 +82,13 @@ if(MIP_USE_SERIAL) add_executable(GX5_45_ExampleC "${EXAMPLE_DIR}/GX5_45/GX5_45_example.c") target_link_libraries(GX5_45_ExampleC mip) + add_executable(GX5_15_ExampleC "${EXAMPLE_DIR}/GX5_15/GX5_15_example.c") + target_link_libraries(GX5_15_ExampleC mip) + add_executable(CX5_15_ExampleC "${EXAMPLE_DIR}/CX5_15/CX5_15_example.c") target_link_libraries(CX5_15_ExampleC mip) + add_executable(CV5_15_ExampleC "${EXAMPLE_DIR}/CV5_15/CV5_15_example.c") + target_link_libraries(CV5_15_ExampleC mip) + endif() diff --git a/examples/CV5_15/CV5_15_example.c b/examples/CV5_15/CV5_15_example.c new file mode 100644 index 000000000..bbbb634dd --- /dev/null +++ b/examples/CV5_15/CV5_15_example.c @@ -0,0 +1,390 @@ + +///////////////////////////////////////////////////////////////////////////// +// +// CV5_15_Example.c +// +// C Example set-up program for the CV5-15 +// +// This example shows a typical setup for the CV5-15 sensor in a wheeled-vehicle application using using C. +// It is not an exhaustive example of all CV5-15 settings. +// If your specific setup needs are not met by this example, please consult +// the MSCL-embedded API documentation for the proper commands. +// +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include +#include + + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + +serial_port device_port; +clock_t start_time; + +int port = -1; +uint8_t parse_buffer[1024]; +mip_interface device; + +//Sensor-to-vehicle frame rotation (Euler Angles) +float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; + +//Device data stores +mip_sensor_gps_timestamp_data sensor_gps_time; +mip_sensor_scaled_accel_data sensor_accel; +mip_sensor_scaled_gyro_data sensor_gyro; + +mip_filter_timestamp_data filter_gps_time; +mip_filter_status_data filter_status; +mip_filter_position_llh_data filter_position_llh; +mip_filter_velocity_ned_data filter_velocity_ned; +mip_filter_euler_angles_data filter_euler_angles; +mip_filter_comp_angular_rate_data filter_comp_angular_rate; +mip_filter_comp_accel_data filter_comp_accel; + +bool filter_state_running = false; + + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + + +//Required MIP interface user-defined functions +timestamp_type get_current_timestamp(); + +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); + +int usage(const char* argv0); + +void exit_gracefully(const char *message); +bool should_exit(); + + +//////////////////////////////////////////////////////////////////////////////// +// Main Function +//////////////////////////////////////////////////////////////////////////////// + + +int main(int argc, const char* argv[]) +{ + + // + //Process arguments + // + + if(argc != 3) + return usage(argv[0]); + + const char* port_name = argv[1]; + uint32_t baudrate = atoi(argv[2]); + + if(baudrate == 0) + return usage(argv[0]); + + // + //Get the program start time + // + + start_time = clock(); + + printf("Connecting to and configuring sensor.\n"); + + // + //Open the device port + // + + if(!serial_port_open(&device_port, port_name, baudrate)) + exit_gracefully("ERROR: Could not open device port!"); + + + // + //Initialize the MIP interface + // + + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(mip_base_ping(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + + // + //Idle the device (note: this is good to do during setup) + // + + if(mip_base_set_idle(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + + if(mip_3dm_default_device_settings(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + const uint16_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience + mip_cmd_queue* queue = mip_interface_cmd_queue(&device); + const int16_t old_mip_sdk_timeout = mip_cmd_queue_base_reply_timeout(queue); + printf("Capturing gyro bias. This will take %d seconds. \n", sampling_time/1000); + mip_cmd_queue_set_base_reply_timeout(queue, sampling_time * 2); + float gyro_bias[3] = {0, 0, 0}; + + if(mip_3dm_capture_gyro_bias(&device, sampling_time, gyro_bias) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + mip_cmd_queue_set_base_reply_timeout(queue, old_mip_sdk_timeout); + + // + //Setup Sensor data format to 100 Hz + // + + uint16_t sensor_base_rate; + + //Note: Querying the device base rate is only one way to calculate the descriptor decimation. + //We could have also set it directly with information from the datasheet. + + if(mip_3dm_imu_get_base_rate(&device, &sensor_base_rate) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not get sensor base rate format!"); + + const uint16_t sensor_sample_rate = 100; // Hz + const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; + + const mip_descriptor_rate sensor_descriptors[3] = { + { MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, sensor_decimation }, + { MIP_DATA_DESC_SENSOR_ACCEL_SCALED, sensor_decimation }, + { MIP_DATA_DESC_SENSOR_GYRO_SCALED, sensor_decimation }, + }; + + if(mip_3dm_write_imu_message_format(&device, 3, sensor_descriptors) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set sensor message format!"); + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + + if(mip_3dm_filter_get_base_rate(&device, &filter_base_rate) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + const mip_descriptor_rate filter_descriptors[5] = { + { MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, filter_decimation }, + { MIP_DATA_DESC_FILTER_FILTER_STATUS, filter_decimation }, + { MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, filter_decimation }, + { MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, filter_decimation }, + { MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, filter_decimation }, + }; + + if(mip_3dm_write_filter_message_format(&device, 5, filter_descriptors) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + // + //Setup the sensor to vehicle rotation + // + + if(mip_filter_write_sensor_to_vehicle_rotation_euler(&device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); + + // + //Enable filter auto-initialization + // + + if(mip_filter_write_auto_init_control(&device, 1) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set filter autoinit control!"); + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(mip_filter_reset(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Sensor Data + mip_dispatch_handler sensor_data_handlers[3]; + + mip_interface_register_extractor(&device, &sensor_data_handlers[0], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, extract_mip_sensor_gps_timestamp_data_from_field, &sensor_gps_time); + mip_interface_register_extractor(&device, &sensor_data_handlers[1], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_ACCEL_SCALED, extract_mip_sensor_scaled_accel_data_from_field, &sensor_accel); + mip_interface_register_extractor(&device, &sensor_data_handlers[2], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_GYRO_SCALED, extract_mip_sensor_scaled_gyro_data_from_field, &sensor_gyro); + + //Filter Data + mip_dispatch_handler filter_data_handlers[5]; + + mip_interface_register_extractor(&device, &filter_data_handlers[0], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, extract_mip_filter_timestamp_data_from_field, &filter_gps_time); + mip_interface_register_extractor(&device, &filter_data_handlers[1], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_STATUS, extract_mip_filter_status_data_from_field, &filter_status); + mip_interface_register_extractor(&device, &filter_data_handlers[2], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_LLH, extract_mip_filter_position_llh_data_from_field, &filter_position_llh); + mip_interface_register_extractor(&device, &filter_data_handlers[3], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_NED, extract_mip_filter_velocity_ned_data_from_field, &filter_velocity_ned); + mip_interface_register_extractor(&device, &filter_data_handlers[4], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, extract_mip_filter_euler_angles_data_from_field, &filter_euler_angles); + mip_interface_register_extractor(&device, &filter_data_handlers[5], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, extract_mip_filter_comp_angular_rate_data_from_field, &filter_comp_angular_rate); + mip_interface_register_extractor(&device, &filter_data_handlers[6], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, extract_mip_filter_comp_accel_data_from_field, &filter_comp_accel); + + + // + //Resume the device + // + + if(mip_base_resume(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + + // + //Main Loop: Update the interface and process data + // + + bool running = true; + timestamp_type prev_print_timestamp = 0; + + printf("Sensor is configured... waiting for filter to enter running mode.\n"); + + while(running) + { + mip_interface_update(&device, false); + + //Check Filter State + if((!filter_state_running) && ((filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_VALID))) + { + printf("NOTE: Filter has entered running mode.\n"); + filter_state_running = true; + } + + //Once in running mode, print out data at 1 Hz + if(filter_state_running) + { + + timestamp_type curr_time = get_current_timestamp(); + + if(curr_time - prev_print_timestamp >= 1000) + { + printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", + filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, + filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], + filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); + + prev_print_timestamp = curr_time; + } + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface Time Access Function +//////////////////////////////////////////////////////////////////////////////// + +timestamp_type get_current_timestamp() +{ + clock_t curr_time; + curr_time = clock(); + + return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface User Recv Data Function +//////////////////////////////////////////////////////////////////////////////// + +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +{ + *timestamp_out = get_current_timestamp(); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface User Send Data Function +//////////////////////////////////////////////////////////////////////////////// + +bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length) +{ + size_t bytes_written; + + return serial_port_write(&device_port, data, length, &bytes_written); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + + //Close com port + if(serial_port_is_open(&device_port)) + serial_port_close(&device_port); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; + +} + diff --git a/examples/CV5_15/CV5_15_example.cpp b/examples/CV5_15/CV5_15_example.cpp new file mode 100644 index 000000000..048fbf9f8 --- /dev/null +++ b/examples/CV5_15/CV5_15_example.cpp @@ -0,0 +1,317 @@ + +///////////////////////////////////////////////////////////////////////////// +// +// CV5_15_Example.cpp +// +// C++ Example set-up program for the CV5-15 +// +// This example shows a typical setup for the CV5-15 sensor in a wheeled-vehicle application using using C++. +// It is not an exhaustive example of all CV5-15 settings. +// If your specific setup needs are not met by this example, please consult +// the MSCL-embedded API documentation for the proper commands. +// +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include "../example_utils.hpp" + +using namespace mip; + + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + + +//Sensor-to-vehicle frame rotation (Euler Angles) +float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; + +//Device data stores +data_sensor::GpsTimestamp sensor_gps_time; +data_sensor::ScaledAccel sensor_accel; +data_sensor::ScaledGyro sensor_gyro; + +data_filter::Timestamp filter_gps_time; +data_filter::Status filter_status; +data_filter::PositionLlh filter_position_llh; +data_filter::VelocityNed filter_velocity_ned; +data_filter::EulerAngles filter_euler_angles; +data_filter::CompAngularRate filter_comp_angular_rate; +data_filter::CompAccel filter_comp_accel; + +bool filter_state_running = false; + + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0); + +void exit_gracefully(const char *message); +bool should_exit(); + + +//////////////////////////////////////////////////////////////////////////////// +// Main Function +//////////////////////////////////////////////////////////////////////////////// + + +int main(int argc, const char* argv[]) +{ + + std::unique_ptr utils = handleCommonArgs(argc, argv); + std::unique_ptr& device = utils->device; + + printf("Connecting to and configuring sensor.\n"); + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(commands_base::ping(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + + // + //Idle the device (note: this is good to do during setup) + // + + if(commands_base::setIdle(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + + if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + + float gyro_bias[3] = {0, 0, 0}; + + const uint32_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience + const int32_t old_mip_sdk_timeout = device->baseReplyTimeout(); + printf("Capturing gyro bias. This will take %d seconds \n", sampling_time/1000); + device->setBaseReplyTimeout(sampling_time * 2); + + if(commands_3dm::captureGyroBias(*device, sampling_time, gyro_bias) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not capture gyro bias!"); + + if(commands_3dm::saveGyroBias(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not save gyro bias!"); + + const uint8_t fn_selector = 1; + const uint8_t device_selector = 3; + const uint8_t enable_flag = 1; + if(commands_3dm::writeDatastreamControl(*device, device_selector, enable_flag) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not enable device data stream!"); + + // Reset the timeout + device->setBaseReplyTimeout(old_mip_sdk_timeout); + + printf("Gyro bias captured with sampling time: %d, and gyro bias captured as: %f %f %f.\n", sampling_time, gyro_bias[0], gyro_bias[1], gyro_bias[3]); + + + // + //Setup Sensor data format to 100 Hz + // + + uint16_t sensor_base_rate; + + //Note: Querying the device base rate is only one way to calculate the descriptor decimation. + //We could have also set it directly with information from the datasheet. + + if(commands_3dm::imuGetBaseRate(*device, &sensor_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get sensor base rate format!"); + + const uint16_t sensor_sample_rate = 100; // Hz + const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; + + std::array sensor_descriptors = {{ + { data_sensor::DATA_TIME_STAMP_GPS, sensor_decimation }, + { data_sensor::DATA_ACCEL_SCALED, sensor_decimation }, + { data_sensor::DATA_GYRO_SCALED, sensor_decimation }, + }}; + + if(commands_3dm::writeImuMessageFormat(*device, sensor_descriptors.size(), sensor_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set sensor message format!"); + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + + if(commands_3dm::filterGetBaseRate(*device, &filter_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + std::array filter_descriptors = {{ + { data_filter::DATA_FILTER_TIMESTAMP, filter_decimation }, + { data_filter::DATA_FILTER_STATUS, filter_decimation }, + { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, + { data_filter::DATA_COMPENSATED_ANGULAR_RATE, filter_decimation }, + { data_filter::DATA_COMPENSATED_ACCELERATION, filter_decimation }, + }}; + + if(commands_3dm::writeFilterMessageFormat(*device, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + // + //Setup the sensor to vehicle rotation + // + + if(commands_filter::writeSensorToVehicleRotationEuler(*device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); + + // + //Enable filter auto-initialization + // + + if(commands_filter::writeAutoInitControl(*device, 1) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter autoinit control!"); + + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(commands_filter::reset(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Sensor Data + DispatchHandler sensor_data_handlers[3]; + + device->registerExtractor(sensor_data_handlers[0], &sensor_gps_time); + device->registerExtractor(sensor_data_handlers[1], &sensor_accel); + device->registerExtractor(sensor_data_handlers[2], &sensor_gyro); + + //Filter Data + DispatchHandler filter_data_handlers[7]; + + device->registerExtractor(filter_data_handlers[0], &filter_gps_time); + device->registerExtractor(filter_data_handlers[1], &filter_status); + device->registerExtractor(filter_data_handlers[2], &filter_position_llh); + device->registerExtractor(filter_data_handlers[3], &filter_velocity_ned); + device->registerExtractor(filter_data_handlers[4], &filter_euler_angles); + device->registerExtractor(filter_data_handlers[5], &filter_comp_angular_rate); + device->registerExtractor(filter_data_handlers[6], &filter_comp_accel); + + + // + //Resume the device + // + + if(commands_base::resume(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + + // + //Main Loop: Update the interface and process data + // + + bool running = true; + mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); + + printf("Sensor is configured... waiting for filter to enter running mode.\n"); + + while(running) + { + device->update(); + + //Check Filter State + if((!filter_state_running) && ((filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_VALID))) + { + printf("NOTE: Filter has entered running mode.\n"); + filter_state_running = true; + } + + //Once in running mode, print out data at 1 Hz + if(filter_state_running) + { + mip::Timestamp curr_timestamp = getCurrentTimestamp(); + + if(curr_timestamp - prev_print_timestamp >= 1000) + { + printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", + filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, + filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], + filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); + + prev_print_timestamp = curr_timestamp; + } + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; + +} + diff --git a/examples/CX5_15/CX5_15_example.c b/examples/CX5_15/CX5_15_example.c index f9c2e6e6c..d58e6fbdf 100644 --- a/examples/CX5_15/CX5_15_example.c +++ b/examples/CX5_15/CX5_15_example.c @@ -49,18 +49,10 @@ mip_interface device; //Sensor-to-vehicle frame rotation (Euler Angles) float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; -//GNSS antenna offset -float gnss_antenna_offset_meters[3] = {-0.25, 0.0, 0.0}; - //Device data stores mip_sensor_gps_timestamp_data sensor_gps_time; mip_sensor_scaled_accel_data sensor_accel; mip_sensor_scaled_gyro_data sensor_gyro; -mip_sensor_scaled_mag_data sensor_mag; - -mip_gnss_fix_info_data gnss_fix_info; - -bool gnss_fix_info_valid = false; mip_filter_timestamp_data filter_gps_time; mip_filter_status_data filter_status; @@ -159,14 +151,14 @@ int main(int argc, const char* argv[]) if(mip_3dm_default_device_settings(&device) != MIP_ACK_OK) exit_gracefully("ERROR: Could not load default device settings!"); - const uint16_t sample_time = 15000; + const uint16_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience mip_cmd_queue* queue = mip_interface_cmd_queue(&device); const int16_t old_mip_sdk_timeout = mip_cmd_queue_base_reply_timeout(queue); - printf("Capturing gyro bias. This will take %d seconds. \n", sample_time/1000); - mip_cmd_queue_set_base_reply_timeout(queue, sample_time * 2); + printf("Capturing gyro bias. This will take %d seconds. \n", sampling_time/1000); + mip_cmd_queue_set_base_reply_timeout(queue, sampling_time * 2); float gyro_bias[3] = {0, 0, 0}; - if(mip_3dm_capture_gyro_bias(&device, sample_time, gyro_bias) != MIP_ACK_OK) + if(mip_3dm_capture_gyro_bias(&device, sampling_time, gyro_bias) != MIP_ACK_OK) exit_gracefully("ERROR: Could not load default device settings!"); mip_cmd_queue_set_base_reply_timeout(queue, old_mip_sdk_timeout); @@ -178,7 +170,7 @@ int main(int argc, const char* argv[]) uint16_t sensor_base_rate; //Note: Querying the device base rate is only one way to calculate the descriptor decimation. - //We could have also set it directly with information from the datasheet (shown in GNSS setup). + //We could have also set it directly with information from the datasheet. if(mip_3dm_imu_get_base_rate(&device, &sensor_base_rate) != MIP_ACK_OK) exit_gracefully("ERROR: Could not get sensor base rate format!"); @@ -196,15 +188,6 @@ int main(int argc, const char* argv[]) exit_gracefully("ERROR: Could not set sensor message format!"); - // - //Setup GNSS data format to 4 Hz (decimation of 1) - // - - const mip_descriptor_rate gnss_descriptors[1] = { - { MIP_DATA_DESC_GNSS_FIX_INFO, 1 } - }; - - // //Setup FILTER data format // @@ -257,17 +240,11 @@ int main(int argc, const char* argv[]) // //Sensor Data - mip_dispatch_handler sensor_data_handlers[4]; + mip_dispatch_handler sensor_data_handlers[3]; mip_interface_register_extractor(&device, &sensor_data_handlers[0], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, extract_mip_sensor_gps_timestamp_data_from_field, &sensor_gps_time); mip_interface_register_extractor(&device, &sensor_data_handlers[1], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_ACCEL_SCALED, extract_mip_sensor_scaled_accel_data_from_field, &sensor_accel); mip_interface_register_extractor(&device, &sensor_data_handlers[2], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_GYRO_SCALED, extract_mip_sensor_scaled_gyro_data_from_field, &sensor_gyro); - mip_interface_register_extractor(&device, &sensor_data_handlers[3], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_MAG_SCALED, extract_mip_sensor_scaled_mag_data_from_field, &sensor_mag); - - //GNSS Data - mip_dispatch_handler gnss_data_handlers[1]; - - mip_interface_register_extractor(&device, &gnss_data_handlers[0], MIP_GNSS1_DATA_DESC_SET, MIP_DATA_DESC_GNSS_FIX_INFO, extract_mip_gnss_fix_info_data_from_field, &gnss_fix_info); //Filter Data mip_dispatch_handler filter_data_handlers[5]; diff --git a/examples/CX5_15/CX5_15_example.cpp b/examples/CX5_15/CX5_15_example.cpp index 529c2e673..742cfd209 100644 --- a/examples/CX5_15/CX5_15_example.cpp +++ b/examples/CX5_15/CX5_15_example.cpp @@ -42,18 +42,10 @@ using namespace mip; //Sensor-to-vehicle frame rotation (Euler Angles) float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; -//GNSS antenna offset -float gnss_antenna_offset_meters[3] = {-0.25, 0.0, 0.0}; - //Device data stores data_sensor::GpsTimestamp sensor_gps_time; data_sensor::ScaledAccel sensor_accel; data_sensor::ScaledGyro sensor_gyro; -data_sensor::ScaledMag sensor_mag; - -data_gnss::FixInfo gnss_fix_info; - -bool gnss_fix_info_valid = false; data_filter::Timestamp filter_gps_time; data_filter::Status filter_status; @@ -115,7 +107,7 @@ int main(int argc, const char* argv[]) float gyro_bias[3] = {0, 0, 0}; - const uint32_t sampling_time = 15000; + const uint32_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience const int32_t old_mip_sdk_timeout = device->baseReplyTimeout(); printf("Capturing gyro bias. This will take %d seconds \n", sampling_time/1000); device->setBaseReplyTimeout(sampling_time * 2); @@ -145,7 +137,7 @@ int main(int argc, const char* argv[]) uint16_t sensor_base_rate; //Note: Querying the device base rate is only one way to calculate the descriptor decimation. - //We could have also set it directly with information from the datasheet (shown in GNSS setup). + //We could have also set it directly with information from the datasheet. if(commands_3dm::imuGetBaseRate(*device, &sensor_base_rate) != CmdResult::ACK_OK) exit_gracefully("ERROR: Could not get sensor base rate format!"); @@ -163,15 +155,6 @@ int main(int argc, const char* argv[]) exit_gracefully("ERROR: Could not set sensor message format!"); - // - //Setup GNSS data format to 4 Hz (decimation of 1) - // - - std::array gnss_descriptors = {{ - { data_gnss::DATA_FIX_INFO, 1 } - }}; - - // //Setup FILTER data format // @@ -225,17 +208,11 @@ int main(int argc, const char* argv[]) // //Sensor Data - DispatchHandler sensor_data_handlers[4]; + DispatchHandler sensor_data_handlers[3]; device->registerExtractor(sensor_data_handlers[0], &sensor_gps_time); device->registerExtractor(sensor_data_handlers[1], &sensor_accel); device->registerExtractor(sensor_data_handlers[2], &sensor_gyro); - device->registerExtractor(sensor_data_handlers[3], &sensor_mag); - - //GNSS Data - DispatchHandler gnss_data_handlers[1]; - - device->registerExtractor(gnss_data_handlers[0], &gnss_fix_info); //Filter Data DispatchHandler filter_data_handlers[7]; diff --git a/examples/GX5_15/GX5_15_example.c b/examples/GX5_15/GX5_15_example.c new file mode 100644 index 000000000..9790338cd --- /dev/null +++ b/examples/GX5_15/GX5_15_example.c @@ -0,0 +1,390 @@ + +///////////////////////////////////////////////////////////////////////////// +// +// GX5_15_Example.c +// +// C Example set-up program for the GX5-15 +// +// This example shows a typical setup for the GX5-15 sensor in a wheeled-vehicle application using using C. +// It is not an exhaustive example of all GX5-15 settings. +// If your specific setup needs are not met by this example, please consult +// the MSCL-embedded API documentation for the proper commands. +// +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include +#include + + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + +serial_port device_port; +clock_t start_time; + +int port = -1; +uint8_t parse_buffer[1024]; +mip_interface device; + +//Sensor-to-vehicle frame rotation (Euler Angles) +float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; + +//Device data stores +mip_sensor_gps_timestamp_data sensor_gps_time; +mip_sensor_scaled_accel_data sensor_accel; +mip_sensor_scaled_gyro_data sensor_gyro; + +mip_filter_timestamp_data filter_gps_time; +mip_filter_status_data filter_status; +mip_filter_position_llh_data filter_position_llh; +mip_filter_velocity_ned_data filter_velocity_ned; +mip_filter_euler_angles_data filter_euler_angles; +mip_filter_comp_angular_rate_data filter_comp_angular_rate; +mip_filter_comp_accel_data filter_comp_accel; + +bool filter_state_running = false; + + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + + +//Required MIP interface user-defined functions +timestamp_type get_current_timestamp(); + +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); + +int usage(const char* argv0); + +void exit_gracefully(const char *message); +bool should_exit(); + + +//////////////////////////////////////////////////////////////////////////////// +// Main Function +//////////////////////////////////////////////////////////////////////////////// + + +int main(int argc, const char* argv[]) +{ + + // + //Process arguments + // + + if(argc != 3) + return usage(argv[0]); + + const char* port_name = argv[1]; + uint32_t baudrate = atoi(argv[2]); + + if(baudrate == 0) + return usage(argv[0]); + + // + //Get the program start time + // + + start_time = clock(); + + printf("Connecting to and configuring sensor.\n"); + + // + //Open the device port + // + + if(!serial_port_open(&device_port, port_name, baudrate)) + exit_gracefully("ERROR: Could not open device port!"); + + + // + //Initialize the MIP interface + // + + mip_interface_init( + &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, + &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL + ); + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(mip_base_ping(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + + // + //Idle the device (note: this is good to do during setup) + // + + if(mip_base_set_idle(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + + if(mip_3dm_default_device_settings(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + const uint16_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience + mip_cmd_queue* queue = mip_interface_cmd_queue(&device); + const int16_t old_mip_sdk_timeout = mip_cmd_queue_base_reply_timeout(queue); + printf("Capturing gyro bias. This will take %d seconds. \n", sampling_time/1000); + mip_cmd_queue_set_base_reply_timeout(queue, sampling_time * 2); + float gyro_bias[3] = {0, 0, 0}; + + if(mip_3dm_capture_gyro_bias(&device, sampling_time, gyro_bias) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + mip_cmd_queue_set_base_reply_timeout(queue, old_mip_sdk_timeout); + + // + //Setup Sensor data format to 100 Hz + // + + uint16_t sensor_base_rate; + + //Note: Querying the device base rate is only one way to calculate the descriptor decimation. + //We could have also set it directly with information from the datasheet. + + if(mip_3dm_imu_get_base_rate(&device, &sensor_base_rate) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not get sensor base rate format!"); + + const uint16_t sensor_sample_rate = 100; // Hz + const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; + + const mip_descriptor_rate sensor_descriptors[3] = { + { MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, sensor_decimation }, + { MIP_DATA_DESC_SENSOR_ACCEL_SCALED, sensor_decimation }, + { MIP_DATA_DESC_SENSOR_GYRO_SCALED, sensor_decimation }, + }; + + if(mip_3dm_write_imu_message_format(&device, 3, sensor_descriptors) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set sensor message format!"); + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + + if(mip_3dm_filter_get_base_rate(&device, &filter_base_rate) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + const mip_descriptor_rate filter_descriptors[5] = { + { MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, filter_decimation }, + { MIP_DATA_DESC_FILTER_FILTER_STATUS, filter_decimation }, + { MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, filter_decimation }, + { MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, filter_decimation }, + { MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, filter_decimation }, + }; + + if(mip_3dm_write_filter_message_format(&device, 5, filter_descriptors) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + // + //Setup the sensor to vehicle rotation + // + + if(mip_filter_write_sensor_to_vehicle_rotation_euler(&device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); + + // + //Enable filter auto-initialization + // + + if(mip_filter_write_auto_init_control(&device, 1) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not set filter autoinit control!"); + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(mip_filter_reset(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Sensor Data + mip_dispatch_handler sensor_data_handlers[3]; + + mip_interface_register_extractor(&device, &sensor_data_handlers[0], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, extract_mip_sensor_gps_timestamp_data_from_field, &sensor_gps_time); + mip_interface_register_extractor(&device, &sensor_data_handlers[1], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_ACCEL_SCALED, extract_mip_sensor_scaled_accel_data_from_field, &sensor_accel); + mip_interface_register_extractor(&device, &sensor_data_handlers[2], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_GYRO_SCALED, extract_mip_sensor_scaled_gyro_data_from_field, &sensor_gyro); + + //Filter Data + mip_dispatch_handler filter_data_handlers[5]; + + mip_interface_register_extractor(&device, &filter_data_handlers[0], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, extract_mip_filter_timestamp_data_from_field, &filter_gps_time); + mip_interface_register_extractor(&device, &filter_data_handlers[1], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_STATUS, extract_mip_filter_status_data_from_field, &filter_status); + mip_interface_register_extractor(&device, &filter_data_handlers[2], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_LLH, extract_mip_filter_position_llh_data_from_field, &filter_position_llh); + mip_interface_register_extractor(&device, &filter_data_handlers[3], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_NED, extract_mip_filter_velocity_ned_data_from_field, &filter_velocity_ned); + mip_interface_register_extractor(&device, &filter_data_handlers[4], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, extract_mip_filter_euler_angles_data_from_field, &filter_euler_angles); + mip_interface_register_extractor(&device, &filter_data_handlers[5], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, extract_mip_filter_comp_angular_rate_data_from_field, &filter_comp_angular_rate); + mip_interface_register_extractor(&device, &filter_data_handlers[6], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, extract_mip_filter_comp_accel_data_from_field, &filter_comp_accel); + + + // + //Resume the device + // + + if(mip_base_resume(&device) != MIP_ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + + // + //Main Loop: Update the interface and process data + // + + bool running = true; + timestamp_type prev_print_timestamp = 0; + + printf("Sensor is configured... waiting for filter to enter running mode.\n"); + + while(running) + { + mip_interface_update(&device, false); + + //Check Filter State + if((!filter_state_running) && ((filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_VALID))) + { + printf("NOTE: Filter has entered running mode.\n"); + filter_state_running = true; + } + + //Once in running mode, print out data at 1 Hz + if(filter_state_running) + { + + timestamp_type curr_time = get_current_timestamp(); + + if(curr_time - prev_print_timestamp >= 1000) + { + printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", + filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, + filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], + filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); + + prev_print_timestamp = curr_time; + } + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface Time Access Function +//////////////////////////////////////////////////////////////////////////////// + +timestamp_type get_current_timestamp() +{ + clock_t curr_time; + curr_time = clock(); + + return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface User Recv Data Function +//////////////////////////////////////////////////////////////////////////////// + +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +{ + *timestamp_out = get_current_timestamp(); + return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); +} + + +//////////////////////////////////////////////////////////////////////////////// +// MIP Interface User Send Data Function +//////////////////////////////////////////////////////////////////////////////// + +bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length) +{ + size_t bytes_written; + + return serial_port_write(&device_port, data, length, &bytes_written); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + + //Close com port + if(serial_port_is_open(&device_port)) + serial_port_close(&device_port); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; + +} + diff --git a/examples/GX5_15/GX5_15_example.cpp b/examples/GX5_15/GX5_15_example.cpp new file mode 100644 index 000000000..d88c81b06 --- /dev/null +++ b/examples/GX5_15/GX5_15_example.cpp @@ -0,0 +1,317 @@ + +///////////////////////////////////////////////////////////////////////////// +// +// GX5_15_Example.cpp +// +// C++ Example set-up program for the GX5-15 +// +// This example shows a typical setup for the GX5-15 sensor in a wheeled-vehicle application using using C++. +// It is not an exhaustive example of all GX5-15 settings. +// If your specific setup needs are not met by this example, please consult +// the MSCL-embedded API documentation for the proper commands. +// +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include "../example_utils.hpp" + +using namespace mip; + + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + + +//Sensor-to-vehicle frame rotation (Euler Angles) +float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; + +//Device data stores +data_sensor::GpsTimestamp sensor_gps_time; +data_sensor::ScaledAccel sensor_accel; +data_sensor::ScaledGyro sensor_gyro; + +data_filter::Timestamp filter_gps_time; +data_filter::Status filter_status; +data_filter::PositionLlh filter_position_llh; +data_filter::VelocityNed filter_velocity_ned; +data_filter::EulerAngles filter_euler_angles; +data_filter::CompAngularRate filter_comp_angular_rate; +data_filter::CompAccel filter_comp_accel; + +bool filter_state_running = false; + + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0); + +void exit_gracefully(const char *message); +bool should_exit(); + + +//////////////////////////////////////////////////////////////////////////////// +// Main Function +//////////////////////////////////////////////////////////////////////////////// + + +int main(int argc, const char* argv[]) +{ + + std::unique_ptr utils = handleCommonArgs(argc, argv); + std::unique_ptr& device = utils->device; + + printf("Connecting to and configuring sensor.\n"); + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(commands_base::ping(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + + // + //Idle the device (note: this is good to do during setup) + // + + if(commands_base::setIdle(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + + if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + + float gyro_bias[3] = {0, 0, 0}; + + const uint32_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience + const int32_t old_mip_sdk_timeout = device->baseReplyTimeout(); + printf("Capturing gyro bias. This will take %d seconds \n", sampling_time/1000); + device->setBaseReplyTimeout(sampling_time * 2); + + if(commands_3dm::captureGyroBias(*device, sampling_time, gyro_bias) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not capture gyro bias!"); + + if(commands_3dm::saveGyroBias(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not save gyro bias!"); + + const uint8_t fn_selector = 1; + const uint8_t device_selector = 3; + const uint8_t enable_flag = 1; + if(commands_3dm::writeDatastreamControl(*device, device_selector, enable_flag) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not enable device data stream!"); + + // Reset the timeout + device->setBaseReplyTimeout(old_mip_sdk_timeout); + + printf("Gyro bias captured with sampling time: %d, and gyro bias captured as: %f %f %f.\n", sampling_time, gyro_bias[0], gyro_bias[1], gyro_bias[3]); + + + // + //Setup Sensor data format to 100 Hz + // + + uint16_t sensor_base_rate; + + //Note: Querying the device base rate is only one way to calculate the descriptor decimation. + //We could have also set it directly with information from the datasheet. + + if(commands_3dm::imuGetBaseRate(*device, &sensor_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get sensor base rate format!"); + + const uint16_t sensor_sample_rate = 100; // Hz + const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; + + std::array sensor_descriptors = {{ + { data_sensor::DATA_TIME_STAMP_GPS, sensor_decimation }, + { data_sensor::DATA_ACCEL_SCALED, sensor_decimation }, + { data_sensor::DATA_GYRO_SCALED, sensor_decimation }, + }}; + + if(commands_3dm::writeImuMessageFormat(*device, sensor_descriptors.size(), sensor_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set sensor message format!"); + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + + if(commands_3dm::filterGetBaseRate(*device, &filter_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + std::array filter_descriptors = {{ + { data_filter::DATA_FILTER_TIMESTAMP, filter_decimation }, + { data_filter::DATA_FILTER_STATUS, filter_decimation }, + { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, + { data_filter::DATA_COMPENSATED_ANGULAR_RATE, filter_decimation }, + { data_filter::DATA_COMPENSATED_ACCELERATION, filter_decimation }, + }}; + + if(commands_3dm::writeFilterMessageFormat(*device, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + // + //Setup the sensor to vehicle rotation + // + + if(commands_filter::writeSensorToVehicleRotationEuler(*device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); + + // + //Enable filter auto-initialization + // + + if(commands_filter::writeAutoInitControl(*device, 1) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter autoinit control!"); + + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(commands_filter::reset(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Sensor Data + DispatchHandler sensor_data_handlers[3]; + + device->registerExtractor(sensor_data_handlers[0], &sensor_gps_time); + device->registerExtractor(sensor_data_handlers[1], &sensor_accel); + device->registerExtractor(sensor_data_handlers[2], &sensor_gyro); + + //Filter Data + DispatchHandler filter_data_handlers[7]; + + device->registerExtractor(filter_data_handlers[0], &filter_gps_time); + device->registerExtractor(filter_data_handlers[1], &filter_status); + device->registerExtractor(filter_data_handlers[2], &filter_position_llh); + device->registerExtractor(filter_data_handlers[3], &filter_velocity_ned); + device->registerExtractor(filter_data_handlers[4], &filter_euler_angles); + device->registerExtractor(filter_data_handlers[5], &filter_comp_angular_rate); + device->registerExtractor(filter_data_handlers[6], &filter_comp_accel); + + + // + //Resume the device + // + + if(commands_base::resume(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + + // + //Main Loop: Update the interface and process data + // + + bool running = true; + mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); + + printf("Sensor is configured... waiting for filter to enter running mode.\n"); + + while(running) + { + device->update(); + + //Check Filter State + if((!filter_state_running) && ((filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_VALID))) + { + printf("NOTE: Filter has entered running mode.\n"); + filter_state_running = true; + } + + //Once in running mode, print out data at 1 Hz + if(filter_state_running) + { + mip::Timestamp curr_timestamp = getCurrentTimestamp(); + + if(curr_timestamp - prev_print_timestamp >= 1000) + { + printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", + filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, + filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], + filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); + + prev_print_timestamp = curr_timestamp; + } + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; + +} + From ccfe5cfff7e08c210ef9d4cb8a06ad6cafbb222d Mon Sep 17 00:00:00 2001 From: ianjmoore Date: Fri, 6 Jan 2023 17:22:21 -0500 Subject: [PATCH 030/252] Removed GX5 and CV5 examples. Removed GNSS pos, vel commands. --- examples/CV5_15/CV5_15_example.c | 390 ----------------------------- examples/CV5_15/CV5_15_example.cpp | 317 ----------------------- examples/CX5_15/CX5_15_example.c | 10 +- examples/CX5_15/CX5_15_example.cpp | 12 +- examples/GX5_15/GX5_15_example.c | 390 ----------------------------- examples/GX5_15/GX5_15_example.cpp | 317 ----------------------- 6 files changed, 7 insertions(+), 1429 deletions(-) delete mode 100644 examples/CV5_15/CV5_15_example.c delete mode 100644 examples/CV5_15/CV5_15_example.cpp delete mode 100644 examples/GX5_15/GX5_15_example.c delete mode 100644 examples/GX5_15/GX5_15_example.cpp diff --git a/examples/CV5_15/CV5_15_example.c b/examples/CV5_15/CV5_15_example.c deleted file mode 100644 index bbbb634dd..000000000 --- a/examples/CV5_15/CV5_15_example.c +++ /dev/null @@ -1,390 +0,0 @@ - -///////////////////////////////////////////////////////////////////////////// -// -// CV5_15_Example.c -// -// C Example set-up program for the CV5-15 -// -// This example shows a typical setup for the CV5-15 sensor in a wheeled-vehicle application using using C. -// It is not an exhaustive example of all CV5-15 settings. -// If your specific setup needs are not met by this example, please consult -// the MSCL-embedded API documentation for the proper commands. -// -// -//!@section LICENSE -//! -//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING -//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD -//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY -//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS -//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -// -///////////////////////////////////////////////////////////////////////////// - - -//////////////////////////////////////////////////////////////////////////////// -// Include Files -//////////////////////////////////////////////////////////////////////////////// - -#include -#include -#include -#include -#include -#include - - -//////////////////////////////////////////////////////////////////////////////// -// Global Variables -//////////////////////////////////////////////////////////////////////////////// - -serial_port device_port; -clock_t start_time; - -int port = -1; -uint8_t parse_buffer[1024]; -mip_interface device; - -//Sensor-to-vehicle frame rotation (Euler Angles) -float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; - -//Device data stores -mip_sensor_gps_timestamp_data sensor_gps_time; -mip_sensor_scaled_accel_data sensor_accel; -mip_sensor_scaled_gyro_data sensor_gyro; - -mip_filter_timestamp_data filter_gps_time; -mip_filter_status_data filter_status; -mip_filter_position_llh_data filter_position_llh; -mip_filter_velocity_ned_data filter_velocity_ned; -mip_filter_euler_angles_data filter_euler_angles; -mip_filter_comp_angular_rate_data filter_comp_angular_rate; -mip_filter_comp_accel_data filter_comp_accel; - -bool filter_state_running = false; - - -//////////////////////////////////////////////////////////////////////////////// -// Function Prototypes -//////////////////////////////////////////////////////////////////////////////// - - -//Required MIP interface user-defined functions -timestamp_type get_current_timestamp(); - -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); -bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); - -int usage(const char* argv0); - -void exit_gracefully(const char *message); -bool should_exit(); - - -//////////////////////////////////////////////////////////////////////////////// -// Main Function -//////////////////////////////////////////////////////////////////////////////// - - -int main(int argc, const char* argv[]) -{ - - // - //Process arguments - // - - if(argc != 3) - return usage(argv[0]); - - const char* port_name = argv[1]; - uint32_t baudrate = atoi(argv[2]); - - if(baudrate == 0) - return usage(argv[0]); - - // - //Get the program start time - // - - start_time = clock(); - - printf("Connecting to and configuring sensor.\n"); - - // - //Open the device port - // - - if(!serial_port_open(&device_port, port_name, baudrate)) - exit_gracefully("ERROR: Could not open device port!"); - - - // - //Initialize the MIP interface - // - - mip_interface_init( - &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, - &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL - ); - - // - //Ping the device (note: this is good to do to make sure the device is present) - // - - if(mip_base_ping(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not ping the device!"); - - - // - //Idle the device (note: this is good to do during setup) - // - - if(mip_base_set_idle(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set the device to idle!"); - - - // - //Load the device default settings (so the device is in a known state) - // - - if(mip_3dm_default_device_settings(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not load default device settings!"); - - const uint16_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience - mip_cmd_queue* queue = mip_interface_cmd_queue(&device); - const int16_t old_mip_sdk_timeout = mip_cmd_queue_base_reply_timeout(queue); - printf("Capturing gyro bias. This will take %d seconds. \n", sampling_time/1000); - mip_cmd_queue_set_base_reply_timeout(queue, sampling_time * 2); - float gyro_bias[3] = {0, 0, 0}; - - if(mip_3dm_capture_gyro_bias(&device, sampling_time, gyro_bias) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not load default device settings!"); - - mip_cmd_queue_set_base_reply_timeout(queue, old_mip_sdk_timeout); - - // - //Setup Sensor data format to 100 Hz - // - - uint16_t sensor_base_rate; - - //Note: Querying the device base rate is only one way to calculate the descriptor decimation. - //We could have also set it directly with information from the datasheet. - - if(mip_3dm_imu_get_base_rate(&device, &sensor_base_rate) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not get sensor base rate format!"); - - const uint16_t sensor_sample_rate = 100; // Hz - const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; - - const mip_descriptor_rate sensor_descriptors[3] = { - { MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, sensor_decimation }, - { MIP_DATA_DESC_SENSOR_ACCEL_SCALED, sensor_decimation }, - { MIP_DATA_DESC_SENSOR_GYRO_SCALED, sensor_decimation }, - }; - - if(mip_3dm_write_imu_message_format(&device, 3, sensor_descriptors) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set sensor message format!"); - - - // - //Setup FILTER data format - // - - uint16_t filter_base_rate; - - if(mip_3dm_filter_get_base_rate(&device, &filter_base_rate) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not get filter base rate format!"); - - const uint16_t filter_sample_rate = 100; // Hz - const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; - - const mip_descriptor_rate filter_descriptors[5] = { - { MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, filter_decimation }, - { MIP_DATA_DESC_FILTER_FILTER_STATUS, filter_decimation }, - { MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, filter_decimation }, - { MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, filter_decimation }, - { MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, filter_decimation }, - }; - - if(mip_3dm_write_filter_message_format(&device, 5, filter_descriptors) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set filter message format!"); - - - // - //Setup the sensor to vehicle rotation - // - - if(mip_filter_write_sensor_to_vehicle_rotation_euler(&device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); - - // - //Enable filter auto-initialization - // - - if(mip_filter_write_auto_init_control(&device, 1) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set filter autoinit control!"); - - - // - //Reset the filter (note: this is good to do after filter setup is complete) - // - - if(mip_filter_reset(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not reset the filter!"); - - - // - // Register data callbacks - // - - //Sensor Data - mip_dispatch_handler sensor_data_handlers[3]; - - mip_interface_register_extractor(&device, &sensor_data_handlers[0], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, extract_mip_sensor_gps_timestamp_data_from_field, &sensor_gps_time); - mip_interface_register_extractor(&device, &sensor_data_handlers[1], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_ACCEL_SCALED, extract_mip_sensor_scaled_accel_data_from_field, &sensor_accel); - mip_interface_register_extractor(&device, &sensor_data_handlers[2], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_GYRO_SCALED, extract_mip_sensor_scaled_gyro_data_from_field, &sensor_gyro); - - //Filter Data - mip_dispatch_handler filter_data_handlers[5]; - - mip_interface_register_extractor(&device, &filter_data_handlers[0], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, extract_mip_filter_timestamp_data_from_field, &filter_gps_time); - mip_interface_register_extractor(&device, &filter_data_handlers[1], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_STATUS, extract_mip_filter_status_data_from_field, &filter_status); - mip_interface_register_extractor(&device, &filter_data_handlers[2], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_LLH, extract_mip_filter_position_llh_data_from_field, &filter_position_llh); - mip_interface_register_extractor(&device, &filter_data_handlers[3], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_NED, extract_mip_filter_velocity_ned_data_from_field, &filter_velocity_ned); - mip_interface_register_extractor(&device, &filter_data_handlers[4], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, extract_mip_filter_euler_angles_data_from_field, &filter_euler_angles); - mip_interface_register_extractor(&device, &filter_data_handlers[5], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, extract_mip_filter_comp_angular_rate_data_from_field, &filter_comp_angular_rate); - mip_interface_register_extractor(&device, &filter_data_handlers[6], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, extract_mip_filter_comp_accel_data_from_field, &filter_comp_accel); - - - // - //Resume the device - // - - if(mip_base_resume(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not resume the device!"); - - - // - //Main Loop: Update the interface and process data - // - - bool running = true; - timestamp_type prev_print_timestamp = 0; - - printf("Sensor is configured... waiting for filter to enter running mode.\n"); - - while(running) - { - mip_interface_update(&device, false); - - //Check Filter State - if((!filter_state_running) && ((filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_VALID))) - { - printf("NOTE: Filter has entered running mode.\n"); - filter_state_running = true; - } - - //Once in running mode, print out data at 1 Hz - if(filter_state_running) - { - - timestamp_type curr_time = get_current_timestamp(); - - if(curr_time - prev_print_timestamp >= 1000) - { - printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", - filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, - filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], - filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); - - prev_print_timestamp = curr_time; - } - } - - running = !should_exit(); - } - - exit_gracefully("Example Completed Successfully."); -} - - -//////////////////////////////////////////////////////////////////////////////// -// MIP Interface Time Access Function -//////////////////////////////////////////////////////////////////////////////// - -timestamp_type get_current_timestamp() -{ - clock_t curr_time; - curr_time = clock(); - - return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); -} - - -//////////////////////////////////////////////////////////////////////////////// -// MIP Interface User Recv Data Function -//////////////////////////////////////////////////////////////////////////////// - -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) -{ - *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); -} - - -//////////////////////////////////////////////////////////////////////////////// -// MIP Interface User Send Data Function -//////////////////////////////////////////////////////////////////////////////// - -bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length) -{ - size_t bytes_written; - - return serial_port_write(&device_port, data, length, &bytes_written); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Print Usage Function -//////////////////////////////////////////////////////////////////////////////// - -int usage(const char* argv0) -{ - printf("Usage: %s \n", argv0); - return 1; -} - - -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - - //Close com port - if(serial_port_is_open(&device_port)) - serial_port_close(&device_port); - -#ifdef _WIN32 - int dummy = getchar(); -#endif - - exit(0); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Check for Exit Condition -//////////////////////////////////////////////////////////////////////////////// - -bool should_exit() -{ - return false; - -} - diff --git a/examples/CV5_15/CV5_15_example.cpp b/examples/CV5_15/CV5_15_example.cpp deleted file mode 100644 index 048fbf9f8..000000000 --- a/examples/CV5_15/CV5_15_example.cpp +++ /dev/null @@ -1,317 +0,0 @@ - -///////////////////////////////////////////////////////////////////////////// -// -// CV5_15_Example.cpp -// -// C++ Example set-up program for the CV5-15 -// -// This example shows a typical setup for the CV5-15 sensor in a wheeled-vehicle application using using C++. -// It is not an exhaustive example of all CV5-15 settings. -// If your specific setup needs are not met by this example, please consult -// the MSCL-embedded API documentation for the proper commands. -// -// -//!@section LICENSE -//! -//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING -//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD -//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY -//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS -//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -// -///////////////////////////////////////////////////////////////////////////// - - -//////////////////////////////////////////////////////////////////////////////// -// Include Files -//////////////////////////////////////////////////////////////////////////////// - -#include -#include -#include "../example_utils.hpp" - -using namespace mip; - - -//////////////////////////////////////////////////////////////////////////////// -// Global Variables -//////////////////////////////////////////////////////////////////////////////// - - -//Sensor-to-vehicle frame rotation (Euler Angles) -float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; - -//Device data stores -data_sensor::GpsTimestamp sensor_gps_time; -data_sensor::ScaledAccel sensor_accel; -data_sensor::ScaledGyro sensor_gyro; - -data_filter::Timestamp filter_gps_time; -data_filter::Status filter_status; -data_filter::PositionLlh filter_position_llh; -data_filter::VelocityNed filter_velocity_ned; -data_filter::EulerAngles filter_euler_angles; -data_filter::CompAngularRate filter_comp_angular_rate; -data_filter::CompAccel filter_comp_accel; - -bool filter_state_running = false; - - -//////////////////////////////////////////////////////////////////////////////// -// Function Prototypes -//////////////////////////////////////////////////////////////////////////////// - -int usage(const char* argv0); - -void exit_gracefully(const char *message); -bool should_exit(); - - -//////////////////////////////////////////////////////////////////////////////// -// Main Function -//////////////////////////////////////////////////////////////////////////////// - - -int main(int argc, const char* argv[]) -{ - - std::unique_ptr utils = handleCommonArgs(argc, argv); - std::unique_ptr& device = utils->device; - - printf("Connecting to and configuring sensor.\n"); - - // - //Ping the device (note: this is good to do to make sure the device is present) - // - - if(commands_base::ping(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not ping the device!"); - - - // - //Idle the device (note: this is good to do during setup) - // - - if(commands_base::setIdle(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set the device to idle!"); - - - // - //Load the device default settings (so the device is in a known state) - // - - if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not load default device settings!"); - - - float gyro_bias[3] = {0, 0, 0}; - - const uint32_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience - const int32_t old_mip_sdk_timeout = device->baseReplyTimeout(); - printf("Capturing gyro bias. This will take %d seconds \n", sampling_time/1000); - device->setBaseReplyTimeout(sampling_time * 2); - - if(commands_3dm::captureGyroBias(*device, sampling_time, gyro_bias) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not capture gyro bias!"); - - if(commands_3dm::saveGyroBias(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not save gyro bias!"); - - const uint8_t fn_selector = 1; - const uint8_t device_selector = 3; - const uint8_t enable_flag = 1; - if(commands_3dm::writeDatastreamControl(*device, device_selector, enable_flag) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not enable device data stream!"); - - // Reset the timeout - device->setBaseReplyTimeout(old_mip_sdk_timeout); - - printf("Gyro bias captured with sampling time: %d, and gyro bias captured as: %f %f %f.\n", sampling_time, gyro_bias[0], gyro_bias[1], gyro_bias[3]); - - - // - //Setup Sensor data format to 100 Hz - // - - uint16_t sensor_base_rate; - - //Note: Querying the device base rate is only one way to calculate the descriptor decimation. - //We could have also set it directly with information from the datasheet. - - if(commands_3dm::imuGetBaseRate(*device, &sensor_base_rate) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not get sensor base rate format!"); - - const uint16_t sensor_sample_rate = 100; // Hz - const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; - - std::array sensor_descriptors = {{ - { data_sensor::DATA_TIME_STAMP_GPS, sensor_decimation }, - { data_sensor::DATA_ACCEL_SCALED, sensor_decimation }, - { data_sensor::DATA_GYRO_SCALED, sensor_decimation }, - }}; - - if(commands_3dm::writeImuMessageFormat(*device, sensor_descriptors.size(), sensor_descriptors.data()) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set sensor message format!"); - - - // - //Setup FILTER data format - // - - uint16_t filter_base_rate; - - if(commands_3dm::filterGetBaseRate(*device, &filter_base_rate) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not get filter base rate format!"); - - const uint16_t filter_sample_rate = 100; // Hz - const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; - - std::array filter_descriptors = {{ - { data_filter::DATA_FILTER_TIMESTAMP, filter_decimation }, - { data_filter::DATA_FILTER_STATUS, filter_decimation }, - { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, - { data_filter::DATA_COMPENSATED_ANGULAR_RATE, filter_decimation }, - { data_filter::DATA_COMPENSATED_ACCELERATION, filter_decimation }, - }}; - - if(commands_3dm::writeFilterMessageFormat(*device, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set filter message format!"); - - - // - //Setup the sensor to vehicle rotation - // - - if(commands_filter::writeSensorToVehicleRotationEuler(*device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); - - // - //Enable filter auto-initialization - // - - if(commands_filter::writeAutoInitControl(*device, 1) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set filter autoinit control!"); - - - - // - //Reset the filter (note: this is good to do after filter setup is complete) - // - - if(commands_filter::reset(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not reset the filter!"); - - - // - // Register data callbacks - // - - //Sensor Data - DispatchHandler sensor_data_handlers[3]; - - device->registerExtractor(sensor_data_handlers[0], &sensor_gps_time); - device->registerExtractor(sensor_data_handlers[1], &sensor_accel); - device->registerExtractor(sensor_data_handlers[2], &sensor_gyro); - - //Filter Data - DispatchHandler filter_data_handlers[7]; - - device->registerExtractor(filter_data_handlers[0], &filter_gps_time); - device->registerExtractor(filter_data_handlers[1], &filter_status); - device->registerExtractor(filter_data_handlers[2], &filter_position_llh); - device->registerExtractor(filter_data_handlers[3], &filter_velocity_ned); - device->registerExtractor(filter_data_handlers[4], &filter_euler_angles); - device->registerExtractor(filter_data_handlers[5], &filter_comp_angular_rate); - device->registerExtractor(filter_data_handlers[6], &filter_comp_accel); - - - // - //Resume the device - // - - if(commands_base::resume(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not resume the device!"); - - - // - //Main Loop: Update the interface and process data - // - - bool running = true; - mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); - - printf("Sensor is configured... waiting for filter to enter running mode.\n"); - - while(running) - { - device->update(); - - //Check Filter State - if((!filter_state_running) && ((filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_VALID))) - { - printf("NOTE: Filter has entered running mode.\n"); - filter_state_running = true; - } - - //Once in running mode, print out data at 1 Hz - if(filter_state_running) - { - mip::Timestamp curr_timestamp = getCurrentTimestamp(); - - if(curr_timestamp - prev_print_timestamp >= 1000) - { - printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", - filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, - filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], - filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); - - prev_print_timestamp = curr_timestamp; - } - } - - running = !should_exit(); - } - - exit_gracefully("Example Completed Successfully."); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Print Usage Function -//////////////////////////////////////////////////////////////////////////////// - -int usage(const char* argv0) -{ - printf("Usage: %s \n", argv0); - return 1; -} - - -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - -#ifdef _WIN32 - int dummy = getchar(); -#endif - - exit(0); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Check for Exit Condition -//////////////////////////////////////////////////////////////////////////////// - -bool should_exit() -{ - return false; - -} - diff --git a/examples/CX5_15/CX5_15_example.c b/examples/CX5_15/CX5_15_example.c index d58e6fbdf..a0f788e65 100644 --- a/examples/CX5_15/CX5_15_example.c +++ b/examples/CX5_15/CX5_15_example.c @@ -56,8 +56,6 @@ mip_sensor_scaled_gyro_data sensor_gyro; mip_filter_timestamp_data filter_gps_time; mip_filter_status_data filter_status; -mip_filter_position_llh_data filter_position_llh; -mip_filter_velocity_ned_data filter_velocity_ned; mip_filter_euler_angles_data filter_euler_angles; mip_filter_comp_angular_rate_data filter_comp_angular_rate; mip_filter_comp_accel_data filter_comp_accel; @@ -251,11 +249,9 @@ int main(int argc, const char* argv[]) mip_interface_register_extractor(&device, &filter_data_handlers[0], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, extract_mip_filter_timestamp_data_from_field, &filter_gps_time); mip_interface_register_extractor(&device, &filter_data_handlers[1], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_STATUS, extract_mip_filter_status_data_from_field, &filter_status); - mip_interface_register_extractor(&device, &filter_data_handlers[2], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_LLH, extract_mip_filter_position_llh_data_from_field, &filter_position_llh); - mip_interface_register_extractor(&device, &filter_data_handlers[3], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_NED, extract_mip_filter_velocity_ned_data_from_field, &filter_velocity_ned); - mip_interface_register_extractor(&device, &filter_data_handlers[4], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, extract_mip_filter_euler_angles_data_from_field, &filter_euler_angles); - mip_interface_register_extractor(&device, &filter_data_handlers[5], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, extract_mip_filter_comp_angular_rate_data_from_field, &filter_comp_angular_rate); - mip_interface_register_extractor(&device, &filter_data_handlers[6], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, extract_mip_filter_comp_accel_data_from_field, &filter_comp_accel); + mip_interface_register_extractor(&device, &filter_data_handlers[2], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, extract_mip_filter_euler_angles_data_from_field, &filter_euler_angles); + mip_interface_register_extractor(&device, &filter_data_handlers[3], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, extract_mip_filter_comp_angular_rate_data_from_field, &filter_comp_angular_rate); + mip_interface_register_extractor(&device, &filter_data_handlers[4], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, extract_mip_filter_comp_accel_data_from_field, &filter_comp_accel); // diff --git a/examples/CX5_15/CX5_15_example.cpp b/examples/CX5_15/CX5_15_example.cpp index 742cfd209..4d92fcd3e 100644 --- a/examples/CX5_15/CX5_15_example.cpp +++ b/examples/CX5_15/CX5_15_example.cpp @@ -49,8 +49,6 @@ data_sensor::ScaledGyro sensor_gyro; data_filter::Timestamp filter_gps_time; data_filter::Status filter_status; -data_filter::PositionLlh filter_position_llh; -data_filter::VelocityNed filter_velocity_ned; data_filter::EulerAngles filter_euler_angles; data_filter::CompAngularRate filter_comp_angular_rate; data_filter::CompAccel filter_comp_accel; @@ -215,15 +213,13 @@ int main(int argc, const char* argv[]) device->registerExtractor(sensor_data_handlers[2], &sensor_gyro); //Filter Data - DispatchHandler filter_data_handlers[7]; + DispatchHandler filter_data_handlers[5]; device->registerExtractor(filter_data_handlers[0], &filter_gps_time); device->registerExtractor(filter_data_handlers[1], &filter_status); - device->registerExtractor(filter_data_handlers[2], &filter_position_llh); - device->registerExtractor(filter_data_handlers[3], &filter_velocity_ned); - device->registerExtractor(filter_data_handlers[4], &filter_euler_angles); - device->registerExtractor(filter_data_handlers[5], &filter_comp_angular_rate); - device->registerExtractor(filter_data_handlers[6], &filter_comp_accel); + device->registerExtractor(filter_data_handlers[2], &filter_euler_angles); + device->registerExtractor(filter_data_handlers[3], &filter_comp_angular_rate); + device->registerExtractor(filter_data_handlers[4], &filter_comp_accel); // diff --git a/examples/GX5_15/GX5_15_example.c b/examples/GX5_15/GX5_15_example.c deleted file mode 100644 index 9790338cd..000000000 --- a/examples/GX5_15/GX5_15_example.c +++ /dev/null @@ -1,390 +0,0 @@ - -///////////////////////////////////////////////////////////////////////////// -// -// GX5_15_Example.c -// -// C Example set-up program for the GX5-15 -// -// This example shows a typical setup for the GX5-15 sensor in a wheeled-vehicle application using using C. -// It is not an exhaustive example of all GX5-15 settings. -// If your specific setup needs are not met by this example, please consult -// the MSCL-embedded API documentation for the proper commands. -// -// -//!@section LICENSE -//! -//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING -//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD -//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY -//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS -//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -// -///////////////////////////////////////////////////////////////////////////// - - -//////////////////////////////////////////////////////////////////////////////// -// Include Files -//////////////////////////////////////////////////////////////////////////////// - -#include -#include -#include -#include -#include -#include - - -//////////////////////////////////////////////////////////////////////////////// -// Global Variables -//////////////////////////////////////////////////////////////////////////////// - -serial_port device_port; -clock_t start_time; - -int port = -1; -uint8_t parse_buffer[1024]; -mip_interface device; - -//Sensor-to-vehicle frame rotation (Euler Angles) -float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; - -//Device data stores -mip_sensor_gps_timestamp_data sensor_gps_time; -mip_sensor_scaled_accel_data sensor_accel; -mip_sensor_scaled_gyro_data sensor_gyro; - -mip_filter_timestamp_data filter_gps_time; -mip_filter_status_data filter_status; -mip_filter_position_llh_data filter_position_llh; -mip_filter_velocity_ned_data filter_velocity_ned; -mip_filter_euler_angles_data filter_euler_angles; -mip_filter_comp_angular_rate_data filter_comp_angular_rate; -mip_filter_comp_accel_data filter_comp_accel; - -bool filter_state_running = false; - - -//////////////////////////////////////////////////////////////////////////////// -// Function Prototypes -//////////////////////////////////////////////////////////////////////////////// - - -//Required MIP interface user-defined functions -timestamp_type get_current_timestamp(); - -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); -bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); - -int usage(const char* argv0); - -void exit_gracefully(const char *message); -bool should_exit(); - - -//////////////////////////////////////////////////////////////////////////////// -// Main Function -//////////////////////////////////////////////////////////////////////////////// - - -int main(int argc, const char* argv[]) -{ - - // - //Process arguments - // - - if(argc != 3) - return usage(argv[0]); - - const char* port_name = argv[1]; - uint32_t baudrate = atoi(argv[2]); - - if(baudrate == 0) - return usage(argv[0]); - - // - //Get the program start time - // - - start_time = clock(); - - printf("Connecting to and configuring sensor.\n"); - - // - //Open the device port - // - - if(!serial_port_open(&device_port, port_name, baudrate)) - exit_gracefully("ERROR: Could not open device port!"); - - - // - //Initialize the MIP interface - // - - mip_interface_init( - &device, parse_buffer, sizeof(parse_buffer), mip_timeout_from_baudrate(baudrate), 1000, - &mip_interface_user_send_to_device, &mip_interface_user_recv_from_device, &mip_interface_default_update, NULL - ); - - // - //Ping the device (note: this is good to do to make sure the device is present) - // - - if(mip_base_ping(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not ping the device!"); - - - // - //Idle the device (note: this is good to do during setup) - // - - if(mip_base_set_idle(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set the device to idle!"); - - - // - //Load the device default settings (so the device is in a known state) - // - - if(mip_3dm_default_device_settings(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not load default device settings!"); - - const uint16_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience - mip_cmd_queue* queue = mip_interface_cmd_queue(&device); - const int16_t old_mip_sdk_timeout = mip_cmd_queue_base_reply_timeout(queue); - printf("Capturing gyro bias. This will take %d seconds. \n", sampling_time/1000); - mip_cmd_queue_set_base_reply_timeout(queue, sampling_time * 2); - float gyro_bias[3] = {0, 0, 0}; - - if(mip_3dm_capture_gyro_bias(&device, sampling_time, gyro_bias) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not load default device settings!"); - - mip_cmd_queue_set_base_reply_timeout(queue, old_mip_sdk_timeout); - - // - //Setup Sensor data format to 100 Hz - // - - uint16_t sensor_base_rate; - - //Note: Querying the device base rate is only one way to calculate the descriptor decimation. - //We could have also set it directly with information from the datasheet. - - if(mip_3dm_imu_get_base_rate(&device, &sensor_base_rate) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not get sensor base rate format!"); - - const uint16_t sensor_sample_rate = 100; // Hz - const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; - - const mip_descriptor_rate sensor_descriptors[3] = { - { MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, sensor_decimation }, - { MIP_DATA_DESC_SENSOR_ACCEL_SCALED, sensor_decimation }, - { MIP_DATA_DESC_SENSOR_GYRO_SCALED, sensor_decimation }, - }; - - if(mip_3dm_write_imu_message_format(&device, 3, sensor_descriptors) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set sensor message format!"); - - - // - //Setup FILTER data format - // - - uint16_t filter_base_rate; - - if(mip_3dm_filter_get_base_rate(&device, &filter_base_rate) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not get filter base rate format!"); - - const uint16_t filter_sample_rate = 100; // Hz - const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; - - const mip_descriptor_rate filter_descriptors[5] = { - { MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, filter_decimation }, - { MIP_DATA_DESC_FILTER_FILTER_STATUS, filter_decimation }, - { MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, filter_decimation }, - { MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, filter_decimation }, - { MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, filter_decimation }, - }; - - if(mip_3dm_write_filter_message_format(&device, 5, filter_descriptors) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set filter message format!"); - - - // - //Setup the sensor to vehicle rotation - // - - if(mip_filter_write_sensor_to_vehicle_rotation_euler(&device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); - - // - //Enable filter auto-initialization - // - - if(mip_filter_write_auto_init_control(&device, 1) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not set filter autoinit control!"); - - - // - //Reset the filter (note: this is good to do after filter setup is complete) - // - - if(mip_filter_reset(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not reset the filter!"); - - - // - // Register data callbacks - // - - //Sensor Data - mip_dispatch_handler sensor_data_handlers[3]; - - mip_interface_register_extractor(&device, &sensor_data_handlers[0], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_TIME_STAMP_GPS, extract_mip_sensor_gps_timestamp_data_from_field, &sensor_gps_time); - mip_interface_register_extractor(&device, &sensor_data_handlers[1], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_ACCEL_SCALED, extract_mip_sensor_scaled_accel_data_from_field, &sensor_accel); - mip_interface_register_extractor(&device, &sensor_data_handlers[2], MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_GYRO_SCALED, extract_mip_sensor_scaled_gyro_data_from_field, &sensor_gyro); - - //Filter Data - mip_dispatch_handler filter_data_handlers[5]; - - mip_interface_register_extractor(&device, &filter_data_handlers[0], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_TIMESTAMP, extract_mip_filter_timestamp_data_from_field, &filter_gps_time); - mip_interface_register_extractor(&device, &filter_data_handlers[1], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_STATUS, extract_mip_filter_status_data_from_field, &filter_status); - mip_interface_register_extractor(&device, &filter_data_handlers[2], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_LLH, extract_mip_filter_position_llh_data_from_field, &filter_position_llh); - mip_interface_register_extractor(&device, &filter_data_handlers[3], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_NED, extract_mip_filter_velocity_ned_data_from_field, &filter_velocity_ned); - mip_interface_register_extractor(&device, &filter_data_handlers[4], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_EULER_ANGLES, extract_mip_filter_euler_angles_data_from_field, &filter_euler_angles); - mip_interface_register_extractor(&device, &filter_data_handlers[5], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, extract_mip_filter_comp_angular_rate_data_from_field, &filter_comp_angular_rate); - mip_interface_register_extractor(&device, &filter_data_handlers[6], MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ACCELERATION, extract_mip_filter_comp_accel_data_from_field, &filter_comp_accel); - - - // - //Resume the device - // - - if(mip_base_resume(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not resume the device!"); - - - // - //Main Loop: Update the interface and process data - // - - bool running = true; - timestamp_type prev_print_timestamp = 0; - - printf("Sensor is configured... waiting for filter to enter running mode.\n"); - - while(running) - { - mip_interface_update(&device, false); - - //Check Filter State - if((!filter_state_running) && ((filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == MIP_FILTER_MODE_GX5_RUN_SOLUTION_VALID))) - { - printf("NOTE: Filter has entered running mode.\n"); - filter_state_running = true; - } - - //Once in running mode, print out data at 1 Hz - if(filter_state_running) - { - - timestamp_type curr_time = get_current_timestamp(); - - if(curr_time - prev_print_timestamp >= 1000) - { - printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", - filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, - filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], - filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); - - prev_print_timestamp = curr_time; - } - } - - running = !should_exit(); - } - - exit_gracefully("Example Completed Successfully."); -} - - -//////////////////////////////////////////////////////////////////////////////// -// MIP Interface Time Access Function -//////////////////////////////////////////////////////////////////////////////// - -timestamp_type get_current_timestamp() -{ - clock_t curr_time; - curr_time = clock(); - - return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); -} - - -//////////////////////////////////////////////////////////////////////////////// -// MIP Interface User Recv Data Function -//////////////////////////////////////////////////////////////////////////////// - -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) -{ - *timestamp_out = get_current_timestamp(); - return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); -} - - -//////////////////////////////////////////////////////////////////////////////// -// MIP Interface User Send Data Function -//////////////////////////////////////////////////////////////////////////////// - -bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length) -{ - size_t bytes_written; - - return serial_port_write(&device_port, data, length, &bytes_written); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Print Usage Function -//////////////////////////////////////////////////////////////////////////////// - -int usage(const char* argv0) -{ - printf("Usage: %s \n", argv0); - return 1; -} - - -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - - //Close com port - if(serial_port_is_open(&device_port)) - serial_port_close(&device_port); - -#ifdef _WIN32 - int dummy = getchar(); -#endif - - exit(0); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Check for Exit Condition -//////////////////////////////////////////////////////////////////////////////// - -bool should_exit() -{ - return false; - -} - diff --git a/examples/GX5_15/GX5_15_example.cpp b/examples/GX5_15/GX5_15_example.cpp deleted file mode 100644 index d88c81b06..000000000 --- a/examples/GX5_15/GX5_15_example.cpp +++ /dev/null @@ -1,317 +0,0 @@ - -///////////////////////////////////////////////////////////////////////////// -// -// GX5_15_Example.cpp -// -// C++ Example set-up program for the GX5-15 -// -// This example shows a typical setup for the GX5-15 sensor in a wheeled-vehicle application using using C++. -// It is not an exhaustive example of all GX5-15 settings. -// If your specific setup needs are not met by this example, please consult -// the MSCL-embedded API documentation for the proper commands. -// -// -//!@section LICENSE -//! -//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING -//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD -//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY -//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS -//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -// -///////////////////////////////////////////////////////////////////////////// - - -//////////////////////////////////////////////////////////////////////////////// -// Include Files -//////////////////////////////////////////////////////////////////////////////// - -#include -#include -#include "../example_utils.hpp" - -using namespace mip; - - -//////////////////////////////////////////////////////////////////////////////// -// Global Variables -//////////////////////////////////////////////////////////////////////////////// - - -//Sensor-to-vehicle frame rotation (Euler Angles) -float sensor_to_vehicle_rotation_euler[3] = {0.0, 0.0, 0.0}; - -//Device data stores -data_sensor::GpsTimestamp sensor_gps_time; -data_sensor::ScaledAccel sensor_accel; -data_sensor::ScaledGyro sensor_gyro; - -data_filter::Timestamp filter_gps_time; -data_filter::Status filter_status; -data_filter::PositionLlh filter_position_llh; -data_filter::VelocityNed filter_velocity_ned; -data_filter::EulerAngles filter_euler_angles; -data_filter::CompAngularRate filter_comp_angular_rate; -data_filter::CompAccel filter_comp_accel; - -bool filter_state_running = false; - - -//////////////////////////////////////////////////////////////////////////////// -// Function Prototypes -//////////////////////////////////////////////////////////////////////////////// - -int usage(const char* argv0); - -void exit_gracefully(const char *message); -bool should_exit(); - - -//////////////////////////////////////////////////////////////////////////////// -// Main Function -//////////////////////////////////////////////////////////////////////////////// - - -int main(int argc, const char* argv[]) -{ - - std::unique_ptr utils = handleCommonArgs(argc, argv); - std::unique_ptr& device = utils->device; - - printf("Connecting to and configuring sensor.\n"); - - // - //Ping the device (note: this is good to do to make sure the device is present) - // - - if(commands_base::ping(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not ping the device!"); - - - // - //Idle the device (note: this is good to do during setup) - // - - if(commands_base::setIdle(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set the device to idle!"); - - - // - //Load the device default settings (so the device is in a known state) - // - - if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not load default device settings!"); - - - float gyro_bias[3] = {0, 0, 0}; - - const uint32_t sampling_time = 2000; // The default is 15000 ms and longer sample times are recommended but shortened for convenience - const int32_t old_mip_sdk_timeout = device->baseReplyTimeout(); - printf("Capturing gyro bias. This will take %d seconds \n", sampling_time/1000); - device->setBaseReplyTimeout(sampling_time * 2); - - if(commands_3dm::captureGyroBias(*device, sampling_time, gyro_bias) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not capture gyro bias!"); - - if(commands_3dm::saveGyroBias(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not save gyro bias!"); - - const uint8_t fn_selector = 1; - const uint8_t device_selector = 3; - const uint8_t enable_flag = 1; - if(commands_3dm::writeDatastreamControl(*device, device_selector, enable_flag) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not enable device data stream!"); - - // Reset the timeout - device->setBaseReplyTimeout(old_mip_sdk_timeout); - - printf("Gyro bias captured with sampling time: %d, and gyro bias captured as: %f %f %f.\n", sampling_time, gyro_bias[0], gyro_bias[1], gyro_bias[3]); - - - // - //Setup Sensor data format to 100 Hz - // - - uint16_t sensor_base_rate; - - //Note: Querying the device base rate is only one way to calculate the descriptor decimation. - //We could have also set it directly with information from the datasheet. - - if(commands_3dm::imuGetBaseRate(*device, &sensor_base_rate) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not get sensor base rate format!"); - - const uint16_t sensor_sample_rate = 100; // Hz - const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; - - std::array sensor_descriptors = {{ - { data_sensor::DATA_TIME_STAMP_GPS, sensor_decimation }, - { data_sensor::DATA_ACCEL_SCALED, sensor_decimation }, - { data_sensor::DATA_GYRO_SCALED, sensor_decimation }, - }}; - - if(commands_3dm::writeImuMessageFormat(*device, sensor_descriptors.size(), sensor_descriptors.data()) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set sensor message format!"); - - - // - //Setup FILTER data format - // - - uint16_t filter_base_rate; - - if(commands_3dm::filterGetBaseRate(*device, &filter_base_rate) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not get filter base rate format!"); - - const uint16_t filter_sample_rate = 100; // Hz - const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; - - std::array filter_descriptors = {{ - { data_filter::DATA_FILTER_TIMESTAMP, filter_decimation }, - { data_filter::DATA_FILTER_STATUS, filter_decimation }, - { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, - { data_filter::DATA_COMPENSATED_ANGULAR_RATE, filter_decimation }, - { data_filter::DATA_COMPENSATED_ACCELERATION, filter_decimation }, - }}; - - if(commands_3dm::writeFilterMessageFormat(*device, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set filter message format!"); - - - // - //Setup the sensor to vehicle rotation - // - - if(commands_filter::writeSensorToVehicleRotationEuler(*device, sensor_to_vehicle_rotation_euler[0], sensor_to_vehicle_rotation_euler[1], sensor_to_vehicle_rotation_euler[2]) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set sensor-2-vehicle rotation!"); - - // - //Enable filter auto-initialization - // - - if(commands_filter::writeAutoInitControl(*device, 1) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set filter autoinit control!"); - - - - // - //Reset the filter (note: this is good to do after filter setup is complete) - // - - if(commands_filter::reset(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not reset the filter!"); - - - // - // Register data callbacks - // - - //Sensor Data - DispatchHandler sensor_data_handlers[3]; - - device->registerExtractor(sensor_data_handlers[0], &sensor_gps_time); - device->registerExtractor(sensor_data_handlers[1], &sensor_accel); - device->registerExtractor(sensor_data_handlers[2], &sensor_gyro); - - //Filter Data - DispatchHandler filter_data_handlers[7]; - - device->registerExtractor(filter_data_handlers[0], &filter_gps_time); - device->registerExtractor(filter_data_handlers[1], &filter_status); - device->registerExtractor(filter_data_handlers[2], &filter_position_llh); - device->registerExtractor(filter_data_handlers[3], &filter_velocity_ned); - device->registerExtractor(filter_data_handlers[4], &filter_euler_angles); - device->registerExtractor(filter_data_handlers[5], &filter_comp_angular_rate); - device->registerExtractor(filter_data_handlers[6], &filter_comp_accel); - - - // - //Resume the device - // - - if(commands_base::resume(*device) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not resume the device!"); - - - // - //Main Loop: Update the interface and process data - // - - bool running = true; - mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); - - printf("Sensor is configured... waiting for filter to enter running mode.\n"); - - while(running) - { - device->update(); - - //Check Filter State - if((!filter_state_running) && ((filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_ERROR) || (filter_status.filter_state == data_filter::FilterMode::GX5_RUN_SOLUTION_VALID))) - { - printf("NOTE: Filter has entered running mode.\n"); - filter_state_running = true; - } - - //Once in running mode, print out data at 1 Hz - if(filter_state_running) - { - mip::Timestamp curr_timestamp = getCurrentTimestamp(); - - if(curr_timestamp - prev_print_timestamp >= 1000) - { - printf("TOW = %f: ATT_EULER = [%f %f %f]: COMP_ANG_RATE = [%f %f %f]: COMP_ACCEL = [%f %f %f]\n", - filter_gps_time.tow, filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw, - filter_comp_angular_rate.gyro[0], filter_comp_angular_rate.gyro[1], filter_comp_angular_rate.gyro[2], - filter_comp_accel.accel[0], filter_comp_accel.accel[1], filter_comp_accel.accel[2]); - - prev_print_timestamp = curr_timestamp; - } - } - - running = !should_exit(); - } - - exit_gracefully("Example Completed Successfully."); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Print Usage Function -//////////////////////////////////////////////////////////////////////////////// - -int usage(const char* argv0) -{ - printf("Usage: %s \n", argv0); - return 1; -} - - -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - -#ifdef _WIN32 - int dummy = getchar(); -#endif - - exit(0); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Check for Exit Condition -//////////////////////////////////////////////////////////////////////////////// - -bool should_exit() -{ - return false; - -} - From 02498f1cbb6116a96d79c888812b70a6b4361b7c Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 23 Jan 2023 10:36:32 -0500 Subject: [PATCH 031/252] Generate MIP definitions from branches/dev@54243. --- src/mip/definitions/commands_3dm.h | 3 + src/mip/definitions/commands_3dm.hpp | 12 +++ src/mip/definitions/commands_base.h | 1 + src/mip/definitions/commands_base.hpp | 4 + src/mip/definitions/commands_filter.h | 3 + src/mip/definitions/commands_filter.hpp | 12 +++ src/mip/definitions/commands_rtk.h | 3 + src/mip/definitions/commands_rtk.hpp | 12 +++ src/mip/definitions/data_filter.h | 4 + src/mip/definitions/data_filter.hpp | 16 +++ src/mip/definitions/data_gnss.h | 32 ++++++ src/mip/definitions/data_gnss.hpp | 128 ++++++++++++++++++++++++ src/mip/definitions/data_sensor.h | 2 + src/mip/definitions/data_sensor.hpp | 8 ++ src/mip/definitions/data_shared.h | 3 + src/mip/definitions/data_shared.hpp | 12 +++ 16 files changed, 255 insertions(+) diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 649f4a397..45fe8ec8a 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -758,6 +758,7 @@ static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SE static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ENABLE_RANGING = 0x0001; ///< Use SBAS pseudo-ranges in position solution static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ENABLE_CORRECTIONS = 0x0002; ///< Use SBAS differential corrections static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_APPLY_INTEGRITY = 0x0004; ///< Use SBAS integrity information. If enabled, only GPS satellites for which integrity information is available will be used. +static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ALL = 0x0007; struct mip_3dm_gnss_sbas_settings_command { @@ -976,6 +977,7 @@ static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PI static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_OPEN_DRAIN = 0x01; ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pull-up resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLDOWN = 0x02; ///< The pin will have an internal pull-down resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pull-up. See the device specifications for the resistance value. static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLUP = 0x04; ///< The pin will have an internal pull-up resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pull-down. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. +static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_ALL = 0x07; struct mip_3dm_gpio_config_command { @@ -1233,6 +1235,7 @@ static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_T static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_ACTIVE = 0x01; ///< True if the trigger is currently active (either due to its logic or being in test mode). static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_ENABLED = 0x02; ///< True if the trigger is enabled. static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_TEST = 0x04; ///< True if the trigger is in test mode. +static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_ALL = 0x07; struct mip_3dm_get_event_trigger_status_command_entry { diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 1cccd0c26..845637b38 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -916,6 +916,7 @@ struct GnssSbasSettings ENABLE_RANGING = 0x0001, ///< Use SBAS pseudo-ranges in position solution ENABLE_CORRECTIONS = 0x0002, ///< Use SBAS differential corrections APPLY_INTEGRITY = 0x0004, ///< Use SBAS integrity information. If enabled, only GPS satellites for which integrity information is available will be used. + ALL = 0x0007, }; uint16_t value = NONE; @@ -933,6 +934,9 @@ struct GnssSbasSettings void enableCorrections(bool val) { if(val) value |= ENABLE_CORRECTIONS; else value &= ~ENABLE_CORRECTIONS; } bool applyIntegrity() const { return (value & APPLY_INTEGRITY) > 0; } void applyIntegrity(bool val) { if(val) value |= APPLY_INTEGRITY; else value &= ~APPLY_INTEGRITY; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -1195,6 +1199,7 @@ struct GpioConfig OPEN_DRAIN = 0x01, ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pull-up resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. PULLDOWN = 0x02, ///< The pin will have an internal pull-down resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pull-up. See the device specifications for the resistance value. PULLUP = 0x04, ///< The pin will have an internal pull-up resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pull-down. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. + ALL = 0x07, }; uint8_t value = NONE; @@ -1212,6 +1217,9 @@ struct GpioConfig void pulldown(bool val) { if(val) value |= PULLDOWN; else value &= ~PULLDOWN; } bool pullup() const { return (value & PULLUP) > 0; } void pullup(bool val) { if(val) value |= PULLUP; else value &= ~PULLUP; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -1502,6 +1510,7 @@ struct GetEventTriggerStatus ACTIVE = 0x01, ///< True if the trigger is currently active (either due to its logic or being in test mode). ENABLED = 0x02, ///< True if the trigger is enabled. TEST = 0x04, ///< True if the trigger is in test mode. + ALL = 0x07, }; uint8_t value = NONE; @@ -1519,6 +1528,9 @@ struct GetEventTriggerStatus void enabled(bool val) { if(val) value |= ENABLED; else value &= ~ENABLED; } bool test() const { return (value & TEST) > 0; } void test(bool val) { if(val) value |= TEST; else value &= ~TEST; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct Entry diff --git a/src/mip/definitions/commands_base.h b/src/mip/definitions/commands_base.h index a63e33403..073542861 100644 --- a/src/mip/definitions/commands_base.h +++ b/src/mip/definitions/commands_base.h @@ -109,6 +109,7 @@ static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_RTCM_F static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_RTK_FAULT = 0x20000000; ///< static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_SOLUTION_FAULT = 0x40000000; ///< static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_GENERAL_FAULT = 0x80000000; ///< +static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_ALL = 0xFFFFFFFF; void insert_mip_commanded_test_bits_gq7(struct mip_serializer* serializer, const mip_commanded_test_bits_gq7 self); void extract_mip_commanded_test_bits_gq7(struct mip_serializer* serializer, mip_commanded_test_bits_gq7* self); diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index ae2c775f3..c0f8abc41 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -109,6 +109,7 @@ struct CommandedTestBitsGq7 : Bitfield GNSS_RTK_FAULT = 0x20000000, ///< GNSS_SOLUTION_FAULT = 0x40000000, ///< GNSS_GENERAL_FAULT = 0x80000000, ///< + ALL = 0xFFFFFFFF, }; uint32_t value = NONE; @@ -174,6 +175,9 @@ struct CommandedTestBitsGq7 : Bitfield void gnssSolutionFault(bool val) { if(val) value |= GNSS_SOLUTION_FAULT; else value &= ~GNSS_SOLUTION_FAULT; } bool gnssGeneralFault() const { return (value & GNSS_GENERAL_FAULT) > 0; } void gnssGeneralFault(bool val) { if(val) value |= GNSS_GENERAL_FAULT; else value &= ~GNSS_GENERAL_FAULT; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 3b52d260f..32f1eb72f 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -251,6 +251,7 @@ static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIM static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_ANTENNA_OFFSET = 0x0010; ///< static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_AUTO_MAG_HARD_IRON = 0x0020; ///< static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_AUTO_MAG_SOFT_IRON = 0x0040; ///< +static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_ALL = 0x007F; struct mip_filter_estimation_control_command { @@ -395,6 +396,7 @@ static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_O static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_ROLL = 0x1; ///< static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_PITCH = 0x2; ///< static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_YAW = 0x4; ///< +static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_ALL = 0x7; struct mip_filter_tare_orientation_command { @@ -1484,6 +1486,7 @@ static const mip_filter_initialization_configuration_command_alignment_selector static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_DUAL_ANTENNA = 0x01; ///< Dual-antenna GNSS alignment static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_KINEMATIC = 0x02; ///< GNSS kinematic alignment (GNSS velocity determines initial heading) static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_MAGNETOMETER = 0x04; ///< Magnetometer heading alignment +static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_ALL = 0x07; typedef uint8_t mip_filter_initialization_configuration_command_initial_condition_source; static const mip_filter_initialization_configuration_command_initial_condition_source MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_INITIAL_CONDITION_SOURCE_AUTO_POS_VEL_ATT = 0; ///< Automatic position, velocity and attitude diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 28a0ef255..06bebc646 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -277,6 +277,7 @@ struct EstimationControl ANTENNA_OFFSET = 0x0010, ///< AUTO_MAG_HARD_IRON = 0x0020, ///< AUTO_MAG_SOFT_IRON = 0x0040, ///< + ALL = 0x007F, }; uint16_t value = NONE; @@ -302,6 +303,9 @@ struct EstimationControl void autoMagHardIron(bool val) { if(val) value |= AUTO_MAG_HARD_IRON; else value &= ~AUTO_MAG_HARD_IRON; } bool autoMagSoftIron() const { return (value & AUTO_MAG_SOFT_IRON) > 0; } void autoMagSoftIron(bool val) { if(val) value |= AUTO_MAG_SOFT_IRON; else value &= ~AUTO_MAG_SOFT_IRON; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -469,6 +473,7 @@ struct TareOrientation ROLL = 0x1, ///< PITCH = 0x2, ///< YAW = 0x4, ///< + ALL = 0x7, }; uint8_t value = NONE; @@ -486,6 +491,9 @@ struct TareOrientation void pitch(bool val) { if(val) value |= PITCH; else value &= ~PITCH; } bool yaw() const { return (value & YAW) > 0; } void yaw(bool val) { if(val) value |= YAW; else value &= ~YAW; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -1838,6 +1846,7 @@ struct InitializationConfiguration DUAL_ANTENNA = 0x01, ///< Dual-antenna GNSS alignment KINEMATIC = 0x02, ///< GNSS kinematic alignment (GNSS velocity determines initial heading) MAGNETOMETER = 0x04, ///< Magnetometer heading alignment + ALL = 0x07, }; uint8_t value = NONE; @@ -1855,6 +1864,9 @@ struct InitializationConfiguration void kinematic(bool val) { if(val) value |= KINEMATIC; else value &= ~KINEMATIC; } bool magnetometer() const { return (value & MAGNETOMETER) > 0; } void magnetometer(bool val) { if(val) value |= MAGNETOMETER; else value &= ~MAGNETOMETER; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; enum class InitialConditionSource : uint8_t diff --git a/src/mip/definitions/commands_rtk.h b/src/mip/definitions/commands_rtk.h index f68eebc06..31f6cb07d 100644 --- a/src/mip/definitions/commands_rtk.h +++ b/src/mip/definitions/commands_rtk.h @@ -98,6 +98,7 @@ static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_ST static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_RSRP = 0x0C000000; ///< static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_RSRQ = 0x30000000; ///< static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_SINR = 0xC0000000; ///< +static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_ALL = 0xFFFFFFFF; typedef uint32_t mip_rtk_get_status_flags_command_status_flags; static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_NONE = 0x00000000; @@ -113,6 +114,7 @@ static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FL static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_CORRECTIONS_UNAVAILABLE = 0x10000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_RESERVED = 0x20000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_VERSION = 0xC0000000; ///< +static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_ALL = 0xFFFFFFFF; void insert_mip_rtk_get_status_flags_command_status_flags_legacy(struct mip_serializer* serializer, const mip_rtk_get_status_flags_command_status_flags_legacy self); void extract_mip_rtk_get_status_flags_command_status_flags_legacy(struct mip_serializer* serializer, mip_rtk_get_status_flags_command_status_flags_legacy* self); @@ -286,6 +288,7 @@ static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_THROTTLE = 0x01; ///< static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_CORRECTIONS_UNAVAILABLE = 0x02; ///< static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_RESERVED = 0xFC; ///< +static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_ALL = 0xFF; struct mip_rtk_service_status_command { diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 6992b1051..4611979f1 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -105,6 +105,7 @@ struct GetStatusFlags RSRP = 0x0C000000, ///< RSRQ = 0x30000000, ///< SINR = 0xC0000000, ///< + ALL = 0xFFFFFFFF, }; uint32_t value = NONE; @@ -138,6 +139,9 @@ struct GetStatusFlags void rsrq(uint32_t val) { value = (value & ~RSRQ) | (val << 28); } uint32_t sinr() const { return (value & SINR) >> 30; } void sinr(uint32_t val) { value = (value & ~SINR) | (val << 30); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct StatusFlags : Bitfield @@ -157,6 +161,7 @@ struct GetStatusFlags CORRECTIONS_UNAVAILABLE = 0x10000000, ///< RESERVED = 0x20000000, ///< VERSION = 0xC0000000, ///< + ALL = 0xFFFFFFFF, }; uint32_t value = NONE; @@ -192,6 +197,9 @@ struct GetStatusFlags void reserved(bool val) { if(val) value |= RESERVED; else value &= ~RESERVED; } uint32_t version() const { return (value & VERSION) >> 30; } void version(uint32_t val) { value = (value & ~VERSION) | (val << 30); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; @@ -470,6 +478,7 @@ struct ServiceStatus THROTTLE = 0x01, ///< CORRECTIONS_UNAVAILABLE = 0x02, ///< RESERVED = 0xFC, ///< + ALL = 0xFF, }; uint8_t value = NONE; @@ -487,6 +496,9 @@ struct ServiceStatus void correctionsUnavailable(bool val) { if(val) value |= CORRECTIONS_UNAVAILABLE; else value &= ~CORRECTIONS_UNAVAILABLE; } uint8_t reserved() const { return (value & RESERVED) >> 2; } void reserved(uint8_t val) { value = (value & ~RESERVED) | (val << 2); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint32_t reserved1 = 0; diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index af7737875..9dfe6eedd 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -151,6 +151,7 @@ static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_ANTENNA_LEVER_A static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_MOUNTING_TRANSFORM_WARNING = 0x0200; ///< static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_TIME_SYNC_WARNING = 0x0400; ///< No time synchronization pulse detected static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_SOLUTION_ERROR = 0xF000; ///< Filter computation warning flags. If any bits 12-15 are set, and all filter outputs will be invalid. +static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_ALL = 0xFFFF; void insert_mip_filter_status_flags(struct mip_serializer* serializer, const mip_filter_status_flags self); void extract_mip_filter_status_flags(struct mip_serializer* serializer, mip_filter_status_flags* self); @@ -174,6 +175,7 @@ static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_R static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_SAMPLE_TIME_WARNING = 0x08; ///< static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_CONFIGURATION_ERROR = 0x10; ///< static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_MAX_NUM_MEAS_EXCEEDED = 0x20; ///< +static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_ALL = 0x3F; void insert_mip_filter_measurement_indicator(struct mip_serializer* serializer, const mip_filter_measurement_indicator self); void extract_mip_filter_measurement_indicator(struct mip_serializer* serializer, mip_filter_measurement_indicator* self); @@ -196,6 +198,7 @@ static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_BEI_B2 static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_BEI_B3 = 0x2000; ///< If 1, the Kalman filter is using Beidou B3 measurements (not available on the GQ7) static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_NO_FIX = 0x4000; ///< If 1, this GNSS module is reporting no position fix static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_CONFIG_ERROR = 0x8000; ///< If 1, there is likely an issue with the antenna offset for this GNSS module +static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_ALL = 0xFFFF; void insert_mip_gnss_aid_status_flags(struct mip_serializer* serializer, const mip_gnss_aid_status_flags self); void extract_mip_gnss_aid_status_flags(struct mip_serializer* serializer, mip_gnss_aid_status_flags* self); @@ -1343,6 +1346,7 @@ static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_RCV_1_DATA_VALID = 0x0001; ///< static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_RCV_2_DATA_VALID = 0x0002; ///< static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_ANTENNA_OFFSETS_VALID = 0x0004; ///< +static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_ALL = 0x0007; struct mip_filter_gnss_dual_antenna_status_data { diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index c47f70cb0..78bf3fd2b 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -151,6 +151,7 @@ struct FilterStatusFlags : Bitfield GQ7_MOUNTING_TRANSFORM_WARNING = 0x0200, ///< GQ7_TIME_SYNC_WARNING = 0x0400, ///< No time synchronization pulse detected GQ7_SOLUTION_ERROR = 0xF000, ///< Filter computation warning flags. If any bits 12-15 are set, and all filter outputs will be invalid. + ALL = 0xFFFF, }; uint16_t value = NONE; @@ -218,6 +219,9 @@ struct FilterStatusFlags : Bitfield void gq7TimeSyncWarning(bool val) { if(val) value |= GQ7_TIME_SYNC_WARNING; else value &= ~GQ7_TIME_SYNC_WARNING; } uint16_t gq7SolutionError() const { return (value & GQ7_SOLUTION_ERROR) >> 12; } void gq7SolutionError(uint16_t val) { value = (value & ~GQ7_SOLUTION_ERROR) | (val << 12); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; enum class FilterAidingMeasurementType : uint8_t @@ -241,6 +245,7 @@ struct FilterMeasurementIndicator : Bitfield SAMPLE_TIME_WARNING = 0x08, ///< CONFIGURATION_ERROR = 0x10, ///< MAX_NUM_MEAS_EXCEEDED = 0x20, ///< + ALL = 0x3F, }; uint8_t value = NONE; @@ -264,6 +269,9 @@ struct FilterMeasurementIndicator : Bitfield void configurationError(bool val) { if(val) value |= CONFIGURATION_ERROR; else value &= ~CONFIGURATION_ERROR; } bool maxNumMeasExceeded() const { return (value & MAX_NUM_MEAS_EXCEEDED) > 0; } void maxNumMeasExceeded(bool val) { if(val) value |= MAX_NUM_MEAS_EXCEEDED; else value &= ~MAX_NUM_MEAS_EXCEEDED; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct GnssAidStatusFlags : Bitfield @@ -287,6 +295,7 @@ struct GnssAidStatusFlags : Bitfield BEI_B3 = 0x2000, ///< If 1, the Kalman filter is using Beidou B3 measurements (not available on the GQ7) NO_FIX = 0x4000, ///< If 1, this GNSS module is reporting no position fix CONFIG_ERROR = 0x8000, ///< If 1, there is likely an issue with the antenna offset for this GNSS module + ALL = 0xFFFF, }; uint16_t value = NONE; @@ -330,6 +339,9 @@ struct GnssAidStatusFlags : Bitfield void noFix(bool val) { if(val) value |= NO_FIX; else value &= ~NO_FIX; } bool configError() const { return (value & CONFIG_ERROR) > 0; } void configError(bool val) { if(val) value |= CONFIG_ERROR; else value &= ~CONFIG_ERROR; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; @@ -1922,6 +1934,7 @@ struct GnssDualAntennaStatus RCV_1_DATA_VALID = 0x0001, ///< RCV_2_DATA_VALID = 0x0002, ///< ANTENNA_OFFSETS_VALID = 0x0004, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -1939,6 +1952,9 @@ struct GnssDualAntennaStatus void rcv2DataValid(bool val) { if(val) value |= RCV_2_DATA_VALID; else value &= ~RCV_2_DATA_VALID; } bool antennaOffsetsValid() const { return (value & ANTENNA_OFFSETS_VALID) > 0; } void antennaOffsetsValid(bool val) { if(val) value |= ANTENNA_OFFSETS_VALID; else value &= ~ANTENNA_OFFSETS_VALID; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float time_of_week = 0; ///< Last dual-antenna GNSS aiding measurement time of week [seconds] diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index d62f57310..9bab81253 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -185,6 +185,7 @@ static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_HORIZONTAL_ACCURACY = 0x0008; ///< static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_VERTICAL_ACCURACY = 0x0010; ///< static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_FLAGS = 0x001F; ///< +static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_ALL = 0x001F; struct mip_gnss_pos_llh_data { @@ -218,6 +219,7 @@ static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLA static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_POSITION = 0x0001; ///< static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_POSITION_ACCURACY = 0x0002; ///< static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_pos_ecef_data { @@ -251,6 +253,7 @@ static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_SPEED_ACCURACY = 0x0010; ///< static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_HEADING_ACCURACY = 0x0020; ///< static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_vel_ned_data { @@ -284,6 +287,7 @@ static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLA static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_VELOCITY = 0x0001; ///< static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_VELOCITY_ACCURACY = 0x0002; ///< static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_vel_ecef_data { @@ -318,6 +322,7 @@ static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_TDOP = static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_NDOP = 0x0020; ///< static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_EDOP = 0x0040; ///< static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_FLAGS = 0x007F; ///< +static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_ALL = 0x007F; struct mip_gnss_dop_data { @@ -352,6 +357,7 @@ static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLA static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_GNSS_DATE_TIME = 0x0001; ///< static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_LEAP_SECONDS_KNOWN = 0x0002; ///< static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_utc_time_data { @@ -386,6 +392,7 @@ static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLA static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_TOW = 0x0001; ///< static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_WEEK_NUMBER = 0x0002; ///< static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_gps_time_data { @@ -416,6 +423,7 @@ static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_DRIFT = 0x0002; ///< static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_ACCURACY_ESTIMATE = 0x0004; ///< static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_clock_info_data { @@ -454,6 +462,7 @@ typedef uint16_t mip_gnss_fix_info_data_fix_flags; static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_NONE = 0x0000; static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_SBAS_USED = 0x0001; ///< static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_DGNSS_USED = 0x0002; ///< +static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_ALL = 0x0003; typedef uint16_t mip_gnss_fix_info_data_valid_flags; static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -461,6 +470,7 @@ static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLA static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_NUM_SV = 0x0002; ///< static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_FIX_FLAGS = 0x0004; ///< static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_fix_info_data { @@ -498,6 +508,7 @@ typedef uint16_t mip_gnss_sv_info_data_svflags; static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_NONE = 0x0000; static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_USED_FOR_NAVIGATION = 0x0001; ///< static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_HEALTHY = 0x0002; ///< +static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_ALL = 0x0003; typedef uint16_t mip_gnss_sv_info_data_valid_flags; static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -508,6 +519,7 @@ static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_ELEVATION = 0x0010; ///< static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_SV_FLAGS = 0x0020; ///< static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_sv_info_data { @@ -562,6 +574,7 @@ static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_F static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_ANTENNA_STATE = 0x0002; ///< static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_ANTENNA_POWER = 0x0004; ///< static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_hw_status_data { @@ -615,6 +628,7 @@ static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_F static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_BASE_STATION_STATUS = 0x0004; ///< static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_NUM_CHANNELS = 0x0008; ///< static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_dgps_info_data { @@ -650,6 +664,7 @@ static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_V static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_RANGE_CORRECTION = 0x0004; ///< static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_RANGE_RATE_CORRECTION = 0x0008; ///< static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_dgps_channel_data { @@ -685,6 +700,7 @@ static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_V static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_BIAS_ACCURACY = 0x0004; ///< static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_DRIFT_ACCURACY = 0x0008; ///< static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_clock_info_2_data { @@ -714,6 +730,7 @@ void extract_mip_gnss_clock_info_2_data_valid_flags(struct mip_serializer* seria typedef uint16_t mip_gnss_gps_leap_seconds_data_valid_flags; static const mip_gnss_gps_leap_seconds_data_valid_flags MIP_GNSS_GPS_LEAP_SECONDS_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_gnss_gps_leap_seconds_data_valid_flags MIP_GNSS_GPS_LEAP_SECONDS_DATA_VALID_FLAGS_LEAP_SECONDS = 0x0002; ///< +static const mip_gnss_gps_leap_seconds_data_valid_flags MIP_GNSS_GPS_LEAP_SECONDS_DATA_VALID_FLAGS_ALL = 0x0002; struct mip_gnss_gps_leap_seconds_data { @@ -743,6 +760,7 @@ static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_ST static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_CORRECTIONS_AVAILABLE = 0x02; ///< static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_INTEGRITY_AVAILABLE = 0x04; ///< static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_TEST_MODE = 0x08; ///< +static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_ALL = 0x0F; typedef uint16_t mip_gnss_sbas_info_data_valid_flags; static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -753,6 +771,7 @@ static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_F static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_COUNT = 0x0010; ///< static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_SBAS_STATUS = 0x0020; ///< static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_sbas_info_data { @@ -812,6 +831,7 @@ static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_ static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_PSEUDORANGE_CORRECTION = 0x0002; ///< static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_IONO_CORRECTION = 0x0004; ///< static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_sbas_correction_data { @@ -867,6 +887,7 @@ static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETE static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_JAMMING_STATE = 0x0002; ///< static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_SPOOFING_STATE = 0x0004; ///< static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_rf_error_detection_data { @@ -915,6 +936,7 @@ static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATI static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_QUARTER_CYCLE_BIT1 = 0x0040; ///< static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_QUARTER_CYCLE_BIT2 = 0x0080; ///< static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_QUARTER_CYCLE_BITS = 0x00C0; ///< +static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_ALL = 0x00FF; typedef uint16_t mip_gnss_base_station_info_data_valid_flags; static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -925,6 +947,7 @@ static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_I static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_STATION_ID = 0x0010; ///< static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_INDICATORS = 0x0020; ///< static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_base_station_info_data { @@ -966,6 +989,7 @@ static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRE static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_GALILEO_LATENCY = 0x0040; ///< static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_BEIDOU_LATENCY = 0x0080; ///< static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_FLAGS = 0x00FF; ///< +static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_ALL = 0x00FF; typedef uint16_t mip_gnss_rtk_corrections_status_data_epoch_status; static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_NONE = 0x0000; @@ -978,6 +1002,7 @@ static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORR static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_USING_GPS_MSM_MESSAGES = 0x0040; ///< Using MSM messages for GPS corrections instead of RTCM messages 1001-1004 static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_USING_GLONASS_MSM_MESSAGES = 0x0080; ///< Using MSM messages for GLONASS corrections instead of RTCM messages 1009-1012 static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_DONGLE_STATUS_READ_FAILED = 0x0100; ///< A read of the dongle status was attempted, but failed +static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_ALL = 0x01FF; struct mip_gnss_rtk_corrections_status_data { @@ -1022,6 +1047,7 @@ static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATU static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_AZIMUTH = 0x0020; ///< static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_HEALTH = 0x0040; ///< static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_FLAGS = 0x007F; ///< +static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_ALL = 0x007F; struct mip_gnss_satellite_status_data { @@ -1080,6 +1106,7 @@ static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_CARRIER static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_DOPPLER_UNCERTAINTY = 0x4000; ///< static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_LOCK_TIME = 0x8000; ///< static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_FLAGS = 0xFFFF; ///< +static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_ALL = 0xFFFF; struct mip_gnss_raw_data { @@ -1128,6 +1155,7 @@ static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_EPHEMERIS = 0x0001; ///< static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_MODERN_DATA = 0x0002; ///< static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_gps_ephemeris_data { @@ -1188,6 +1216,7 @@ static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEME static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_EPHEMERIS = 0x0001; ///< static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_MODERN_DATA = 0x0002; ///< static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_galileo_ephemeris_data { @@ -1247,6 +1276,7 @@ typedef uint16_t mip_gnss_glo_ephemeris_data_valid_flags; static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_EPHEMERIS = 0x0001; ///< static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_FLAGS = 0x0001; ///< +static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_ALL = 0x0001; struct mip_gnss_glo_ephemeris_data { @@ -1300,6 +1330,7 @@ static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_ALPHA = 0x0004; ///< static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_BETA = 0x0008; ///< static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_gps_iono_corr_data { @@ -1333,6 +1364,7 @@ static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_C static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_ALPHA = 0x0004; ///< static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_DISTURBANCE_FLAGS = 0x0008; ///< static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_galileo_iono_corr_data { diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 8dbff4186..cf38aef40 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -196,6 +196,7 @@ struct PosLlh HORIZONTAL_ACCURACY = 0x0008, ///< VERTICAL_ACCURACY = 0x0010, ///< FLAGS = 0x001F, ///< + ALL = 0x001F, }; uint16_t value = NONE; @@ -219,6 +220,9 @@ struct PosLlh void verticalAccuracy(bool val) { if(val) value |= VERTICAL_ACCURACY; else value &= ~VERTICAL_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double latitude = 0; ///< [degrees] @@ -261,6 +265,7 @@ struct PosEcef POSITION = 0x0001, ///< POSITION_ACCURACY = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -278,6 +283,9 @@ struct PosEcef void positionAccuracy(bool val) { if(val) value |= POSITION_ACCURACY; else value &= ~POSITION_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double x[3] = {0}; ///< [meters] @@ -320,6 +328,7 @@ struct VelNed SPEED_ACCURACY = 0x0010, ///< HEADING_ACCURACY = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -345,6 +354,9 @@ struct VelNed void headingAccuracy(bool val) { if(val) value |= HEADING_ACCURACY; else value &= ~HEADING_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float v[3] = {0}; ///< [meters/second] @@ -387,6 +399,7 @@ struct VelEcef VELOCITY = 0x0001, ///< VELOCITY_ACCURACY = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -404,6 +417,9 @@ struct VelEcef void velocityAccuracy(bool val) { if(val) value |= VELOCITY_ACCURACY; else value &= ~VELOCITY_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float v[3] = {0}; ///< [meters/second] @@ -447,6 +463,7 @@ struct Dop NDOP = 0x0020, ///< EDOP = 0x0040, ///< FLAGS = 0x007F, ///< + ALL = 0x007F, }; uint16_t value = NONE; @@ -474,6 +491,9 @@ struct Dop void edop(bool val) { if(val) value |= EDOP; else value &= ~EDOP; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float gdop = 0; ///< Geometric DOP @@ -517,6 +537,7 @@ struct UtcTime GNSS_DATE_TIME = 0x0001, ///< LEAP_SECONDS_KNOWN = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -534,6 +555,9 @@ struct UtcTime void leapSecondsKnown(bool val) { if(val) value |= LEAP_SECONDS_KNOWN; else value &= ~LEAP_SECONDS_KNOWN; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint16_t year = 0; @@ -577,6 +601,7 @@ struct GpsTime TOW = 0x0001, ///< WEEK_NUMBER = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -594,6 +619,9 @@ struct GpsTime void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double tow = 0; ///< GPS Time of week [seconds] @@ -633,6 +661,7 @@ struct ClockInfo DRIFT = 0x0002, ///< ACCURACY_ESTIMATE = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -652,6 +681,9 @@ struct ClockInfo void accuracyEstimate(bool val) { if(val) value |= ACCURACY_ESTIMATE; else value &= ~ACCURACY_ESTIMATE; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double bias = 0; ///< [seconds] @@ -701,6 +733,7 @@ struct FixInfo NONE = 0x0000, SBAS_USED = 0x0001, ///< DGNSS_USED = 0x0002, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -716,6 +749,9 @@ struct FixInfo void sbasUsed(bool val) { if(val) value |= SBAS_USED; else value &= ~SBAS_USED; } bool dgnssUsed() const { return (value & DGNSS_USED) > 0; } void dgnssUsed(bool val) { if(val) value |= DGNSS_USED; else value &= ~DGNSS_USED; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -727,6 +763,7 @@ struct FixInfo NUM_SV = 0x0002, ///< FIX_FLAGS = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -746,6 +783,9 @@ struct FixInfo void fixFlags(bool val) { if(val) value |= FIX_FLAGS; else value &= ~FIX_FLAGS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FixType fix_type = static_cast(0); @@ -786,6 +826,7 @@ struct SvInfo NONE = 0x0000, USED_FOR_NAVIGATION = 0x0001, ///< HEALTHY = 0x0002, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -801,6 +842,9 @@ struct SvInfo void usedForNavigation(bool val) { if(val) value |= USED_FOR_NAVIGATION; else value &= ~USED_FOR_NAVIGATION; } bool healthy() const { return (value & HEALTHY) > 0; } void healthy(bool val) { if(val) value |= HEALTHY; else value &= ~HEALTHY; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -815,6 +859,7 @@ struct SvInfo ELEVATION = 0x0010, ///< SV_FLAGS = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -840,6 +885,9 @@ struct SvInfo void svFlags(bool val) { if(val) value |= SV_FLAGS; else value &= ~SV_FLAGS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t channel = 0; ///< Receiver channel number @@ -906,6 +954,7 @@ struct HwStatus ANTENNA_STATE = 0x0002, ///< ANTENNA_POWER = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -925,6 +974,9 @@ struct HwStatus void antennaPower(bool val) { if(val) value |= ANTENNA_POWER; else value &= ~ANTENNA_POWER; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; ReceiverState receiver_state = static_cast(0); @@ -978,6 +1030,7 @@ struct DgpsInfo BASE_STATION_STATUS = 0x0004, ///< NUM_CHANNELS = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -999,6 +1052,9 @@ struct DgpsInfo void numChannels(bool val) { if(val) value |= NUM_CHANNELS; else value &= ~NUM_CHANNELS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t sv_id = 0; @@ -1043,6 +1099,7 @@ struct DgpsChannel RANGE_CORRECTION = 0x0004, ///< RANGE_RATE_CORRECTION = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -1064,6 +1121,9 @@ struct DgpsChannel void rangeRateCorrection(bool val) { if(val) value |= RANGE_RATE_CORRECTION; else value &= ~RANGE_RATE_CORRECTION; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t sv_id = 0; @@ -1108,6 +1168,7 @@ struct ClockInfo2 BIAS_ACCURACY = 0x0004, ///< DRIFT_ACCURACY = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -1129,6 +1190,9 @@ struct ClockInfo2 void driftAccuracy(bool val) { if(val) value |= DRIFT_ACCURACY; else value &= ~DRIFT_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double bias = 0; @@ -1167,6 +1231,7 @@ struct GpsLeapSeconds { NONE = 0x0000, LEAP_SECONDS = 0x0002, ///< + ALL = 0x0002, }; uint16_t value = NONE; @@ -1180,6 +1245,9 @@ struct GpsLeapSeconds bool leapSeconds() const { return (value & LEAP_SECONDS) > 0; } void leapSeconds(bool val) { if(val) value |= LEAP_SECONDS; else value &= ~LEAP_SECONDS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t leap_seconds = 0; ///< [s] @@ -1218,6 +1286,7 @@ struct SbasInfo CORRECTIONS_AVAILABLE = 0x02, ///< INTEGRITY_AVAILABLE = 0x04, ///< TEST_MODE = 0x08, ///< + ALL = 0x0F, }; uint8_t value = NONE; @@ -1237,6 +1306,9 @@ struct SbasInfo void integrityAvailable(bool val) { if(val) value |= INTEGRITY_AVAILABLE; else value &= ~INTEGRITY_AVAILABLE; } bool testMode() const { return (value & TEST_MODE) > 0; } void testMode(bool val) { if(val) value |= TEST_MODE; else value &= ~TEST_MODE; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -1251,6 +1323,7 @@ struct SbasInfo COUNT = 0x0010, ///< SBAS_STATUS = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -1276,6 +1349,9 @@ struct SbasInfo void sbasStatus(bool val) { if(val) value |= SBAS_STATUS; else value &= ~SBAS_STATUS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -1341,6 +1417,7 @@ struct SbasCorrection PSEUDORANGE_CORRECTION = 0x0002, ///< IONO_CORRECTION = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -1360,6 +1437,9 @@ struct SbasCorrection void ionoCorrection(bool val) { if(val) value |= IONO_CORRECTION; else value &= ~IONO_CORRECTION; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1430,6 +1510,7 @@ struct RfErrorDetection JAMMING_STATE = 0x0002, ///< SPOOFING_STATE = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -1449,6 +1530,9 @@ struct RfErrorDetection void spoofingState(bool val) { if(val) value |= SPOOFING_STATE; else value &= ~SPOOFING_STATE; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; RFBand rf_band = static_cast(0); ///< RF Band of the reported information @@ -1497,6 +1581,7 @@ struct BaseStationInfo QUARTER_CYCLE_BIT1 = 0x0040, ///< QUARTER_CYCLE_BIT2 = 0x0080, ///< QUARTER_CYCLE_BITS = 0x00C0, ///< + ALL = 0x00FF, }; uint16_t value = NONE; @@ -1526,6 +1611,9 @@ struct BaseStationInfo void quarterCycleBit2(bool val) { if(val) value |= QUARTER_CYCLE_BIT2; else value &= ~QUARTER_CYCLE_BIT2; } uint16_t quarterCycleBits() const { return (value & QUARTER_CYCLE_BITS) >> 6; } void quarterCycleBits(uint16_t val) { value = (value & ~QUARTER_CYCLE_BITS) | (val << 6); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -1540,6 +1628,7 @@ struct BaseStationInfo STATION_ID = 0x0010, ///< INDICATORS = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -1565,6 +1654,9 @@ struct BaseStationInfo void indicators(bool val) { if(val) value |= INDICATORS; else value &= ~INDICATORS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week the message was received [seconds] @@ -1612,6 +1704,7 @@ struct RtkCorrectionsStatus GALILEO_LATENCY = 0x0040, ///< BEIDOU_LATENCY = 0x0080, ///< FLAGS = 0x00FF, ///< + ALL = 0x00FF, }; uint16_t value = NONE; @@ -1641,6 +1734,9 @@ struct RtkCorrectionsStatus void beidouLatency(bool val) { if(val) value |= BEIDOU_LATENCY; else value &= ~BEIDOU_LATENCY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct EpochStatus : Bitfield @@ -1657,6 +1753,7 @@ struct RtkCorrectionsStatus USING_GPS_MSM_MESSAGES = 0x0040, ///< Using MSM messages for GPS corrections instead of RTCM messages 1001-1004 USING_GLONASS_MSM_MESSAGES = 0x0080, ///< Using MSM messages for GLONASS corrections instead of RTCM messages 1009-1012 DONGLE_STATUS_READ_FAILED = 0x0100, ///< A read of the dongle status was attempted, but failed + ALL = 0x01FF, }; uint16_t value = NONE; @@ -1686,6 +1783,9 @@ struct RtkCorrectionsStatus void usingGlonassMsmMessages(bool val) { if(val) value |= USING_GLONASS_MSM_MESSAGES; else value &= ~USING_GLONASS_MSM_MESSAGES; } bool dongleStatusReadFailed() const { return (value & DONGLE_STATUS_READ_FAILED) > 0; } void dongleStatusReadFailed(bool val) { if(val) value |= DONGLE_STATUS_READ_FAILED; else value &= ~DONGLE_STATUS_READ_FAILED; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -1736,6 +1836,7 @@ struct SatelliteStatus AZIMUTH = 0x0020, ///< HEALTH = 0x0040, ///< FLAGS = 0x007F, ///< + ALL = 0x007F, }; uint16_t value = NONE; @@ -1763,6 +1864,9 @@ struct SatelliteStatus void health(bool val) { if(val) value |= HEALTH; else value &= ~HEALTH; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1832,6 +1936,7 @@ struct Raw DOPPLER_UNCERTAINTY = 0x4000, ///< LOCK_TIME = 0x8000, ///< FLAGS = 0xFFFF, ///< + ALL = 0xFFFF, }; uint16_t value = NONE; @@ -1877,6 +1982,9 @@ struct Raw void lockTime(bool val) { if(val) value |= LOCK_TIME; else value &= ~LOCK_TIME; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1931,6 +2039,7 @@ struct GpsEphemeris EPHEMERIS = 0x0001, ///< MODERN_DATA = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -1948,6 +2057,9 @@ struct GpsEphemeris void modernData(bool val) { if(val) value |= MODERN_DATA; else value &= ~MODERN_DATA; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -2017,6 +2129,7 @@ struct GalileoEphemeris EPHEMERIS = 0x0001, ///< MODERN_DATA = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -2034,6 +2147,9 @@ struct GalileoEphemeris void modernData(bool val) { if(val) value |= MODERN_DATA; else value &= ~MODERN_DATA; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -2102,6 +2218,7 @@ struct GloEphemeris NONE = 0x0000, EPHEMERIS = 0x0001, ///< FLAGS = 0x0001, ///< + ALL = 0x0001, }; uint16_t value = NONE; @@ -2117,6 +2234,9 @@ struct GloEphemeris void ephemeris(bool val) { if(val) value |= EPHEMERIS; else value &= ~EPHEMERIS; } bool flags() const { return (value & FLAGS) > 0; } void flags(bool val) { if(val) value |= FLAGS; else value &= ~FLAGS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -2179,6 +2299,7 @@ struct GpsIonoCorr ALPHA = 0x0004, ///< BETA = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -2200,6 +2321,9 @@ struct GpsIonoCorr void beta(bool val) { if(val) value |= BETA; else value &= ~BETA; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -2242,6 +2366,7 @@ struct GalileoIonoCorr ALPHA = 0x0004, ///< DISTURBANCE_FLAGS = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -2263,6 +2388,9 @@ struct GalileoIonoCorr void disturbanceFlags(bool val) { if(val) value |= DISTURBANCE_FLAGS; else value &= ~DISTURBANCE_FLAGS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] diff --git a/src/mip/definitions/data_sensor.h b/src/mip/definitions/data_sensor.h index ae2cee70a..40267b92a 100644 --- a/src/mip/definitions/data_sensor.h +++ b/src/mip/definitions/data_sensor.h @@ -426,6 +426,7 @@ static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_ static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_TIME_INITIALIZED = 0x0004; ///< True if the time has ever been set. static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_TOW_VALID = 0x0008; ///< True if the time of week is valid. static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_WEEK_NUMBER_VALID = 0x0010; ///< True if the week number is valid. +static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_ALL = 0x001F; struct mip_sensor_gps_timestamp_data { @@ -530,6 +531,7 @@ static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_MAG_Y = 0x0200; ///< static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_MAG_Z = 0x0400; ///< static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_PRESS = 0x1000; ///< +static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_ALL = 0x1777; struct mip_sensor_overrange_status_data { diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index b5868dc2e..7ed980dd1 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -576,6 +576,7 @@ struct GpsTimestamp TIME_INITIALIZED = 0x0004, ///< True if the time has ever been set. TOW_VALID = 0x0008, ///< True if the time of week is valid. WEEK_NUMBER_VALID = 0x0010, ///< True if the week number is valid. + ALL = 0x001F, }; uint16_t value = NONE; @@ -597,6 +598,9 @@ struct GpsTimestamp void towValid(bool val) { if(val) value |= TOW_VALID; else value &= ~TOW_VALID; } bool weekNumberValid() const { return (value & WEEK_NUMBER_VALID) > 0; } void weekNumberValid(bool val) { if(val) value |= WEEK_NUMBER_VALID; else value &= ~WEEK_NUMBER_VALID; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double tow = 0; ///< GPS Time of Week [seconds] @@ -734,6 +738,7 @@ struct OverrangeStatus MAG_Y = 0x0200, ///< MAG_Z = 0x0400, ///< PRESS = 0x1000, ///< + ALL = 0x1777, }; uint16_t value = NONE; @@ -765,6 +770,9 @@ struct OverrangeStatus void magZ(bool val) { if(val) value |= MAG_Z; else value &= ~MAG_Z; } bool press() const { return (value & PRESS) > 0; } void press(bool val) { if(val) value |= PRESS; else value &= ~PRESS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; Status status; diff --git a/src/mip/definitions/data_shared.h b/src/mip/definitions/data_shared.h index 8f7c4ead6..90f2dbc6e 100644 --- a/src/mip/definitions/data_shared.h +++ b/src/mip/definitions/data_shared.h @@ -133,6 +133,7 @@ static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_ static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_TOW = 0x0001; ///< Whole number seconds TOW has been set static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_WEEK_NUMBER = 0x0002; ///< Week number has been set static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_TIME_VALID = 0x0003; ///< Both TOW and Week Number have been set +static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_shared_gps_timestamp_data { @@ -247,6 +248,7 @@ bool extract_mip_shared_reference_time_delta_data_from_field(const struct mip_fi typedef uint16_t mip_shared_external_timestamp_data_valid_flags; static const mip_shared_external_timestamp_data_valid_flags MIP_SHARED_EXTERNAL_TIMESTAMP_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_shared_external_timestamp_data_valid_flags MIP_SHARED_EXTERNAL_TIMESTAMP_DATA_VALID_FLAGS_NANOSECONDS = 0x0001; ///< +static const mip_shared_external_timestamp_data_valid_flags MIP_SHARED_EXTERNAL_TIMESTAMP_DATA_VALID_FLAGS_ALL = 0x0001; struct mip_shared_external_timestamp_data { @@ -285,6 +287,7 @@ void extract_mip_shared_external_timestamp_data_valid_flags(struct mip_serialize typedef uint16_t mip_shared_external_time_delta_data_valid_flags; static const mip_shared_external_time_delta_data_valid_flags MIP_SHARED_EXTERNAL_TIME_DELTA_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_shared_external_time_delta_data_valid_flags MIP_SHARED_EXTERNAL_TIME_DELTA_DATA_VALID_FLAGS_DT_NANOS = 0x0001; ///< +static const mip_shared_external_time_delta_data_valid_flags MIP_SHARED_EXTERNAL_TIME_DELTA_DATA_VALID_FLAGS_ALL = 0x0001; struct mip_shared_external_time_delta_data { diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 7963b132f..fef0ff682 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -171,6 +171,7 @@ struct GpsTimestamp TOW = 0x0001, ///< Whole number seconds TOW has been set WEEK_NUMBER = 0x0002, ///< Week number has been set TIME_VALID = 0x0003, ///< Both TOW and Week Number have been set + ALL = 0x0003, }; uint16_t value = NONE; @@ -188,6 +189,9 @@ struct GpsTimestamp void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } uint16_t timeValid() const { return (value & TIME_VALID) >> 0; } void timeValid(uint16_t val) { value = (value & ~TIME_VALID) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double tow = 0; ///< GPS Time of Week [seconds] @@ -335,6 +339,7 @@ struct ExternalTimestamp { NONE = 0x0000, NANOSECONDS = 0x0001, ///< + ALL = 0x0001, }; uint16_t value = NONE; @@ -348,6 +353,9 @@ struct ExternalTimestamp bool nanoseconds() const { return (value & NANOSECONDS) > 0; } void nanoseconds(bool val) { if(val) value |= NANOSECONDS; else value &= ~NANOSECONDS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint64_t nanoseconds = 0; @@ -395,6 +403,7 @@ struct ExternalTimeDelta { NONE = 0x0000, DT_NANOS = 0x0001, ///< + ALL = 0x0001, }; uint16_t value = NONE; @@ -408,6 +417,9 @@ struct ExternalTimeDelta bool dtNanos() const { return (value & DT_NANOS) > 0; } void dtNanos(bool val) { if(val) value |= DT_NANOS; else value &= ~DT_NANOS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. From a136d15ae703acbb7125fc509c9bcf67d4f18751 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Mon, 13 Feb 2023 16:04:40 -0500 Subject: [PATCH 032/252] Removes the MIP version from the all header (#51) --- src/mip/mip_all.h | 3 --- src/mip/mip_all.hpp | 3 --- 2 files changed, 6 deletions(-) diff --git a/src/mip/mip_all.h b/src/mip/mip_all.h index f6274a795..f07cf6852 100644 --- a/src/mip/mip_all.h +++ b/src/mip/mip_all.h @@ -27,6 +27,3 @@ #include "definitions/data_sensor.h" #include "definitions/data_gnss.h" #include "definitions/data_filter.h" - -//SDK version -#include "mip_version.h" \ No newline at end of file diff --git a/src/mip/mip_all.hpp b/src/mip/mip_all.hpp index 90b7a6885..f24221576 100644 --- a/src/mip/mip_all.hpp +++ b/src/mip/mip_all.hpp @@ -31,6 +31,3 @@ //MIP Helpers #include "mip.hpp" #include "mip_device.hpp" - -//SDK version -#include "mip_version.h" From 2d9afa437b723df873d4bbb6ccadd491ecf4fd3d Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 16 Feb 2023 14:04:33 -0500 Subject: [PATCH 033/252] Generate MIP definitions from branches/dev@54435. --- src/mip/definitions/commands_gnss.c | 213 -------------------------- src/mip/definitions/commands_gnss.cpp | 202 ------------------------ src/mip/definitions/commands_gnss.h | 52 ------- src/mip/definitions/commands_gnss.hpp | 62 -------- 4 files changed, 529 deletions(-) diff --git a/src/mip/definitions/commands_gnss.c b/src/mip/definitions/commands_gnss.c index a0e0672a6..cb41c03a3 100644 --- a/src/mip/definitions/commands_gnss.c +++ b/src/mip/definitions/commands_gnss.c @@ -239,219 +239,6 @@ mip_cmd_result mip_gnss_default_signal_configuration(struct mip_interface* devic return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_gnss_mip_gnss_spartn_configuration_command(mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_command* self) -{ - insert_mip_function_selector(serializer, self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - insert_u8(serializer, self->enable); - - insert_u8(serializer, self->type); - - insert_u32(serializer, self->current_key_tow); - - insert_u16(serializer, self->current_key_week); - - for(unsigned int i=0; i < 32; i++) - insert_u8(serializer, self->current_key[i]); - - insert_u32(serializer, self->next_key_tow); - - insert_u16(serializer, self->next_key_week); - - for(unsigned int i=0; i < 32; i++) - insert_u8(serializer, self->next_key[i]); - - } -} -void extract_mip_gnss_mip_gnss_spartn_configuration_command(mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_command* self) -{ - extract_mip_function_selector(serializer, &self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - extract_u8(serializer, &self->enable); - - extract_u8(serializer, &self->type); - - extract_u32(serializer, &self->current_key_tow); - - extract_u16(serializer, &self->current_key_week); - - for(unsigned int i=0; i < 32; i++) - extract_u8(serializer, &self->current_key[i]); - - extract_u32(serializer, &self->next_key_tow); - - extract_u16(serializer, &self->next_key_week); - - for(unsigned int i=0; i < 32; i++) - extract_u8(serializer, &self->next_key[i]); - - } -} - -void insert_mip_gnss_mip_gnss_spartn_configuration_response(mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_response* self) -{ - insert_u8(serializer, self->enable); - - insert_u8(serializer, self->type); - - insert_u32(serializer, self->current_key_tow); - - insert_u16(serializer, self->current_key_week); - - for(unsigned int i=0; i < 32; i++) - insert_u8(serializer, self->current_key[i]); - - insert_u32(serializer, self->next_key_tow); - - insert_u16(serializer, self->next_key_week); - - for(unsigned int i=0; i < 32; i++) - insert_u8(serializer, self->next_key[i]); - -} -void extract_mip_gnss_mip_gnss_spartn_configuration_response(mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_response* self) -{ - extract_u8(serializer, &self->enable); - - extract_u8(serializer, &self->type); - - extract_u32(serializer, &self->current_key_tow); - - extract_u16(serializer, &self->current_key_week); - - for(unsigned int i=0; i < 32; i++) - extract_u8(serializer, &self->current_key[i]); - - extract_u32(serializer, &self->next_key_tow); - - extract_u16(serializer, &self->next_key_week); - - for(unsigned int i=0; i < 32; i++) - extract_u8(serializer, &self->next_key[i]); - -} - -mip_cmd_result mip_gnss_write_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t enable, uint8_t type, uint32_t current_key_tow, uint16_t current_key_week, const uint8_t* current_key, uint32_t next_key_tow, uint16_t next_key_week, const uint8_t* next_key) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - - insert_u8(&serializer, enable); - - insert_u8(&serializer, type); - - insert_u32(&serializer, current_key_tow); - - insert_u16(&serializer, current_key_week); - - assert(current_key || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - insert_u8(&serializer, current_key[i]); - - insert_u32(&serializer, next_key_tow); - - insert_u16(&serializer, next_key_week); - - assert(next_key || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - insert_u8(&serializer, next_key[i]); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_gnss_read_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t* enable_out, uint8_t* type_out, uint32_t* current_key_tow_out, uint16_t* current_key_week_out, uint8_t* current_key_out, uint32_t* next_key_tow_out, uint16_t* next_key_week_out, uint8_t* next_key_out) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - - assert(mip_serializer_is_ok(&serializer)); - - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_GNSS_SPARTN_CONFIGURATION, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(enable_out); - extract_u8(&deserializer, enable_out); - - assert(type_out); - extract_u8(&deserializer, type_out); - - assert(current_key_tow_out); - extract_u32(&deserializer, current_key_tow_out); - - assert(current_key_week_out); - extract_u16(&deserializer, current_key_week_out); - - assert(current_key_out || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - extract_u8(&deserializer, ¤t_key_out[i]); - - assert(next_key_tow_out); - extract_u32(&deserializer, next_key_tow_out); - - assert(next_key_week_out); - extract_u16(&deserializer, next_key_week_out); - - assert(next_key_out || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - extract_u8(&deserializer, &next_key_out[i]); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -mip_cmd_result mip_gnss_save_mip_gnss_spartn_configuration(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_gnss_load_mip_gnss_spartn_configuration(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_gnss_default_mip_gnss_spartn_configuration(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_GNSS_CMD_DESC_SET, MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert_mip_gnss_rtk_dongle_configuration_command(mip_serializer* serializer, const mip_gnss_rtk_dongle_configuration_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_gnss.cpp b/src/mip/definitions/commands_gnss.cpp index 0cb2da4df..00bb9e59c 100644 --- a/src/mip/definitions/commands_gnss.cpp +++ b/src/mip/definitions/commands_gnss.cpp @@ -254,208 +254,6 @@ CmdResult defaultSignalConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const MipGnssSpartnConfiguration& self) -{ - insert(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - insert(serializer, self.enable); - - insert(serializer, self.type); - - insert(serializer, self.current_key_tow); - - insert(serializer, self.current_key_week); - - for(unsigned int i=0; i < 32; i++) - insert(serializer, self.current_key[i]); - - insert(serializer, self.next_key_tow); - - insert(serializer, self.next_key_week); - - for(unsigned int i=0; i < 32; i++) - insert(serializer, self.next_key[i]); - - } -} -void extract(Serializer& serializer, MipGnssSpartnConfiguration& self) -{ - extract(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - extract(serializer, self.enable); - - extract(serializer, self.type); - - extract(serializer, self.current_key_tow); - - extract(serializer, self.current_key_week); - - for(unsigned int i=0; i < 32; i++) - extract(serializer, self.current_key[i]); - - extract(serializer, self.next_key_tow); - - extract(serializer, self.next_key_week); - - for(unsigned int i=0; i < 32; i++) - extract(serializer, self.next_key[i]); - - } -} - -void insert(Serializer& serializer, const MipGnssSpartnConfiguration::Response& self) -{ - insert(serializer, self.enable); - - insert(serializer, self.type); - - insert(serializer, self.current_key_tow); - - insert(serializer, self.current_key_week); - - for(unsigned int i=0; i < 32; i++) - insert(serializer, self.current_key[i]); - - insert(serializer, self.next_key_tow); - - insert(serializer, self.next_key_week); - - for(unsigned int i=0; i < 32; i++) - insert(serializer, self.next_key[i]); - -} -void extract(Serializer& serializer, MipGnssSpartnConfiguration::Response& self) -{ - extract(serializer, self.enable); - - extract(serializer, self.type); - - extract(serializer, self.current_key_tow); - - extract(serializer, self.current_key_week); - - for(unsigned int i=0; i < 32; i++) - extract(serializer, self.current_key[i]); - - extract(serializer, self.next_key_tow); - - extract(serializer, self.next_key_week); - - for(unsigned int i=0; i < 32; i++) - extract(serializer, self.next_key[i]); - -} - -CmdResult writeMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t enable, uint8_t type, uint32_t currentKeyTow, uint16_t currentKeyWeek, const uint8_t* currentKey, uint32_t nextKeyTow, uint16_t nextKeyWeek, const uint8_t* nextKey) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::WRITE); - insert(serializer, enable); - - insert(serializer, type); - - insert(serializer, currentKeyTow); - - insert(serializer, currentKeyWeek); - - assert(currentKey || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - insert(serializer, currentKey[i]); - - insert(serializer, nextKeyTow); - - insert(serializer, nextKeyWeek); - - assert(nextKey || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - insert(serializer, nextKey[i]); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult readMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* typeOut, uint32_t* currentKeyTowOut, uint16_t* currentKeyWeekOut, uint8_t* currentKeyOut, uint32_t* nextKeyTowOut, uint16_t* nextKeyWeekOut, uint8_t* nextKeyOut) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::READ); - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPARTN_CONFIGURATION, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(enableOut); - extract(deserializer, *enableOut); - - assert(typeOut); - extract(deserializer, *typeOut); - - assert(currentKeyTowOut); - extract(deserializer, *currentKeyTowOut); - - assert(currentKeyWeekOut); - extract(deserializer, *currentKeyWeekOut); - - assert(currentKeyOut || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - extract(deserializer, currentKeyOut[i]); - - assert(nextKeyTowOut); - extract(deserializer, *nextKeyTowOut); - - assert(nextKeyWeekOut); - extract(deserializer, *nextKeyWeekOut); - - assert(nextKeyOut || (32 == 0)); - for(unsigned int i=0; i < 32; i++) - extract(deserializer, nextKeyOut[i]); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -CmdResult saveMipGnssSpartnConfiguration(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::SAVE); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult loadMipGnssSpartnConfiguration(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::LOAD); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult defaultMipGnssSpartnConfiguration(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::RESET); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPARTN_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert(Serializer& serializer, const RtkDongleConfiguration& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_gnss.h b/src/mip/definitions/commands_gnss.h index 0de8cfd6a..28959ca40 100644 --- a/src/mip/definitions/commands_gnss.h +++ b/src/mip/definitions/commands_gnss.h @@ -35,12 +35,10 @@ enum MIP_CMD_DESC_GNSS_LIST_RECEIVERS = 0x01, MIP_CMD_DESC_GNSS_SIGNAL_CONFIGURATION = 0x02, MIP_CMD_DESC_GNSS_RTK_DONGLE_CONFIGURATION = 0x10, - MIP_CMD_DESC_GNSS_SPARTN_CONFIGURATION = 0x20, MIP_REPLY_DESC_GNSS_LIST_RECEIVERS = 0x81, MIP_REPLY_DESC_GNSS_SIGNAL_CONFIGURATION = 0x82, MIP_REPLY_DESC_GNSS_RTK_DONGLE_CONFIGURATION = 0x90, - MIP_REPLY_DESC_GNSS_SPARTN_CONFIGURATION = 0xA0, }; //////////////////////////////////////////////////////////////////////////////// @@ -133,56 +131,6 @@ mip_cmd_result mip_gnss_default_signal_configuration(struct mip_interface* devic ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_gnss_mip_gnss_spartn_configuration (0x0E,0x20) Mip Gnss Spartn Configuration [C] -/// Configure the SPARTN corrections service parameters. -/// Notes:
-/// - Enable and type settings will only update after a power cycle
-/// - Type settings will only take effect after a power cycle
-/// - Key information can be updated while running -/// -///@{ - -struct mip_gnss_mip_gnss_spartn_configuration_command -{ - mip_function_selector function; - uint8_t enable; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) - uint32_t current_key_tow; ///< The GPS time of week the current key is valid until - uint16_t current_key_week; ///< The GPS week number the current key is valid until - uint8_t current_key[32]; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) - uint32_t next_key_tow; ///< The GPS time of week the next key is valid until - uint16_t next_key_week; ///< The GPS week number the next key is valid until - uint8_t next_key[32]; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) - -}; -typedef struct mip_gnss_mip_gnss_spartn_configuration_command mip_gnss_mip_gnss_spartn_configuration_command; -void insert_mip_gnss_mip_gnss_spartn_configuration_command(struct mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_command* self); -void extract_mip_gnss_mip_gnss_spartn_configuration_command(struct mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_command* self); - -struct mip_gnss_mip_gnss_spartn_configuration_response -{ - uint8_t enable; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) - uint32_t current_key_tow; ///< The GPS time of week the current key is valid until - uint16_t current_key_week; ///< The GPS week number the current key is valid until - uint8_t current_key[32]; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) - uint32_t next_key_tow; ///< The GPS time of week the next key is valid until - uint16_t next_key_week; ///< The GPS week number the next key is valid until - uint8_t next_key[32]; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) - -}; -typedef struct mip_gnss_mip_gnss_spartn_configuration_response mip_gnss_mip_gnss_spartn_configuration_response; -void insert_mip_gnss_mip_gnss_spartn_configuration_response(struct mip_serializer* serializer, const mip_gnss_mip_gnss_spartn_configuration_response* self); -void extract_mip_gnss_mip_gnss_spartn_configuration_response(struct mip_serializer* serializer, mip_gnss_mip_gnss_spartn_configuration_response* self); - -mip_cmd_result mip_gnss_write_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t enable, uint8_t type, uint32_t current_key_tow, uint16_t current_key_week, const uint8_t* current_key, uint32_t next_key_tow, uint16_t next_key_week, const uint8_t* next_key); -mip_cmd_result mip_gnss_read_mip_gnss_spartn_configuration(struct mip_interface* device, uint8_t* enable_out, uint8_t* type_out, uint32_t* current_key_tow_out, uint16_t* current_key_week_out, uint8_t* current_key_out, uint32_t* next_key_tow_out, uint16_t* next_key_week_out, uint8_t* next_key_out); -mip_cmd_result mip_gnss_save_mip_gnss_spartn_configuration(struct mip_interface* device); -mip_cmd_result mip_gnss_load_mip_gnss_spartn_configuration(struct mip_interface* device); -mip_cmd_result mip_gnss_default_mip_gnss_spartn_configuration(struct mip_interface* device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_gnss_rtk_dongle_configuration (0x0E,0x10) Rtk Dongle Configuration [C] /// Configure the communications with the RTK Dongle connected to the device. /// diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 63c518c37..1ecefbd15 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -34,12 +34,10 @@ enum CMD_LIST_RECEIVERS = 0x01, CMD_SIGNAL_CONFIGURATION = 0x02, CMD_RTK_DONGLE_CONFIGURATION = 0x10, - CMD_SPARTN_CONFIGURATION = 0x20, REPLY_LIST_RECEIVERS = 0x81, REPLY_SIGNAL_CONFIGURATION = 0x82, REPLY_RTK_DONGLE_CONFIGURATION = 0x90, - REPLY_SPARTN_CONFIGURATION = 0xA0, }; //////////////////////////////////////////////////////////////////////////////// @@ -155,66 +153,6 @@ CmdResult defaultSignalConfiguration(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_gnss_mip_gnss_spartn_configuration (0x0E,0x20) Mip Gnss Spartn Configuration [CPP] -/// Configure the SPARTN corrections service parameters. -/// Notes:
-/// - Enable and type settings will only update after a power cycle
-/// - Type settings will only take effect after a power cycle
-/// - Key information can be updated while running -/// -///@{ - -struct MipGnssSpartnConfiguration -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SPARTN_CONFIGURATION; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type = 0; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) - uint32_t current_key_tow = 0; ///< The GPS time of week the current key is valid until - uint16_t current_key_week = 0; ///< The GPS week number the current key is valid until - uint8_t current_key[32] = {0}; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) - uint32_t next_key_tow = 0; ///< The GPS time of week the next key is valid until - uint16_t next_key_week = 0; ///< The GPS week number the next key is valid until - uint8_t next_key[32] = {0}; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SPARTN_CONFIGURATION; - - uint8_t enable = 0; ///< Enable/Disable the SPARTN subsystem (0 = Disabled, 1 = Enabled) - uint8_t type = 0; ///< Connection type (0 - None, 1 = Network, 2 = L-Band) - uint32_t current_key_tow = 0; ///< The GPS time of week the current key is valid until - uint16_t current_key_week = 0; ///< The GPS week number the current key is valid until - uint8_t current_key[32] = {0}; ///< 32 character string of ASCII hex values for the current key (e.g. "bc" for 0xBC) - uint32_t next_key_tow = 0; ///< The GPS time of week the next key is valid until - uint16_t next_key_week = 0; ///< The GPS week number the next key is valid until - uint8_t next_key[32] = {0}; ///< 32 character string of ASCII hex valuesfor the next key (e.g. "bc" for 0xBC) - - }; -}; -void insert(Serializer& serializer, const MipGnssSpartnConfiguration& self); -void extract(Serializer& serializer, MipGnssSpartnConfiguration& self); - -void insert(Serializer& serializer, const MipGnssSpartnConfiguration::Response& self); -void extract(Serializer& serializer, MipGnssSpartnConfiguration::Response& self); - -CmdResult writeMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t enable, uint8_t type, uint32_t currentKeyTow, uint16_t currentKeyWeek, const uint8_t* currentKey, uint32_t nextKeyTow, uint16_t nextKeyWeek, const uint8_t* nextKey); -CmdResult readMipGnssSpartnConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* typeOut, uint32_t* currentKeyTowOut, uint16_t* currentKeyWeekOut, uint8_t* currentKeyOut, uint32_t* nextKeyTowOut, uint16_t* nextKeyWeekOut, uint8_t* nextKeyOut); -CmdResult saveMipGnssSpartnConfiguration(C::mip_interface& device); -CmdResult loadMipGnssSpartnConfiguration(C::mip_interface& device); -CmdResult defaultMipGnssSpartnConfiguration(C::mip_interface& device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_gnss_rtk_dongle_configuration (0x0E,0x10) Rtk Dongle Configuration [CPP] /// Configure the communications with the RTK Dongle connected to the device. /// From c4229a1417a843b3b42d19a484d5a9c3ab171aed Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 20 Jan 2023 15:09:35 -0500 Subject: [PATCH 034/252] Fix cmake installation of internal header files. --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a76ff7b13..e6c833197 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -355,6 +355,9 @@ set(ALL_MIP_HEADERS "${ALL_MIP_SOURCES}") list(FILTER ALL_MIP_HEADERS INCLUDE REGEX "^.*\.(h|hpp)$") foreach(MIP_HEADER ${ALL_MIP_HEADERS}) string(REPLACE "${SRC_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER}") + string(REPLACE "${INT_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER_RELATIVE}") + #string(REGEX MATCH ".*\/mip\/(.*)$" MIP_HEADER_RELATIVE "${MIP_HEADER}") + #message("hdr=${MIP_HEADER} out=${MIP_HEADER_RELATIVE}") set(MIP_HEADER_DESTINATION_FULL "${CMAKE_INSTALL_INCLUDEDIR}/${MIP_HEADER_RELATIVE}") get_filename_component(MIP_HEADER_DESTINATION "${MIP_HEADER_DESTINATION_FULL}" DIRECTORY) install( From ab976a3785756d16202e0eeb8455e86e6e539127 Mon Sep 17 00:00:00 2001 From: "Ian J. Moore" <46928068+ianjmoore@users.noreply.github.com> Date: Thu, 16 Feb 2023 17:25:30 -0500 Subject: [PATCH 035/252] Update CV7_example.cpp (#46) Changed ln 280 to say "Once in AHRS Filter Mode ..." from "Once in full nav mode ..." because Full Nav mode is not supported on CV7-AHRS. --- examples/CV7/CV7_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index fb1cc991e..a47a81c21 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -277,7 +277,7 @@ int main(int argc, const char* argv[]) filter_state_ahrs = true; } - //Once in full nav, print out data at 10 Hz + //Once in AHRS Flter Mode, print out data at 10 Hz if(filter_state_ahrs) { mip::Timestamp curr_timestamp = getCurrentTimestamp(); From c78c8ec392c9bb2c7611f0427b77aa9f264de526 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 17 Feb 2023 11:24:16 -0500 Subject: [PATCH 036/252] Serial Port: (#54) - Set exclusive access mode on Linux. - Fix file descriptor leaks if serial port config fails after the handle is opened. - Remove redundant is_open struct parameter and check the handle value directly. --- src/mip/utils/serial_port.c | 35 ++++++++++++++++++++++++++--------- src/mip/utils/serial_port.h | 1 - 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 5da24d3a4..2f714cec4 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -78,6 +78,8 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) if(SetupComm(port->handle, COM_PORT_BUFFER_SIZE, COM_PORT_BUFFER_SIZE) == 0) { MIP_LOG_ERROR("Unable to setup com port buffer size (%d)\n", GetLastError()); + CloseHandle(port->handle); + port->handle = INVALID_HANDLE_VALUE; return false; } @@ -102,6 +104,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) { MIP_LOG_ERROR("Unable to get com state\n"); CloseHandle(port->handle); + port->handle = INVALID_HANDLE_VALUE; return false; } @@ -119,6 +122,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) { MIP_LOG_ERROR("Unable to set com state\n"); CloseHandle(port->handle); + port->handle = INVALID_HANDLE_VALUE; return false; } @@ -132,17 +136,26 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) return false; } + if( ioctl(port->handle, TIOCEXCL) < 0 ) + { + MIP_LOG_WARN("Unable to set exclusive mode on serial port (%d): %s\n", errno, strerror(errno)); + } + // Set up baud rate and other serial device options struct termios serial_port_settings; if (tcgetattr(port->handle, &serial_port_settings) < 0) { MIP_LOG_ERROR("Unable to get serial port settings (%d): %s\n", errno, strerror(errno)); + close(port->handle); + port->handle = -1; return false; } if (cfsetispeed(&serial_port_settings, baud_rate_to_speed(baudrate)) < 0 || cfsetospeed(&serial_port_settings, baud_rate_to_speed(baudrate)) < 0) { MIP_LOG_ERROR("Unable to set baud rate (%d): %s\n", errno, strerror(errno)); + close(port->handle); + port->handle = -1; return false; } @@ -160,22 +173,23 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) if(tcsetattr(port->handle, TCSANOW, &serial_port_settings) < 0) { MIP_LOG_ERROR("Unable to save serial port settings (%d): %s\n", errno, strerror(errno)); + close(port->handle); + port->handle = -1; return false; } - + // Flush any waiting data tcflush(port->handle, TCIOFLUSH); #endif //Success - port->is_open = true; return true; } bool serial_port_close(serial_port *port) { - if(!port->is_open) + if(!serial_port_is_open(port)) return false; #ifdef WIN32 //Windows @@ -187,7 +201,6 @@ bool serial_port_close(serial_port *port) close(port->handle); #endif - port->is_open = false; return true; } @@ -197,7 +210,7 @@ bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, *bytes_written = 0; //Check for a valid port handle - if(!port->is_open) + if(!serial_port_is_open(port)) return false; #ifdef WIN32 //Windows @@ -219,7 +232,7 @@ bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, return true; else if(*bytes_written == (size_t)-1) MIP_LOG_ERROR("Failed to write serial data (%d): %s\n", errno, strerror(errno)); - + #endif return false; @@ -231,7 +244,7 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai *bytes_read = 0; //Check for a valid port handle - if(!port->is_open) + if(!serial_port_is_open(port)) return false; #ifdef WIN32 //Windows @@ -278,7 +291,7 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai uint32_t serial_port_read_count(serial_port *port) { //Check for a valid port handle - if(!port->is_open) + if(!serial_port_is_open(port)) return 0; #ifdef WIN32 //Windows @@ -303,5 +316,9 @@ uint32_t serial_port_read_count(serial_port *port) bool serial_port_is_open(serial_port *port) { - return port->is_open; +#ifdef WIN32 + return port->handle != INVALID_HANDLE_VALUE; +#else + return port->handle >= 0; +#endif } diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index d8850b78d..d06359d26 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -35,7 +35,6 @@ extern "C" { typedef struct serial_port { - bool is_open; #ifdef WIN32 //Windows HANDLE handle; #else //Linux From 411d1750dd53820224813434dfb44e6dc18631b9 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 27 Mar 2023 17:22:46 -0400 Subject: [PATCH 037/252] Remove line ending stuff from .gitattributes. (#56) --- .gitattributes | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/.gitattributes b/.gitattributes index 1537c1938..c88e27595 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,13 +1,3 @@ -# Set the default behavior, in case people don't have core.autocrlf set. -* text=auto - -# Declare files that will always have LF line endings on checkout. -*.c* text eol=LF -*.h* text eol=LF -*.md text eol=LF -CMakeLists.txt text eol=LF -LICENSE.txt text eol=LF - # Denote all files that are truly binary and should not be modified. *.bin binary -*.svg binary \ No newline at end of file +*.svg binary From 950e65178d63d1d98364111186586a9701829048 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 27 Mar 2023 17:48:04 -0400 Subject: [PATCH 038/252] Merge definitions/trunk to develop (#53) * Generate MIP definitions from trunk@54432. * Generate MIP definitions from trunk@54434. * Fix cmake installation of internal header files. * Remove commented code in CMakeLists.txt prior to merge. --- CMakeLists.txt | 1 + src/mip/definitions/commands_3dm.c | 18 +-- src/mip/definitions/commands_3dm.cpp | 18 +-- src/mip/definitions/commands_3dm.h | 49 ++++---- src/mip/definitions/commands_3dm.hpp | 52 +++++---- src/mip/definitions/commands_base.h | 2 + src/mip/definitions/commands_base.hpp | 5 + src/mip/definitions/commands_filter.h | 3 + src/mip/definitions/commands_filter.hpp | 12 ++ src/mip/definitions/commands_rtk.h | 5 +- src/mip/definitions/commands_rtk.hpp | 18 ++- src/mip/definitions/data_filter.h | 4 + src/mip/definitions/data_filter.hpp | 82 ++++++++------ src/mip/definitions/data_gnss.h | 32 ++++++ src/mip/definitions/data_gnss.hpp | 144 ++++++++++++++++++++++-- src/mip/definitions/data_sensor.h | 2 + src/mip/definitions/data_sensor.hpp | 36 +++--- src/mip/definitions/data_shared.h | 3 + src/mip/definitions/data_shared.hpp | 12 ++ 19 files changed, 378 insertions(+), 120 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a76ff7b13..5542a12ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -355,6 +355,7 @@ set(ALL_MIP_HEADERS "${ALL_MIP_SOURCES}") list(FILTER ALL_MIP_HEADERS INCLUDE REGEX "^.*\.(h|hpp)$") foreach(MIP_HEADER ${ALL_MIP_HEADERS}) string(REPLACE "${SRC_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER}") + string(REPLACE "${INT_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER_RELATIVE}") set(MIP_HEADER_DESTINATION_FULL "${CMAKE_INSTALL_INCLUDEDIR}/${MIP_HEADER_RELATIVE}") get_filename_component(MIP_HEADER_DESTINATION "${MIP_HEADER_DESTINATION_FULL}" DIRECTORY) install( diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index caef68235..2e39a0fd1 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -4761,7 +4761,7 @@ mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mi } return result; } -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_command* self) +void insert_mip_3dm_lowpass_filter_command(mip_serializer* serializer, const mip_3dm_lowpass_filter_command* self) { insert_mip_function_selector(serializer, self->function); @@ -4779,7 +4779,7 @@ void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serialize } } -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_command* self) +void extract_mip_3dm_lowpass_filter_command(mip_serializer* serializer, mip_3dm_lowpass_filter_command* self) { extract_mip_function_selector(serializer, &self->function); @@ -4798,7 +4798,7 @@ void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(mip_serializer* serializ } } -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_response* self) +void insert_mip_3dm_lowpass_filter_response(mip_serializer* serializer, const mip_3dm_lowpass_filter_response* self) { insert_u8(serializer, self->desc_set); @@ -4811,7 +4811,7 @@ void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializ insert_float(serializer, self->frequency); } -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_response* self) +void extract_mip_3dm_lowpass_filter_response(mip_serializer* serializer, mip_3dm_lowpass_filter_response* self) { extract_u8(serializer, &self->desc_set); @@ -4825,7 +4825,7 @@ void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(mip_serializer* seriali } -mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency) +mip_cmd_result mip_3dm_write_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4847,7 +4847,7 @@ mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* de return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out) +mip_cmd_result mip_3dm_read_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4887,7 +4887,7 @@ mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* dev } return result; } -mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +mip_cmd_result mip_3dm_save_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4903,7 +4903,7 @@ mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* dev return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +mip_cmd_result mip_3dm_load_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4919,7 +4919,7 @@ mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* dev return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_default_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) +mip_cmd_result mip_3dm_default_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index fd5471d53..c727face8 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -4227,7 +4227,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso } return result; } -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self) +void insert(Serializer& serializer, const LowpassFilter& self) { insert(serializer, self.function); @@ -4245,7 +4245,7 @@ void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self) } } -void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self) +void extract(Serializer& serializer, LowpassFilter& self) { extract(serializer, self.function); @@ -4264,7 +4264,7 @@ void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self) } } -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self) +void insert(Serializer& serializer, const LowpassFilter::Response& self) { insert(serializer, self.desc_set); @@ -4277,7 +4277,7 @@ void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self insert(serializer, self.frequency); } -void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self) +void extract(Serializer& serializer, LowpassFilter::Response& self) { extract(serializer, self.desc_set); @@ -4291,7 +4291,7 @@ void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self) } -CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) +CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4311,7 +4311,7 @@ CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) +CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4348,7 +4348,7 @@ CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, } return result; } -CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4362,7 +4362,7 @@ CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4376,7 +4376,7 @@ CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 3d96ab5ed..45fe8ec8a 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -147,27 +147,27 @@ enum typedef uint8_t mip_nmea_message_message_id; static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GGA = 1; ///< GPS System Fix Data. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GLL = 2; ///< Geographic Position Lat/Lon. Source can be the Filter or GNSS1/2 datasets. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GSV = 3; ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID is ignored (talker depends on the satellite). +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_GSV = 3; ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID must be set to IGNORED. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_RMC = 4; ///< Recommended Minimum Specific GNSS Data. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_VTG = 5; ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_HDT = 6; ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_ZDA = 7; ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKA = 129; ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID is ignored. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PRKR = 130; ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID is ignored. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PKRA = 129; ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PKRR = 130; ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. typedef uint8_t mip_nmea_message_talker_id; -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_RESERVED = 0; ///< -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GNSS = 1; ///< NMEA message will be produced with talker id "GN" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GPS = 2; ///< NMEA message will be produced with talker id "GP" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GALILEO = 3; ///< NMEA message will be produced with talker id "GA" -static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GLONASS = 4; ///< NMEA message will be produced with talker id "GL" +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_IGNORED = 0; ///< Talker ID cannot be changed. +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GNSS = 1; ///< NMEA message will be produced with talker id "GN". +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GPS = 2; ///< NMEA message will be produced with talker id "GP". +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GALILEO = 3; ///< NMEA message will be produced with talker id "GA". +static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_GLONASS = 4; ///< NMEA message will be produced with talker id "GL". struct mip_nmea_message { mip_nmea_message_message_id message_id; ///< NMEA sentence type. mip_nmea_message_talker_id talker_id; ///< NMEA talker ID. Ignored for proprietary sentences. uint8_t source_desc_set; ///< Data descriptor set where the data will be sourced. Available options depend on the sentence. - uint16_t decimation; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. + uint16_t decimation; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. Must be 0 when polling. }; typedef struct mip_nmea_message mip_nmea_message; @@ -758,6 +758,7 @@ static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SE static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ENABLE_RANGING = 0x0001; ///< Use SBAS pseudo-ranges in position solution static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ENABLE_CORRECTIONS = 0x0002; ///< Use SBAS differential corrections static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_APPLY_INTEGRITY = 0x0004; ///< Use SBAS integrity information. If enabled, only GPS satellites for which integrity information is available will be used. +static const mip_3dm_gnss_sbas_settings_command_sbasoptions MIP_3DM_GNSS_SBAS_SETTINGS_COMMAND_SBASOPTIONS_ALL = 0x0007; struct mip_3dm_gnss_sbas_settings_command { @@ -976,6 +977,7 @@ static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PI static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_OPEN_DRAIN = 0x01; ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pull-up resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLDOWN = 0x02; ///< The pin will have an internal pull-down resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pull-up. See the device specifications for the resistance value. static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_PULLUP = 0x04; ///< The pin will have an internal pull-up resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pull-down. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. +static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_ALL = 0x07; struct mip_3dm_gpio_config_command { @@ -1233,6 +1235,7 @@ static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_T static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_ACTIVE = 0x01; ///< True if the trigger is currently active (either due to its logic or being in test mode). static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_ENABLED = 0x02; ///< True if the trigger is enabled. static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_TEST = 0x04; ///< True if the trigger is in test mode. +static const mip_3dm_get_event_trigger_status_command_status MIP_3DM_GET_EVENT_TRIGGER_STATUS_COMMAND_STATUS_ALL = 0x07; struct mip_3dm_get_event_trigger_status_command_entry { @@ -2042,7 +2045,7 @@ mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mi ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_3dm_mip_cmd_3dm_lowpass_filter (0x0C,0x54) Mip Cmd 3Dm Lowpass Filter [C] +///@defgroup c_3dm_lowpass_filter (0x0C,0x54) Lowpass Filter [C] /// This command controls the low-pass anti-aliasing filter supported data quantities. /// /// See the device user manual for data quantities which support the anti-aliasing filter. @@ -2060,7 +2063,7 @@ mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mi /// ///@{ -struct mip_3dm_mip_cmd_3dm_lowpass_filter_command +struct mip_3dm_lowpass_filter_command { mip_function_selector function; uint8_t desc_set; ///< Descriptor set of the quantity to be filtered. @@ -2070,11 +2073,11 @@ struct mip_3dm_mip_cmd_3dm_lowpass_filter_command float frequency; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. }; -typedef struct mip_3dm_mip_cmd_3dm_lowpass_filter_command mip_3dm_mip_cmd_3dm_lowpass_filter_command; -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_command(struct mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_command* self); -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_command(struct mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_command* self); +typedef struct mip_3dm_lowpass_filter_command mip_3dm_lowpass_filter_command; +void insert_mip_3dm_lowpass_filter_command(struct mip_serializer* serializer, const mip_3dm_lowpass_filter_command* self); +void extract_mip_3dm_lowpass_filter_command(struct mip_serializer* serializer, mip_3dm_lowpass_filter_command* self); -struct mip_3dm_mip_cmd_3dm_lowpass_filter_response +struct mip_3dm_lowpass_filter_response { uint8_t desc_set; ///< Descriptor set of the quantity to be filtered. uint8_t field_desc; ///< Field descriptor of the quantity to be filtered. @@ -2083,15 +2086,15 @@ struct mip_3dm_mip_cmd_3dm_lowpass_filter_response float frequency; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. }; -typedef struct mip_3dm_mip_cmd_3dm_lowpass_filter_response mip_3dm_mip_cmd_3dm_lowpass_filter_response; -void insert_mip_3dm_mip_cmd_3dm_lowpass_filter_response(struct mip_serializer* serializer, const mip_3dm_mip_cmd_3dm_lowpass_filter_response* self); -void extract_mip_3dm_mip_cmd_3dm_lowpass_filter_response(struct mip_serializer* serializer, mip_3dm_mip_cmd_3dm_lowpass_filter_response* self); +typedef struct mip_3dm_lowpass_filter_response mip_3dm_lowpass_filter_response; +void insert_mip_3dm_lowpass_filter_response(struct mip_serializer* serializer, const mip_3dm_lowpass_filter_response* self); +void extract_mip_3dm_lowpass_filter_response(struct mip_serializer* serializer, mip_3dm_lowpass_filter_response* self); -mip_cmd_result mip_3dm_write_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency); -mip_cmd_result mip_3dm_read_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out); -mip_cmd_result mip_3dm_save_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); -mip_cmd_result mip_3dm_load_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); -mip_cmd_result mip_3dm_default_mip_cmd_3dm_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_write_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool enable, bool manual, float frequency); +mip_cmd_result mip_3dm_read_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc, bool* enable_out, bool* manual_out, float* frequency_out); +mip_cmd_result mip_3dm_save_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_load_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); +mip_cmd_result mip_3dm_default_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); ///@} /// diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 989d08120..845637b38 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -149,28 +149,28 @@ struct NmeaMessage { GGA = 1, ///< GPS System Fix Data. Source can be the Filter or GNSS1/2 datasets. GLL = 2, ///< Geographic Position Lat/Lon. Source can be the Filter or GNSS1/2 datasets. - GSV = 3, ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID is ignored (talker depends on the satellite). + GSV = 3, ///< GNSS Satellites in View. Source must be either GNSS1 or GNSS2 datasets. The talker ID must be set to IGNORED. RMC = 4, ///< Recommended Minimum Specific GNSS Data. Source can be the Filter or GNSS1/2 datasets. VTG = 5, ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. HDT = 6, ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. ZDA = 7, ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. - PRKA = 129, ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID is ignored. - PRKR = 130, ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID is ignored. + PKRA = 129, ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. + PKRR = 130, ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. }; enum class TalkerID : uint8_t { - RESERVED = 0, ///< - GNSS = 1, ///< NMEA message will be produced with talker id "GN" - GPS = 2, ///< NMEA message will be produced with talker id "GP" - GALILEO = 3, ///< NMEA message will be produced with talker id "GA" - GLONASS = 4, ///< NMEA message will be produced with talker id "GL" + IGNORED = 0, ///< Talker ID cannot be changed. + GNSS = 1, ///< NMEA message will be produced with talker id "GN". + GPS = 2, ///< NMEA message will be produced with talker id "GP". + GALILEO = 3, ///< NMEA message will be produced with talker id "GA". + GLONASS = 4, ///< NMEA message will be produced with talker id "GL". }; MessageID message_id = static_cast(0); ///< NMEA sentence type. TalkerID talker_id = static_cast(0); ///< NMEA talker ID. Ignored for proprietary sentences. uint8_t source_desc_set = 0; ///< Data descriptor set where the data will be sourced. Available options depend on the sentence. - uint16_t decimation = 0; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. + uint16_t decimation = 0; ///< Decimation from the base rate for source_desc_set. Frequency is limited to 10 Hz or the base rate, whichever is lower. Must be 0 when polling. }; void insert(Serializer& serializer, const NmeaMessage& self); @@ -916,6 +916,7 @@ struct GnssSbasSettings ENABLE_RANGING = 0x0001, ///< Use SBAS pseudo-ranges in position solution ENABLE_CORRECTIONS = 0x0002, ///< Use SBAS differential corrections APPLY_INTEGRITY = 0x0004, ///< Use SBAS integrity information. If enabled, only GPS satellites for which integrity information is available will be used. + ALL = 0x0007, }; uint16_t value = NONE; @@ -933,6 +934,9 @@ struct GnssSbasSettings void enableCorrections(bool val) { if(val) value |= ENABLE_CORRECTIONS; else value &= ~ENABLE_CORRECTIONS; } bool applyIntegrity() const { return (value & APPLY_INTEGRITY) > 0; } void applyIntegrity(bool val) { if(val) value |= APPLY_INTEGRITY; else value &= ~APPLY_INTEGRITY; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -1195,6 +1199,7 @@ struct GpioConfig OPEN_DRAIN = 0x01, ///< The pin will be an open-drain output. The state will be either LOW or FLOATING instead of LOW or HIGH, respectively. This is used to connect multiple open-drain outputs from several devices. An internal or external pull-up resistor is typically used in combination. The maximum voltage of an open drain output is subject to the device maximum input voltage range found in the specifications. PULLDOWN = 0x02, ///< The pin will have an internal pull-down resistor enabled. This is useful for connecting inputs to signals which can only be pulled high such as mechanical switches. Cannot be used in combination with pull-up. See the device specifications for the resistance value. PULLUP = 0x04, ///< The pin will have an internal pull-up resistor enabled. Useful for connecting inputs to signals which can only be pulled low such as mechanical switches, or in combination with an open drain output. Cannot be used in combination with pull-down. See the device specifications for the resistance value. Use of this mode may restrict the maximum allowed input voltage. See the device datasheet for details. + ALL = 0x07, }; uint8_t value = NONE; @@ -1212,6 +1217,9 @@ struct GpioConfig void pulldown(bool val) { if(val) value |= PULLDOWN; else value &= ~PULLDOWN; } bool pullup() const { return (value & PULLUP) > 0; } void pullup(bool val) { if(val) value |= PULLUP; else value &= ~PULLUP; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -1502,6 +1510,7 @@ struct GetEventTriggerStatus ACTIVE = 0x01, ///< True if the trigger is currently active (either due to its logic or being in test mode). ENABLED = 0x02, ///< True if the trigger is enabled. TEST = 0x04, ///< True if the trigger is in test mode. + ALL = 0x07, }; uint8_t value = NONE; @@ -1519,6 +1528,9 @@ struct GetEventTriggerStatus void enabled(bool val) { if(val) value |= ENABLED; else value &= ~ENABLED; } bool test() const { return (value & TEST) > 0; } void test(bool val) { if(val) value |= TEST; else value &= ~TEST; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct Entry @@ -2450,7 +2462,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_3dm_mip_cmd_3dm_lowpass_filter (0x0C,0x54) Mip Cmd 3Dm Lowpass Filter [CPP] +///@defgroup cpp_3dm_lowpass_filter (0x0C,0x54) Lowpass Filter [CPP] /// This command controls the low-pass anti-aliasing filter supported data quantities. /// /// See the device user manual for data quantities which support the anti-aliasing filter. @@ -2468,7 +2480,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso /// ///@{ -struct MipCmd3dmLowpassFilter +struct LowpassFilter { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; @@ -2499,17 +2511,17 @@ struct MipCmd3dmLowpassFilter }; }; -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter& self); -void extract(Serializer& serializer, MipCmd3dmLowpassFilter& self); +void insert(Serializer& serializer, const LowpassFilter& self); +void extract(Serializer& serializer, LowpassFilter& self); -void insert(Serializer& serializer, const MipCmd3dmLowpassFilter::Response& self); -void extract(Serializer& serializer, MipCmd3dmLowpassFilter::Response& self); +void insert(Serializer& serializer, const LowpassFilter::Response& self); +void extract(Serializer& serializer, LowpassFilter::Response& self); -CmdResult writeMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); -CmdResult readMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); -CmdResult saveMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult loadMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult defaultMipCmd3dmLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); +CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); +CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); ///@} /// diff --git a/src/mip/definitions/commands_base.h b/src/mip/definitions/commands_base.h index f94559e66..073542861 100644 --- a/src/mip/definitions/commands_base.h +++ b/src/mip/definitions/commands_base.h @@ -109,6 +109,7 @@ static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_RTCM_F static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_RTK_FAULT = 0x20000000; ///< static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_SOLUTION_FAULT = 0x40000000; ///< static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_GNSS_GENERAL_FAULT = 0x80000000; ///< +static const mip_commanded_test_bits_gq7 MIP_COMMANDED_TEST_BITS_GQ7_ALL = 0xFFFFFFFF; void insert_mip_commanded_test_bits_gq7(struct mip_serializer* serializer, const mip_commanded_test_bits_gq7 self); void extract_mip_commanded_test_bits_gq7(struct mip_serializer* serializer, mip_commanded_test_bits_gq7* self); @@ -310,6 +311,7 @@ mip_cmd_result mip_base_default_comm_speed(struct mip_interface* device, uint8_t /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_base_gps_time_update (0x01,0x72) Gps Time Update [C] +/// Set device internal GPS time /// When combined with a PPS input signal applied to the I/O connector, this command enables complete synchronization of data outputs /// with an external time base, such as GPS system time. Since the hardware PPS synchronization can only detect the fractional number of seconds when pulses arrive, /// complete synchronization requires that the user provide the whole number of seconds via this command. After achieving PPS synchronization, this command should be sent twice: once to set the time-of-week and once to set the week number. PPS synchronization can be verified by monitoring the time sync status message (0xA0, 0x02) or the valid flags of any shared external timestamp (0x--, D7) data field. diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 9bb661ce2..c0f8abc41 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -109,6 +109,7 @@ struct CommandedTestBitsGq7 : Bitfield GNSS_RTK_FAULT = 0x20000000, ///< GNSS_SOLUTION_FAULT = 0x40000000, ///< GNSS_GENERAL_FAULT = 0x80000000, ///< + ALL = 0xFFFFFFFF, }; uint32_t value = NONE; @@ -174,6 +175,9 @@ struct CommandedTestBitsGq7 : Bitfield void gnssSolutionFault(bool val) { if(val) value |= GNSS_SOLUTION_FAULT; else value &= ~GNSS_SOLUTION_FAULT; } bool gnssGeneralFault() const { return (value & GNSS_GENERAL_FAULT) > 0; } void gnssGeneralFault(bool val) { if(val) value |= GNSS_GENERAL_FAULT; else value &= ~GNSS_GENERAL_FAULT; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; @@ -489,6 +493,7 @@ CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_base_gps_time_update (0x01,0x72) Gps Time Update [CPP] +/// Set device internal GPS time /// When combined with a PPS input signal applied to the I/O connector, this command enables complete synchronization of data outputs /// with an external time base, such as GPS system time. Since the hardware PPS synchronization can only detect the fractional number of seconds when pulses arrive, /// complete synchronization requires that the user provide the whole number of seconds via this command. After achieving PPS synchronization, this command should be sent twice: once to set the time-of-week and once to set the week number. PPS synchronization can be verified by monitoring the time sync status message (0xA0, 0x02) or the valid flags of any shared external timestamp (0x--, D7) data field. diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 3b52d260f..32f1eb72f 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -251,6 +251,7 @@ static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIM static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_ANTENNA_OFFSET = 0x0010; ///< static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_AUTO_MAG_HARD_IRON = 0x0020; ///< static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_AUTO_MAG_SOFT_IRON = 0x0040; ///< +static const mip_filter_estimation_control_command_enable_flags MIP_FILTER_ESTIMATION_CONTROL_COMMAND_ENABLE_FLAGS_ALL = 0x007F; struct mip_filter_estimation_control_command { @@ -395,6 +396,7 @@ static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_O static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_ROLL = 0x1; ///< static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_PITCH = 0x2; ///< static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_YAW = 0x4; ///< +static const mip_filter_tare_orientation_command_mip_tare_axes MIP_FILTER_TARE_ORIENTATION_COMMAND_MIP_TARE_AXES_ALL = 0x7; struct mip_filter_tare_orientation_command { @@ -1484,6 +1486,7 @@ static const mip_filter_initialization_configuration_command_alignment_selector static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_DUAL_ANTENNA = 0x01; ///< Dual-antenna GNSS alignment static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_KINEMATIC = 0x02; ///< GNSS kinematic alignment (GNSS velocity determines initial heading) static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_MAGNETOMETER = 0x04; ///< Magnetometer heading alignment +static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_ALL = 0x07; typedef uint8_t mip_filter_initialization_configuration_command_initial_condition_source; static const mip_filter_initialization_configuration_command_initial_condition_source MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_INITIAL_CONDITION_SOURCE_AUTO_POS_VEL_ATT = 0; ///< Automatic position, velocity and attitude diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 28a0ef255..06bebc646 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -277,6 +277,7 @@ struct EstimationControl ANTENNA_OFFSET = 0x0010, ///< AUTO_MAG_HARD_IRON = 0x0020, ///< AUTO_MAG_SOFT_IRON = 0x0040, ///< + ALL = 0x007F, }; uint16_t value = NONE; @@ -302,6 +303,9 @@ struct EstimationControl void autoMagHardIron(bool val) { if(val) value |= AUTO_MAG_HARD_IRON; else value &= ~AUTO_MAG_HARD_IRON; } bool autoMagSoftIron() const { return (value & AUTO_MAG_SOFT_IRON) > 0; } void autoMagSoftIron(bool val) { if(val) value |= AUTO_MAG_SOFT_IRON; else value &= ~AUTO_MAG_SOFT_IRON; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -469,6 +473,7 @@ struct TareOrientation ROLL = 0x1, ///< PITCH = 0x2, ///< YAW = 0x4, ///< + ALL = 0x7, }; uint8_t value = NONE; @@ -486,6 +491,9 @@ struct TareOrientation void pitch(bool val) { if(val) value |= PITCH; else value &= ~PITCH; } bool yaw() const { return (value & YAW) > 0; } void yaw(bool val) { if(val) value |= YAW; else value &= ~YAW; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FunctionSelector function = static_cast(0); @@ -1838,6 +1846,7 @@ struct InitializationConfiguration DUAL_ANTENNA = 0x01, ///< Dual-antenna GNSS alignment KINEMATIC = 0x02, ///< GNSS kinematic alignment (GNSS velocity determines initial heading) MAGNETOMETER = 0x04, ///< Magnetometer heading alignment + ALL = 0x07, }; uint8_t value = NONE; @@ -1855,6 +1864,9 @@ struct InitializationConfiguration void kinematic(bool val) { if(val) value |= KINEMATIC; else value &= ~KINEMATIC; } bool magnetometer() const { return (value & MAGNETOMETER) > 0; } void magnetometer(bool val) { if(val) value |= MAGNETOMETER; else value &= ~MAGNETOMETER; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; enum class InitialConditionSource : uint8_t diff --git a/src/mip/definitions/commands_rtk.h b/src/mip/definitions/commands_rtk.h index 028ca7c0f..31f6cb07d 100644 --- a/src/mip/definitions/commands_rtk.h +++ b/src/mip/definitions/commands_rtk.h @@ -98,6 +98,7 @@ static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_ST static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_RSRP = 0x0C000000; ///< static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_RSRQ = 0x30000000; ///< static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_SINR = 0xC0000000; ///< +static const mip_rtk_get_status_flags_command_status_flags_legacy MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_LEGACY_ALL = 0xFFFFFFFF; typedef uint32_t mip_rtk_get_status_flags_command_status_flags; static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_NONE = 0x00000000; @@ -108,11 +109,12 @@ static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FL static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_TOWER_CHANGE_INDICATOR = 0x00F00000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_NMEA_TIMEOUT = 0x01000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_SERVER_TIMEOUT = 0x02000000; ///< -static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_RTCM_TIMEOUT = 0x04000000; ///< +static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_CORRECTIONS_TIMEOUT = 0x04000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_DEVICE_OUT_OF_RANGE = 0x08000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_CORRECTIONS_UNAVAILABLE = 0x10000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_RESERVED = 0x20000000; ///< static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_VERSION = 0xC0000000; ///< +static const mip_rtk_get_status_flags_command_status_flags MIP_RTK_GET_STATUS_FLAGS_COMMAND_STATUS_FLAGS_ALL = 0xFFFFFFFF; void insert_mip_rtk_get_status_flags_command_status_flags_legacy(struct mip_serializer* serializer, const mip_rtk_get_status_flags_command_status_flags_legacy self); void extract_mip_rtk_get_status_flags_command_status_flags_legacy(struct mip_serializer* serializer, mip_rtk_get_status_flags_command_status_flags_legacy* self); @@ -286,6 +288,7 @@ static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_THROTTLE = 0x01; ///< static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_CORRECTIONS_UNAVAILABLE = 0x02; ///< static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_RESERVED = 0xFC; ///< +static const mip_rtk_service_status_command_service_flags MIP_RTK_SERVICE_STATUS_COMMAND_SERVICE_FLAGS_ALL = 0xFF; struct mip_rtk_service_status_command { diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 8cdf9b891..4611979f1 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -105,6 +105,7 @@ struct GetStatusFlags RSRP = 0x0C000000, ///< RSRQ = 0x30000000, ///< SINR = 0xC0000000, ///< + ALL = 0xFFFFFFFF, }; uint32_t value = NONE; @@ -138,6 +139,9 @@ struct GetStatusFlags void rsrq(uint32_t val) { value = (value & ~RSRQ) | (val << 28); } uint32_t sinr() const { return (value & SINR) >> 30; } void sinr(uint32_t val) { value = (value & ~SINR) | (val << 30); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct StatusFlags : Bitfield @@ -152,11 +156,12 @@ struct GetStatusFlags TOWER_CHANGE_INDICATOR = 0x00F00000, ///< NMEA_TIMEOUT = 0x01000000, ///< SERVER_TIMEOUT = 0x02000000, ///< - RTCM_TIMEOUT = 0x04000000, ///< + CORRECTIONS_TIMEOUT = 0x04000000, ///< DEVICE_OUT_OF_RANGE = 0x08000000, ///< CORRECTIONS_UNAVAILABLE = 0x10000000, ///< RESERVED = 0x20000000, ///< VERSION = 0xC0000000, ///< + ALL = 0xFFFFFFFF, }; uint32_t value = NONE; @@ -182,8 +187,8 @@ struct GetStatusFlags void nmeaTimeout(bool val) { if(val) value |= NMEA_TIMEOUT; else value &= ~NMEA_TIMEOUT; } bool serverTimeout() const { return (value & SERVER_TIMEOUT) > 0; } void serverTimeout(bool val) { if(val) value |= SERVER_TIMEOUT; else value &= ~SERVER_TIMEOUT; } - bool rtcmTimeout() const { return (value & RTCM_TIMEOUT) > 0; } - void rtcmTimeout(bool val) { if(val) value |= RTCM_TIMEOUT; else value &= ~RTCM_TIMEOUT; } + bool correctionsTimeout() const { return (value & CORRECTIONS_TIMEOUT) > 0; } + void correctionsTimeout(bool val) { if(val) value |= CORRECTIONS_TIMEOUT; else value &= ~CORRECTIONS_TIMEOUT; } bool deviceOutOfRange() const { return (value & DEVICE_OUT_OF_RANGE) > 0; } void deviceOutOfRange(bool val) { if(val) value |= DEVICE_OUT_OF_RANGE; else value &= ~DEVICE_OUT_OF_RANGE; } bool correctionsUnavailable() const { return (value & CORRECTIONS_UNAVAILABLE) > 0; } @@ -192,6 +197,9 @@ struct GetStatusFlags void reserved(bool val) { if(val) value |= RESERVED; else value &= ~RESERVED; } uint32_t version() const { return (value & VERSION) >> 30; } void version(uint32_t val) { value = (value & ~VERSION) | (val << 30); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; @@ -470,6 +478,7 @@ struct ServiceStatus THROTTLE = 0x01, ///< CORRECTIONS_UNAVAILABLE = 0x02, ///< RESERVED = 0xFC, ///< + ALL = 0xFF, }; uint8_t value = NONE; @@ -487,6 +496,9 @@ struct ServiceStatus void correctionsUnavailable(bool val) { if(val) value |= CORRECTIONS_UNAVAILABLE; else value &= ~CORRECTIONS_UNAVAILABLE; } uint8_t reserved() const { return (value & RESERVED) >> 2; } void reserved(uint8_t val) { value = (value & ~RESERVED) | (val << 2); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint32_t reserved1 = 0; diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index af7737875..9dfe6eedd 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -151,6 +151,7 @@ static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_ANTENNA_LEVER_A static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_MOUNTING_TRANSFORM_WARNING = 0x0200; ///< static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_TIME_SYNC_WARNING = 0x0400; ///< No time synchronization pulse detected static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_GQ7_SOLUTION_ERROR = 0xF000; ///< Filter computation warning flags. If any bits 12-15 are set, and all filter outputs will be invalid. +static const mip_filter_status_flags MIP_FILTER_STATUS_FLAGS_ALL = 0xFFFF; void insert_mip_filter_status_flags(struct mip_serializer* serializer, const mip_filter_status_flags self); void extract_mip_filter_status_flags(struct mip_serializer* serializer, mip_filter_status_flags* self); @@ -174,6 +175,7 @@ static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_R static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_SAMPLE_TIME_WARNING = 0x08; ///< static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_CONFIGURATION_ERROR = 0x10; ///< static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_MAX_NUM_MEAS_EXCEEDED = 0x20; ///< +static const mip_filter_measurement_indicator MIP_FILTER_MEASUREMENT_INDICATOR_ALL = 0x3F; void insert_mip_filter_measurement_indicator(struct mip_serializer* serializer, const mip_filter_measurement_indicator self); void extract_mip_filter_measurement_indicator(struct mip_serializer* serializer, mip_filter_measurement_indicator* self); @@ -196,6 +198,7 @@ static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_BEI_B2 static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_BEI_B3 = 0x2000; ///< If 1, the Kalman filter is using Beidou B3 measurements (not available on the GQ7) static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_NO_FIX = 0x4000; ///< If 1, this GNSS module is reporting no position fix static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_CONFIG_ERROR = 0x8000; ///< If 1, there is likely an issue with the antenna offset for this GNSS module +static const mip_gnss_aid_status_flags MIP_GNSS_AID_STATUS_FLAGS_ALL = 0xFFFF; void insert_mip_gnss_aid_status_flags(struct mip_serializer* serializer, const mip_gnss_aid_status_flags self); void extract_mip_gnss_aid_status_flags(struct mip_serializer* serializer, mip_gnss_aid_status_flags* self); @@ -1343,6 +1346,7 @@ static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_RCV_1_DATA_VALID = 0x0001; ///< static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_RCV_2_DATA_VALID = 0x0002; ///< static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_ANTENNA_OFFSETS_VALID = 0x0004; ///< +static const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags MIP_FILTER_GNSS_DUAL_ANTENNA_STATUS_DATA_DUAL_ANTENNA_STATUS_FLAGS_ALL = 0x0007; struct mip_filter_gnss_dual_antenna_status_data { diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index d6a9b1599..78bf3fd2b 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -151,6 +151,7 @@ struct FilterStatusFlags : Bitfield GQ7_MOUNTING_TRANSFORM_WARNING = 0x0200, ///< GQ7_TIME_SYNC_WARNING = 0x0400, ///< No time synchronization pulse detected GQ7_SOLUTION_ERROR = 0xF000, ///< Filter computation warning flags. If any bits 12-15 are set, and all filter outputs will be invalid. + ALL = 0xFFFF, }; uint16_t value = NONE; @@ -218,6 +219,9 @@ struct FilterStatusFlags : Bitfield void gq7TimeSyncWarning(bool val) { if(val) value |= GQ7_TIME_SYNC_WARNING; else value &= ~GQ7_TIME_SYNC_WARNING; } uint16_t gq7SolutionError() const { return (value & GQ7_SOLUTION_ERROR) >> 12; } void gq7SolutionError(uint16_t val) { value = (value & ~GQ7_SOLUTION_ERROR) | (val << 12); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; enum class FilterAidingMeasurementType : uint8_t @@ -241,6 +245,7 @@ struct FilterMeasurementIndicator : Bitfield SAMPLE_TIME_WARNING = 0x08, ///< CONFIGURATION_ERROR = 0x10, ///< MAX_NUM_MEAS_EXCEEDED = 0x20, ///< + ALL = 0x3F, }; uint8_t value = NONE; @@ -264,6 +269,9 @@ struct FilterMeasurementIndicator : Bitfield void configurationError(bool val) { if(val) value |= CONFIGURATION_ERROR; else value &= ~CONFIGURATION_ERROR; } bool maxNumMeasExceeded() const { return (value & MAX_NUM_MEAS_EXCEEDED) > 0; } void maxNumMeasExceeded(bool val) { if(val) value |= MAX_NUM_MEAS_EXCEEDED; else value &= ~MAX_NUM_MEAS_EXCEEDED; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct GnssAidStatusFlags : Bitfield @@ -287,6 +295,7 @@ struct GnssAidStatusFlags : Bitfield BEI_B3 = 0x2000, ///< If 1, the Kalman filter is using Beidou B3 measurements (not available on the GQ7) NO_FIX = 0x4000, ///< If 1, this GNSS module is reporting no position fix CONFIG_ERROR = 0x8000, ///< If 1, there is likely an issue with the antenna offset for this GNSS module + ALL = 0xFFFF, }; uint16_t value = NONE; @@ -330,6 +339,9 @@ struct GnssAidStatusFlags : Bitfield void noFix(bool val) { if(val) value |= NO_FIX; else value &= ~NO_FIX; } bool configError() const { return (value & CONFIG_ERROR) > 0; } void configError(bool val) { if(val) value |= CONFIG_ERROR; else value &= ~CONFIG_ERROR; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; @@ -418,7 +430,7 @@ struct AttitudeQuaternion auto as_tuple() const { - return std::make_tuple(q,valid_flags); + return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND @@ -455,7 +467,7 @@ struct AttitudeDcm auto as_tuple() const { - return std::make_tuple(dcm,valid_flags); + return std::make_tuple(dcm[0],dcm[1],dcm[2],dcm[3],dcm[4],dcm[5],dcm[6],dcm[7],dcm[8],valid_flags); } float dcm[9] = {0}; ///< Matrix elements in row-major order. @@ -512,7 +524,7 @@ struct GyroBias auto as_tuple() const { - return std::make_tuple(bias,valid_flags); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } float bias[3] = {0}; ///< (x, y, z) [radians/second] @@ -539,7 +551,7 @@ struct AccelBias auto as_tuple() const { - return std::make_tuple(bias,valid_flags); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } float bias[3] = {0}; ///< (x, y, z) [meters/second^2] @@ -654,7 +666,7 @@ struct GyroBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert,valid_flags); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } float bias_uncert[3] = {0}; ///< (x,y,z) [radians/sec] @@ -681,7 +693,7 @@ struct AccelBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert,valid_flags); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } float bias_uncert[3] = {0}; ///< (x,y,z) [meters/second^2] @@ -771,7 +783,7 @@ struct LinearAccel auto as_tuple() const { - return std::make_tuple(accel,valid_flags); + return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } float accel[3] = {0}; ///< (x,y,z) [meters/second^2] @@ -798,7 +810,7 @@ struct GravityVector auto as_tuple() const { - return std::make_tuple(gravity,valid_flags); + return std::make_tuple(gravity[0],gravity[1],gravity[2],valid_flags); } float gravity[3] = {0}; ///< (x, y, z) [meters/second^2] @@ -825,7 +837,7 @@ struct CompAccel auto as_tuple() const { - return std::make_tuple(accel,valid_flags); + return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } float accel[3] = {0}; ///< (x,y,z) [meters/second^2] @@ -852,7 +864,7 @@ struct CompAngularRate auto as_tuple() const { - return std::make_tuple(gyro,valid_flags); + return std::make_tuple(gyro[0],gyro[1],gyro[2],valid_flags); } float gyro[3] = {0}; ///< (x, y, z) [radians/second] @@ -879,7 +891,7 @@ struct QuaternionAttitudeUncertainty auto as_tuple() const { - return std::make_tuple(q,valid_flags); + return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } float q[4] = {0}; ///< [dimensionless] @@ -1006,7 +1018,7 @@ struct AccelScaleFactor auto as_tuple() const { - return std::make_tuple(scale_factor,valid_flags); + return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1033,7 +1045,7 @@ struct AccelScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(scale_factor_uncert,valid_flags); + return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1060,7 +1072,7 @@ struct GyroScaleFactor auto as_tuple() const { - return std::make_tuple(scale_factor,valid_flags); + return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1087,7 +1099,7 @@ struct GyroScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(scale_factor_uncert,valid_flags); + return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] @@ -1114,7 +1126,7 @@ struct MagBias auto as_tuple() const { - return std::make_tuple(bias,valid_flags); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } float bias[3] = {0}; ///< (x,y,z) [Gauss] @@ -1141,7 +1153,7 @@ struct MagBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert,valid_flags); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } float bias_uncert[3] = {0}; ///< (x,y,z) [Gauss] @@ -1260,7 +1272,7 @@ struct AntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(offset,valid_flags); + return std::make_tuple(offset[0],offset[1],offset[2],valid_flags); } float offset[3] = {0}; ///< (x,y,z) [meters] @@ -1287,7 +1299,7 @@ struct AntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(offset_uncert,valid_flags); + return std::make_tuple(offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } float offset_uncert[3] = {0}; ///< (x,y,z) [meters] @@ -1316,7 +1328,7 @@ struct MultiAntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(receiver_id,offset,valid_flags); + return std::make_tuple(receiver_id,offset[0],offset[1],offset[2],valid_flags); } uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached @@ -1344,7 +1356,7 @@ struct MultiAntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(receiver_id,offset_uncert,valid_flags); + return std::make_tuple(receiver_id,offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached @@ -1374,7 +1386,7 @@ struct MagnetometerOffset auto as_tuple() const { - return std::make_tuple(hard_iron,valid_flags); + return std::make_tuple(hard_iron[0],hard_iron[1],hard_iron[2],valid_flags); } float hard_iron[3] = {0}; ///< (x,y,z) [Gauss] @@ -1403,7 +1415,7 @@ struct MagnetometerMatrix auto as_tuple() const { - return std::make_tuple(soft_iron,valid_flags); + return std::make_tuple(soft_iron[0],soft_iron[1],soft_iron[2],soft_iron[3],soft_iron[4],soft_iron[5],soft_iron[6],soft_iron[7],soft_iron[8],valid_flags); } float soft_iron[9] = {0}; ///< Row-major [dimensionless] @@ -1430,7 +1442,7 @@ struct MagnetometerOffsetUncertainty auto as_tuple() const { - return std::make_tuple(hard_iron_uncertainty,valid_flags); + return std::make_tuple(hard_iron_uncertainty[0],hard_iron_uncertainty[1],hard_iron_uncertainty[2],valid_flags); } float hard_iron_uncertainty[3] = {0}; ///< (x,y,z) [Gauss] @@ -1457,7 +1469,7 @@ struct MagnetometerMatrixUncertainty auto as_tuple() const { - return std::make_tuple(soft_iron_uncertainty,valid_flags); + return std::make_tuple(soft_iron_uncertainty[0],soft_iron_uncertainty[1],soft_iron_uncertainty[2],soft_iron_uncertainty[3],soft_iron_uncertainty[4],soft_iron_uncertainty[5],soft_iron_uncertainty[6],soft_iron_uncertainty[7],soft_iron_uncertainty[8],valid_flags); } float soft_iron_uncertainty[9] = {0}; ///< Row-major [dimensionless] @@ -1483,7 +1495,7 @@ struct MagnetometerCovarianceMatrix auto as_tuple() const { - return std::make_tuple(covariance,valid_flags); + return std::make_tuple(covariance[0],covariance[1],covariance[2],covariance[3],covariance[4],covariance[5],covariance[6],covariance[7],covariance[8],valid_flags); } float covariance[9] = {0}; @@ -1510,7 +1522,7 @@ struct MagnetometerResidualVector auto as_tuple() const { - return std::make_tuple(residual,valid_flags); + return std::make_tuple(residual[0],residual[1],residual[2],valid_flags); } float residual[3] = {0}; ///< (x,y,z) [Gauss] @@ -1652,7 +1664,7 @@ struct HeadAidStatus auto as_tuple() const { - return std::make_tuple(time_of_week,type,reserved); + return std::make_tuple(time_of_week,type,reserved[0],reserved[1]); } enum class HeadingAidType : uint8_t @@ -1686,7 +1698,7 @@ struct RelPosNed auto as_tuple() const { - return std::make_tuple(relative_position,valid_flags); + return std::make_tuple(relative_position[0],relative_position[1],relative_position[2],valid_flags); } double relative_position[3] = {0}; ///< [meters, NED] @@ -1713,7 +1725,7 @@ struct EcefPos auto as_tuple() const { - return std::make_tuple(position_ecef,valid_flags); + return std::make_tuple(position_ecef[0],position_ecef[1],position_ecef[2],valid_flags); } double position_ecef[3] = {0}; ///< [meters, ECEF] @@ -1740,7 +1752,7 @@ struct EcefVel auto as_tuple() const { - return std::make_tuple(velocity_ecef,valid_flags); + return std::make_tuple(velocity_ecef[0],velocity_ecef[1],velocity_ecef[2],valid_flags); } float velocity_ecef[3] = {0}; ///< [meters/second, ECEF] @@ -1767,7 +1779,7 @@ struct EcefPosUncertainty auto as_tuple() const { - return std::make_tuple(pos_uncertainty,valid_flags); + return std::make_tuple(pos_uncertainty[0],pos_uncertainty[1],pos_uncertainty[2],valid_flags); } float pos_uncertainty[3] = {0}; ///< [meters] @@ -1794,7 +1806,7 @@ struct EcefVelUncertainty auto as_tuple() const { - return std::make_tuple(vel_uncertainty,valid_flags); + return std::make_tuple(vel_uncertainty[0],vel_uncertainty[1],vel_uncertainty[2],valid_flags); } float vel_uncertainty[3] = {0}; ///< [meters/second] @@ -1922,6 +1934,7 @@ struct GnssDualAntennaStatus RCV_1_DATA_VALID = 0x0001, ///< RCV_2_DATA_VALID = 0x0002, ///< ANTENNA_OFFSETS_VALID = 0x0004, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -1939,6 +1952,9 @@ struct GnssDualAntennaStatus void rcv2DataValid(bool val) { if(val) value |= RCV_2_DATA_VALID; else value &= ~RCV_2_DATA_VALID; } bool antennaOffsetsValid() const { return (value & ANTENNA_OFFSETS_VALID) > 0; } void antennaOffsetsValid(bool val) { if(val) value |= ANTENNA_OFFSETS_VALID; else value &= ~ANTENNA_OFFSETS_VALID; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float time_of_week = 0; ///< Last dual-antenna GNSS aiding measurement time of week [seconds] diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index d62f57310..9bab81253 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -185,6 +185,7 @@ static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_HORIZONTAL_ACCURACY = 0x0008; ///< static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_VERTICAL_ACCURACY = 0x0010; ///< static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_FLAGS = 0x001F; ///< +static const mip_gnss_pos_llh_data_valid_flags MIP_GNSS_POS_LLH_DATA_VALID_FLAGS_ALL = 0x001F; struct mip_gnss_pos_llh_data { @@ -218,6 +219,7 @@ static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLA static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_POSITION = 0x0001; ///< static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_POSITION_ACCURACY = 0x0002; ///< static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_pos_ecef_data { @@ -251,6 +253,7 @@ static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_SPEED_ACCURACY = 0x0010; ///< static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_HEADING_ACCURACY = 0x0020; ///< static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_vel_ned_data { @@ -284,6 +287,7 @@ static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLA static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_VELOCITY = 0x0001; ///< static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_VELOCITY_ACCURACY = 0x0002; ///< static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_vel_ecef_data { @@ -318,6 +322,7 @@ static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_TDOP = static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_NDOP = 0x0020; ///< static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_EDOP = 0x0040; ///< static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_FLAGS = 0x007F; ///< +static const mip_gnss_dop_data_valid_flags MIP_GNSS_DOP_DATA_VALID_FLAGS_ALL = 0x007F; struct mip_gnss_dop_data { @@ -352,6 +357,7 @@ static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLA static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_GNSS_DATE_TIME = 0x0001; ///< static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_LEAP_SECONDS_KNOWN = 0x0002; ///< static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_utc_time_data { @@ -386,6 +392,7 @@ static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLA static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_TOW = 0x0001; ///< static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_WEEK_NUMBER = 0x0002; ///< static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_gps_time_data_valid_flags MIP_GNSS_GPS_TIME_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_gps_time_data { @@ -416,6 +423,7 @@ static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_DRIFT = 0x0002; ///< static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_ACCURACY_ESTIMATE = 0x0004; ///< static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_clock_info_data_valid_flags MIP_GNSS_CLOCK_INFO_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_clock_info_data { @@ -454,6 +462,7 @@ typedef uint16_t mip_gnss_fix_info_data_fix_flags; static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_NONE = 0x0000; static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_SBAS_USED = 0x0001; ///< static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_DGNSS_USED = 0x0002; ///< +static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_ALL = 0x0003; typedef uint16_t mip_gnss_fix_info_data_valid_flags; static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -461,6 +470,7 @@ static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLA static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_NUM_SV = 0x0002; ///< static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_FIX_FLAGS = 0x0004; ///< static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_fix_info_data_valid_flags MIP_GNSS_FIX_INFO_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_fix_info_data { @@ -498,6 +508,7 @@ typedef uint16_t mip_gnss_sv_info_data_svflags; static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_NONE = 0x0000; static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_USED_FOR_NAVIGATION = 0x0001; ///< static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_HEALTHY = 0x0002; ///< +static const mip_gnss_sv_info_data_svflags MIP_GNSS_SV_INFO_DATA_SVFLAGS_ALL = 0x0003; typedef uint16_t mip_gnss_sv_info_data_valid_flags; static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -508,6 +519,7 @@ static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_ELEVATION = 0x0010; ///< static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_SV_FLAGS = 0x0020; ///< static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_sv_info_data_valid_flags MIP_GNSS_SV_INFO_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_sv_info_data { @@ -562,6 +574,7 @@ static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_F static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_ANTENNA_STATE = 0x0002; ///< static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_ANTENNA_POWER = 0x0004; ///< static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_hw_status_data_valid_flags MIP_GNSS_HW_STATUS_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_hw_status_data { @@ -615,6 +628,7 @@ static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_F static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_BASE_STATION_STATUS = 0x0004; ///< static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_NUM_CHANNELS = 0x0008; ///< static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_dgps_info_data_valid_flags MIP_GNSS_DGPS_INFO_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_dgps_info_data { @@ -650,6 +664,7 @@ static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_V static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_RANGE_CORRECTION = 0x0004; ///< static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_RANGE_RATE_CORRECTION = 0x0008; ///< static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_dgps_channel_data_valid_flags MIP_GNSS_DGPS_CHANNEL_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_dgps_channel_data { @@ -685,6 +700,7 @@ static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_V static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_BIAS_ACCURACY = 0x0004; ///< static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_DRIFT_ACCURACY = 0x0008; ///< static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_clock_info_2_data_valid_flags MIP_GNSS_CLOCK_INFO_2_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_clock_info_2_data { @@ -714,6 +730,7 @@ void extract_mip_gnss_clock_info_2_data_valid_flags(struct mip_serializer* seria typedef uint16_t mip_gnss_gps_leap_seconds_data_valid_flags; static const mip_gnss_gps_leap_seconds_data_valid_flags MIP_GNSS_GPS_LEAP_SECONDS_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_gnss_gps_leap_seconds_data_valid_flags MIP_GNSS_GPS_LEAP_SECONDS_DATA_VALID_FLAGS_LEAP_SECONDS = 0x0002; ///< +static const mip_gnss_gps_leap_seconds_data_valid_flags MIP_GNSS_GPS_LEAP_SECONDS_DATA_VALID_FLAGS_ALL = 0x0002; struct mip_gnss_gps_leap_seconds_data { @@ -743,6 +760,7 @@ static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_ST static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_CORRECTIONS_AVAILABLE = 0x02; ///< static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_INTEGRITY_AVAILABLE = 0x04; ///< static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_TEST_MODE = 0x08; ///< +static const mip_gnss_sbas_info_data_sbas_status MIP_GNSS_SBAS_INFO_DATA_SBAS_STATUS_ALL = 0x0F; typedef uint16_t mip_gnss_sbas_info_data_valid_flags; static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -753,6 +771,7 @@ static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_F static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_COUNT = 0x0010; ///< static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_SBAS_STATUS = 0x0020; ///< static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_sbas_info_data_valid_flags MIP_GNSS_SBAS_INFO_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_sbas_info_data { @@ -812,6 +831,7 @@ static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_ static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_PSEUDORANGE_CORRECTION = 0x0002; ///< static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_IONO_CORRECTION = 0x0004; ///< static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_sbas_correction_data_valid_flags MIP_GNSS_SBAS_CORRECTION_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_sbas_correction_data { @@ -867,6 +887,7 @@ static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETE static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_JAMMING_STATE = 0x0002; ///< static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_SPOOFING_STATE = 0x0004; ///< static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_FLAGS = 0x0007; ///< +static const mip_gnss_rf_error_detection_data_valid_flags MIP_GNSS_RF_ERROR_DETECTION_DATA_VALID_FLAGS_ALL = 0x0007; struct mip_gnss_rf_error_detection_data { @@ -915,6 +936,7 @@ static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATI static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_QUARTER_CYCLE_BIT1 = 0x0040; ///< static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_QUARTER_CYCLE_BIT2 = 0x0080; ///< static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_QUARTER_CYCLE_BITS = 0x00C0; ///< +static const mip_gnss_base_station_info_data_indicator_flags MIP_GNSS_BASE_STATION_INFO_DATA_INDICATOR_FLAGS_ALL = 0x00FF; typedef uint16_t mip_gnss_base_station_info_data_valid_flags; static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_NONE = 0x0000; @@ -925,6 +947,7 @@ static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_I static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_STATION_ID = 0x0010; ///< static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_INDICATORS = 0x0020; ///< static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_FLAGS = 0x003F; ///< +static const mip_gnss_base_station_info_data_valid_flags MIP_GNSS_BASE_STATION_INFO_DATA_VALID_FLAGS_ALL = 0x003F; struct mip_gnss_base_station_info_data { @@ -966,6 +989,7 @@ static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRE static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_GALILEO_LATENCY = 0x0040; ///< static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_BEIDOU_LATENCY = 0x0080; ///< static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_FLAGS = 0x00FF; ///< +static const mip_gnss_rtk_corrections_status_data_valid_flags MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_VALID_FLAGS_ALL = 0x00FF; typedef uint16_t mip_gnss_rtk_corrections_status_data_epoch_status; static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_NONE = 0x0000; @@ -978,6 +1002,7 @@ static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORR static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_USING_GPS_MSM_MESSAGES = 0x0040; ///< Using MSM messages for GPS corrections instead of RTCM messages 1001-1004 static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_USING_GLONASS_MSM_MESSAGES = 0x0080; ///< Using MSM messages for GLONASS corrections instead of RTCM messages 1009-1012 static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_DONGLE_STATUS_READ_FAILED = 0x0100; ///< A read of the dongle status was attempted, but failed +static const mip_gnss_rtk_corrections_status_data_epoch_status MIP_GNSS_RTK_CORRECTIONS_STATUS_DATA_EPOCH_STATUS_ALL = 0x01FF; struct mip_gnss_rtk_corrections_status_data { @@ -1022,6 +1047,7 @@ static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATU static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_AZIMUTH = 0x0020; ///< static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_HEALTH = 0x0040; ///< static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_FLAGS = 0x007F; ///< +static const mip_gnss_satellite_status_data_valid_flags MIP_GNSS_SATELLITE_STATUS_DATA_VALID_FLAGS_ALL = 0x007F; struct mip_gnss_satellite_status_data { @@ -1080,6 +1106,7 @@ static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_CARRIER static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_DOPPLER_UNCERTAINTY = 0x4000; ///< static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_LOCK_TIME = 0x8000; ///< static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_FLAGS = 0xFFFF; ///< +static const mip_gnss_raw_data_valid_flags MIP_GNSS_RAW_DATA_VALID_FLAGS_ALL = 0xFFFF; struct mip_gnss_raw_data { @@ -1128,6 +1155,7 @@ static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_EPHEMERIS = 0x0001; ///< static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_MODERN_DATA = 0x0002; ///< static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_gps_ephemeris_data_valid_flags MIP_GNSS_GPS_EPHEMERIS_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_gps_ephemeris_data { @@ -1188,6 +1216,7 @@ static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEME static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_EPHEMERIS = 0x0001; ///< static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_MODERN_DATA = 0x0002; ///< static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_FLAGS = 0x0003; ///< +static const mip_gnss_galileo_ephemeris_data_valid_flags MIP_GNSS_GALILEO_EPHEMERIS_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_gnss_galileo_ephemeris_data { @@ -1247,6 +1276,7 @@ typedef uint16_t mip_gnss_glo_ephemeris_data_valid_flags; static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_EPHEMERIS = 0x0001; ///< static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_FLAGS = 0x0001; ///< +static const mip_gnss_glo_ephemeris_data_valid_flags MIP_GNSS_GLO_EPHEMERIS_DATA_VALID_FLAGS_ALL = 0x0001; struct mip_gnss_glo_ephemeris_data { @@ -1300,6 +1330,7 @@ static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_ALPHA = 0x0004; ///< static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_BETA = 0x0008; ///< static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_gps_iono_corr_data_valid_flags MIP_GNSS_GPS_IONO_CORR_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_gps_iono_corr_data { @@ -1333,6 +1364,7 @@ static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_C static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_ALPHA = 0x0004; ///< static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_DISTURBANCE_FLAGS = 0x0008; ///< static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_FLAGS = 0x000F; ///< +static const mip_gnss_galileo_iono_corr_data_valid_flags MIP_GNSS_GALILEO_IONO_CORR_DATA_VALID_FLAGS_ALL = 0x000F; struct mip_gnss_galileo_iono_corr_data { diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index a8a465bd4..cf38aef40 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -196,6 +196,7 @@ struct PosLlh HORIZONTAL_ACCURACY = 0x0008, ///< VERTICAL_ACCURACY = 0x0010, ///< FLAGS = 0x001F, ///< + ALL = 0x001F, }; uint16_t value = NONE; @@ -219,6 +220,9 @@ struct PosLlh void verticalAccuracy(bool val) { if(val) value |= VERTICAL_ACCURACY; else value &= ~VERTICAL_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double latitude = 0; ///< [degrees] @@ -250,7 +254,7 @@ struct PosEcef auto as_tuple() const { - return std::make_tuple(x,x_accuracy,valid_flags); + return std::make_tuple(x[0],x[1],x[2],x_accuracy,valid_flags); } struct ValidFlags : Bitfield @@ -261,6 +265,7 @@ struct PosEcef POSITION = 0x0001, ///< POSITION_ACCURACY = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -278,6 +283,9 @@ struct PosEcef void positionAccuracy(bool val) { if(val) value |= POSITION_ACCURACY; else value &= ~POSITION_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double x[3] = {0}; ///< [meters] @@ -305,7 +313,7 @@ struct VelNed auto as_tuple() const { - return std::make_tuple(v,speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); + return std::make_tuple(v[0],v[1],v[2],speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); } struct ValidFlags : Bitfield @@ -320,6 +328,7 @@ struct VelNed SPEED_ACCURACY = 0x0010, ///< HEADING_ACCURACY = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -345,6 +354,9 @@ struct VelNed void headingAccuracy(bool val) { if(val) value |= HEADING_ACCURACY; else value &= ~HEADING_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float v[3] = {0}; ///< [meters/second] @@ -376,7 +388,7 @@ struct VelEcef auto as_tuple() const { - return std::make_tuple(v,v_accuracy,valid_flags); + return std::make_tuple(v[0],v[1],v[2],v_accuracy,valid_flags); } struct ValidFlags : Bitfield @@ -387,6 +399,7 @@ struct VelEcef VELOCITY = 0x0001, ///< VELOCITY_ACCURACY = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -404,6 +417,9 @@ struct VelEcef void velocityAccuracy(bool val) { if(val) value |= VELOCITY_ACCURACY; else value &= ~VELOCITY_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float v[3] = {0}; ///< [meters/second] @@ -447,6 +463,7 @@ struct Dop NDOP = 0x0020, ///< EDOP = 0x0040, ///< FLAGS = 0x007F, ///< + ALL = 0x007F, }; uint16_t value = NONE; @@ -474,6 +491,9 @@ struct Dop void edop(bool val) { if(val) value |= EDOP; else value &= ~EDOP; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; float gdop = 0; ///< Geometric DOP @@ -517,6 +537,7 @@ struct UtcTime GNSS_DATE_TIME = 0x0001, ///< LEAP_SECONDS_KNOWN = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -534,6 +555,9 @@ struct UtcTime void leapSecondsKnown(bool val) { if(val) value |= LEAP_SECONDS_KNOWN; else value &= ~LEAP_SECONDS_KNOWN; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint16_t year = 0; @@ -577,6 +601,7 @@ struct GpsTime TOW = 0x0001, ///< WEEK_NUMBER = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -594,6 +619,9 @@ struct GpsTime void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double tow = 0; ///< GPS Time of week [seconds] @@ -633,6 +661,7 @@ struct ClockInfo DRIFT = 0x0002, ///< ACCURACY_ESTIMATE = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -652,6 +681,9 @@ struct ClockInfo void accuracyEstimate(bool val) { if(val) value |= ACCURACY_ESTIMATE; else value &= ~ACCURACY_ESTIMATE; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double bias = 0; ///< [seconds] @@ -701,6 +733,7 @@ struct FixInfo NONE = 0x0000, SBAS_USED = 0x0001, ///< DGNSS_USED = 0x0002, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -716,6 +749,9 @@ struct FixInfo void sbasUsed(bool val) { if(val) value |= SBAS_USED; else value &= ~SBAS_USED; } bool dgnssUsed() const { return (value & DGNSS_USED) > 0; } void dgnssUsed(bool val) { if(val) value |= DGNSS_USED; else value &= ~DGNSS_USED; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -727,6 +763,7 @@ struct FixInfo NUM_SV = 0x0002, ///< FIX_FLAGS = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -746,6 +783,9 @@ struct FixInfo void fixFlags(bool val) { if(val) value |= FIX_FLAGS; else value &= ~FIX_FLAGS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; FixType fix_type = static_cast(0); @@ -786,6 +826,7 @@ struct SvInfo NONE = 0x0000, USED_FOR_NAVIGATION = 0x0001, ///< HEALTHY = 0x0002, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -801,6 +842,9 @@ struct SvInfo void usedForNavigation(bool val) { if(val) value |= USED_FOR_NAVIGATION; else value &= ~USED_FOR_NAVIGATION; } bool healthy() const { return (value & HEALTHY) > 0; } void healthy(bool val) { if(val) value |= HEALTHY; else value &= ~HEALTHY; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -815,6 +859,7 @@ struct SvInfo ELEVATION = 0x0010, ///< SV_FLAGS = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -840,6 +885,9 @@ struct SvInfo void svFlags(bool val) { if(val) value |= SV_FLAGS; else value &= ~SV_FLAGS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t channel = 0; ///< Receiver channel number @@ -906,6 +954,7 @@ struct HwStatus ANTENNA_STATE = 0x0002, ///< ANTENNA_POWER = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -925,6 +974,9 @@ struct HwStatus void antennaPower(bool val) { if(val) value |= ANTENNA_POWER; else value &= ~ANTENNA_POWER; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; ReceiverState receiver_state = static_cast(0); @@ -978,6 +1030,7 @@ struct DgpsInfo BASE_STATION_STATUS = 0x0004, ///< NUM_CHANNELS = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -999,6 +1052,9 @@ struct DgpsInfo void numChannels(bool val) { if(val) value |= NUM_CHANNELS; else value &= ~NUM_CHANNELS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t sv_id = 0; @@ -1043,6 +1099,7 @@ struct DgpsChannel RANGE_CORRECTION = 0x0004, ///< RANGE_RATE_CORRECTION = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -1064,6 +1121,9 @@ struct DgpsChannel void rangeRateCorrection(bool val) { if(val) value |= RANGE_RATE_CORRECTION; else value &= ~RANGE_RATE_CORRECTION; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t sv_id = 0; @@ -1108,6 +1168,7 @@ struct ClockInfo2 BIAS_ACCURACY = 0x0004, ///< DRIFT_ACCURACY = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -1129,6 +1190,9 @@ struct ClockInfo2 void driftAccuracy(bool val) { if(val) value |= DRIFT_ACCURACY; else value &= ~DRIFT_ACCURACY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double bias = 0; @@ -1167,6 +1231,7 @@ struct GpsLeapSeconds { NONE = 0x0000, LEAP_SECONDS = 0x0002, ///< + ALL = 0x0002, }; uint16_t value = NONE; @@ -1180,6 +1245,9 @@ struct GpsLeapSeconds bool leapSeconds() const { return (value & LEAP_SECONDS) > 0; } void leapSeconds(bool val) { if(val) value |= LEAP_SECONDS; else value &= ~LEAP_SECONDS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t leap_seconds = 0; ///< [s] @@ -1218,6 +1286,7 @@ struct SbasInfo CORRECTIONS_AVAILABLE = 0x02, ///< INTEGRITY_AVAILABLE = 0x04, ///< TEST_MODE = 0x08, ///< + ALL = 0x0F, }; uint8_t value = NONE; @@ -1237,6 +1306,9 @@ struct SbasInfo void integrityAvailable(bool val) { if(val) value |= INTEGRITY_AVAILABLE; else value &= ~INTEGRITY_AVAILABLE; } bool testMode() const { return (value & TEST_MODE) > 0; } void testMode(bool val) { if(val) value |= TEST_MODE; else value &= ~TEST_MODE; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -1251,6 +1323,7 @@ struct SbasInfo COUNT = 0x0010, ///< SBAS_STATUS = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -1276,6 +1349,9 @@ struct SbasInfo void sbasStatus(bool val) { if(val) value |= SBAS_STATUS; else value &= ~SBAS_STATUS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -1341,6 +1417,7 @@ struct SbasCorrection PSEUDORANGE_CORRECTION = 0x0002, ///< IONO_CORRECTION = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -1360,6 +1437,9 @@ struct SbasCorrection void ionoCorrection(bool val) { if(val) value |= IONO_CORRECTION; else value &= ~IONO_CORRECTION; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1430,6 +1510,7 @@ struct RfErrorDetection JAMMING_STATE = 0x0002, ///< SPOOFING_STATE = 0x0004, ///< FLAGS = 0x0007, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -1449,6 +1530,9 @@ struct RfErrorDetection void spoofingState(bool val) { if(val) value |= SPOOFING_STATE; else value &= ~SPOOFING_STATE; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; RFBand rf_band = static_cast(0); ///< RF Band of the reported information @@ -1480,7 +1564,7 @@ struct BaseStationInfo auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,ecef_pos,height,station_id,indicators,valid_flags); + return std::make_tuple(time_of_week,week_number,ecef_pos[0],ecef_pos[1],ecef_pos[2],height,station_id,indicators,valid_flags); } struct IndicatorFlags : Bitfield @@ -1497,6 +1581,7 @@ struct BaseStationInfo QUARTER_CYCLE_BIT1 = 0x0040, ///< QUARTER_CYCLE_BIT2 = 0x0080, ///< QUARTER_CYCLE_BITS = 0x00C0, ///< + ALL = 0x00FF, }; uint16_t value = NONE; @@ -1526,6 +1611,9 @@ struct BaseStationInfo void quarterCycleBit2(bool val) { if(val) value |= QUARTER_CYCLE_BIT2; else value &= ~QUARTER_CYCLE_BIT2; } uint16_t quarterCycleBits() const { return (value & QUARTER_CYCLE_BITS) >> 6; } void quarterCycleBits(uint16_t val) { value = (value & ~QUARTER_CYCLE_BITS) | (val << 6); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct ValidFlags : Bitfield @@ -1540,6 +1628,7 @@ struct BaseStationInfo STATION_ID = 0x0010, ///< INDICATORS = 0x0020, ///< FLAGS = 0x003F, ///< + ALL = 0x003F, }; uint16_t value = NONE; @@ -1565,6 +1654,9 @@ struct BaseStationInfo void indicators(bool val) { if(val) value |= INDICATORS; else value &= ~INDICATORS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week the message was received [seconds] @@ -1595,7 +1687,7 @@ struct RtkCorrectionsStatus auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved,valid_flags); + return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved[0],reserved[1],reserved[2],reserved[3],valid_flags); } struct ValidFlags : Bitfield @@ -1612,6 +1704,7 @@ struct RtkCorrectionsStatus GALILEO_LATENCY = 0x0040, ///< BEIDOU_LATENCY = 0x0080, ///< FLAGS = 0x00FF, ///< + ALL = 0x00FF, }; uint16_t value = NONE; @@ -1641,6 +1734,9 @@ struct RtkCorrectionsStatus void beidouLatency(bool val) { if(val) value |= BEIDOU_LATENCY; else value &= ~BEIDOU_LATENCY; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; struct EpochStatus : Bitfield @@ -1657,6 +1753,7 @@ struct RtkCorrectionsStatus USING_GPS_MSM_MESSAGES = 0x0040, ///< Using MSM messages for GPS corrections instead of RTCM messages 1001-1004 USING_GLONASS_MSM_MESSAGES = 0x0080, ///< Using MSM messages for GLONASS corrections instead of RTCM messages 1009-1012 DONGLE_STATUS_READ_FAILED = 0x0100, ///< A read of the dongle status was attempted, but failed + ALL = 0x01FF, }; uint16_t value = NONE; @@ -1686,6 +1783,9 @@ struct RtkCorrectionsStatus void usingGlonassMsmMessages(bool val) { if(val) value |= USING_GLONASS_MSM_MESSAGES; else value &= ~USING_GLONASS_MSM_MESSAGES; } bool dongleStatusReadFailed() const { return (value & DONGLE_STATUS_READ_FAILED) > 0; } void dongleStatusReadFailed(bool val) { if(val) value |= DONGLE_STATUS_READ_FAILED; else value &= ~DONGLE_STATUS_READ_FAILED; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -1736,6 +1836,7 @@ struct SatelliteStatus AZIMUTH = 0x0020, ///< HEALTH = 0x0040, ///< FLAGS = 0x007F, ///< + ALL = 0x007F, }; uint16_t value = NONE; @@ -1763,6 +1864,9 @@ struct SatelliteStatus void health(bool val) { if(val) value |= HEALTH; else value &= ~HEALTH; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1832,6 +1936,7 @@ struct Raw DOPPLER_UNCERTAINTY = 0x4000, ///< LOCK_TIME = 0x8000, ///< FLAGS = 0xFFFF, ///< + ALL = 0xFFFF, }; uint16_t value = NONE; @@ -1877,6 +1982,9 @@ struct Raw void lockTime(bool val) { if(val) value |= LOCK_TIME; else value &= ~LOCK_TIME; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -1931,6 +2039,7 @@ struct GpsEphemeris EPHEMERIS = 0x0001, ///< MODERN_DATA = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -1948,6 +2057,9 @@ struct GpsEphemeris void modernData(bool val) { if(val) value |= MODERN_DATA; else value &= ~MODERN_DATA; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -2017,6 +2129,7 @@ struct GalileoEphemeris EPHEMERIS = 0x0001, ///< MODERN_DATA = 0x0002, ///< FLAGS = 0x0003, ///< + ALL = 0x0003, }; uint16_t value = NONE; @@ -2034,6 +2147,9 @@ struct GalileoEphemeris void modernData(bool val) { if(val) value |= MODERN_DATA; else value &= ~MODERN_DATA; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -2092,7 +2208,7 @@ struct GloEphemeris auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x,v,a,health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x[0],x[1],x[2],v[0],v[1],v[2],a[0],a[1],a[2],health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); } struct ValidFlags : Bitfield @@ -2102,6 +2218,7 @@ struct GloEphemeris NONE = 0x0000, EPHEMERIS = 0x0001, ///< FLAGS = 0x0001, ///< + ALL = 0x0001, }; uint16_t value = NONE; @@ -2117,6 +2234,9 @@ struct GloEphemeris void ephemeris(bool val) { if(val) value |= EPHEMERIS; else value &= ~EPHEMERIS; } bool flags() const { return (value & FLAGS) > 0; } void flags(bool val) { if(val) value |= FLAGS; else value &= ~FLAGS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint8_t index = 0; ///< Index of this field in this epoch. @@ -2166,7 +2286,7 @@ struct GpsIonoCorr auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,alpha,beta,valid_flags); + return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],alpha[3],beta[0],beta[1],beta[2],beta[3],valid_flags); } struct ValidFlags : Bitfield @@ -2179,6 +2299,7 @@ struct GpsIonoCorr ALPHA = 0x0004, ///< BETA = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -2200,6 +2321,9 @@ struct GpsIonoCorr void beta(bool val) { if(val) value |= BETA; else value &= ~BETA; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] @@ -2229,7 +2353,7 @@ struct GalileoIonoCorr auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,alpha,disturbance_flags,valid_flags); + return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],disturbance_flags,valid_flags); } struct ValidFlags : Bitfield @@ -2242,6 +2366,7 @@ struct GalileoIonoCorr ALPHA = 0x0004, ///< DISTURBANCE_FLAGS = 0x0008, ///< FLAGS = 0x000F, ///< + ALL = 0x000F, }; uint16_t value = NONE; @@ -2263,6 +2388,9 @@ struct GalileoIonoCorr void disturbanceFlags(bool val) { if(val) value |= DISTURBANCE_FLAGS; else value &= ~DISTURBANCE_FLAGS; } uint16_t flags() const { return (value & FLAGS) >> 0; } void flags(uint16_t val) { value = (value & ~FLAGS) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double time_of_week = 0; ///< GPS Time of week [seconds] diff --git a/src/mip/definitions/data_sensor.h b/src/mip/definitions/data_sensor.h index ae2cee70a..40267b92a 100644 --- a/src/mip/definitions/data_sensor.h +++ b/src/mip/definitions/data_sensor.h @@ -426,6 +426,7 @@ static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_ static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_TIME_INITIALIZED = 0x0004; ///< True if the time has ever been set. static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_TOW_VALID = 0x0008; ///< True if the time of week is valid. static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_WEEK_NUMBER_VALID = 0x0010; ///< True if the week number is valid. +static const mip_sensor_gps_timestamp_data_valid_flags MIP_SENSOR_GPS_TIMESTAMP_DATA_VALID_FLAGS_ALL = 0x001F; struct mip_sensor_gps_timestamp_data { @@ -530,6 +531,7 @@ static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_MAG_Y = 0x0200; ///< static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_MAG_Z = 0x0400; ///< static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_PRESS = 0x1000; ///< +static const mip_sensor_overrange_status_data_status MIP_SENSOR_OVERRANGE_STATUS_DATA_STATUS_ALL = 0x1777; struct mip_sensor_overrange_status_data { diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 0621df768..7ed980dd1 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -85,7 +85,7 @@ struct RawAccel auto as_tuple() const { - return std::make_tuple(raw_accel); + return std::make_tuple(raw_accel[0],raw_accel[1],raw_accel[2]); } float raw_accel[3] = {0}; ///< Native sensor counts @@ -112,7 +112,7 @@ struct RawGyro auto as_tuple() const { - return std::make_tuple(raw_gyro); + return std::make_tuple(raw_gyro[0],raw_gyro[1],raw_gyro[2]); } float raw_gyro[3] = {0}; ///< Native sensor counts @@ -139,7 +139,7 @@ struct RawMag auto as_tuple() const { - return std::make_tuple(raw_mag); + return std::make_tuple(raw_mag[0],raw_mag[1],raw_mag[2]); } float raw_mag[3] = {0}; ///< Native sensor counts @@ -193,7 +193,7 @@ struct ScaledAccel auto as_tuple() const { - return std::make_tuple(scaled_accel); + return std::make_tuple(scaled_accel[0],scaled_accel[1],scaled_accel[2]); } float scaled_accel[3] = {0}; ///< (x, y, z)[g] @@ -220,7 +220,7 @@ struct ScaledGyro auto as_tuple() const { - return std::make_tuple(scaled_gyro); + return std::make_tuple(scaled_gyro[0],scaled_gyro[1],scaled_gyro[2]); } float scaled_gyro[3] = {0}; ///< (x, y, z) [radians/second] @@ -247,7 +247,7 @@ struct ScaledMag auto as_tuple() const { - return std::make_tuple(scaled_mag); + return std::make_tuple(scaled_mag[0],scaled_mag[1],scaled_mag[2]); } float scaled_mag[3] = {0}; ///< (x, y, z) [Gauss] @@ -300,7 +300,7 @@ struct DeltaTheta auto as_tuple() const { - return std::make_tuple(delta_theta); + return std::make_tuple(delta_theta[0],delta_theta[1],delta_theta[2]); } float delta_theta[3] = {0}; ///< (x, y, z) [radians] @@ -327,7 +327,7 @@ struct DeltaVelocity auto as_tuple() const { - return std::make_tuple(delta_velocity); + return std::make_tuple(delta_velocity[0],delta_velocity[1],delta_velocity[2]); } float delta_velocity[3] = {0}; ///< (x, y, z) [g*sec] @@ -363,7 +363,7 @@ struct CompOrientationMatrix auto as_tuple() const { - return std::make_tuple(m); + return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } float m[9] = {0}; ///< Matrix elements in row-major order. @@ -397,7 +397,7 @@ struct CompQuaternion auto as_tuple() const { - return std::make_tuple(q); + return std::make_tuple(q[0],q[1],q[2],q[3]); } float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND @@ -452,7 +452,7 @@ struct CompOrientationUpdateMatrix auto as_tuple() const { - return std::make_tuple(m); + return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } float m[9] = {0}; @@ -478,7 +478,7 @@ struct OrientationRawTemp auto as_tuple() const { - return std::make_tuple(raw_temp); + return std::make_tuple(raw_temp[0],raw_temp[1],raw_temp[2],raw_temp[3]); } uint16_t raw_temp[4] = {0}; @@ -576,6 +576,7 @@ struct GpsTimestamp TIME_INITIALIZED = 0x0004, ///< True if the time has ever been set. TOW_VALID = 0x0008, ///< True if the time of week is valid. WEEK_NUMBER_VALID = 0x0010, ///< True if the week number is valid. + ALL = 0x001F, }; uint16_t value = NONE; @@ -597,6 +598,9 @@ struct GpsTimestamp void towValid(bool val) { if(val) value |= TOW_VALID; else value &= ~TOW_VALID; } bool weekNumberValid() const { return (value & WEEK_NUMBER_VALID) > 0; } void weekNumberValid(bool val) { if(val) value |= WEEK_NUMBER_VALID; else value &= ~WEEK_NUMBER_VALID; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double tow = 0; ///< GPS Time of Week [seconds] @@ -662,7 +666,7 @@ struct UpVector auto as_tuple() const { - return std::make_tuple(up); + return std::make_tuple(up[0],up[1],up[2]); } float up[3] = {0}; ///< [Gs] @@ -691,7 +695,7 @@ struct NorthVector auto as_tuple() const { - return std::make_tuple(north); + return std::make_tuple(north[0],north[1],north[2]); } float north[3] = {0}; ///< [Gauss] @@ -734,6 +738,7 @@ struct OverrangeStatus MAG_Y = 0x0200, ///< MAG_Z = 0x0400, ///< PRESS = 0x1000, ///< + ALL = 0x1777, }; uint16_t value = NONE; @@ -765,6 +770,9 @@ struct OverrangeStatus void magZ(bool val) { if(val) value |= MAG_Z; else value &= ~MAG_Z; } bool press() const { return (value & PRESS) > 0; } void press(bool val) { if(val) value |= PRESS; else value &= ~PRESS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; Status status; diff --git a/src/mip/definitions/data_shared.h b/src/mip/definitions/data_shared.h index 8f7c4ead6..90f2dbc6e 100644 --- a/src/mip/definitions/data_shared.h +++ b/src/mip/definitions/data_shared.h @@ -133,6 +133,7 @@ static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_ static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_TOW = 0x0001; ///< Whole number seconds TOW has been set static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_WEEK_NUMBER = 0x0002; ///< Week number has been set static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_TIME_VALID = 0x0003; ///< Both TOW and Week Number have been set +static const mip_shared_gps_timestamp_data_valid_flags MIP_SHARED_GPS_TIMESTAMP_DATA_VALID_FLAGS_ALL = 0x0003; struct mip_shared_gps_timestamp_data { @@ -247,6 +248,7 @@ bool extract_mip_shared_reference_time_delta_data_from_field(const struct mip_fi typedef uint16_t mip_shared_external_timestamp_data_valid_flags; static const mip_shared_external_timestamp_data_valid_flags MIP_SHARED_EXTERNAL_TIMESTAMP_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_shared_external_timestamp_data_valid_flags MIP_SHARED_EXTERNAL_TIMESTAMP_DATA_VALID_FLAGS_NANOSECONDS = 0x0001; ///< +static const mip_shared_external_timestamp_data_valid_flags MIP_SHARED_EXTERNAL_TIMESTAMP_DATA_VALID_FLAGS_ALL = 0x0001; struct mip_shared_external_timestamp_data { @@ -285,6 +287,7 @@ void extract_mip_shared_external_timestamp_data_valid_flags(struct mip_serialize typedef uint16_t mip_shared_external_time_delta_data_valid_flags; static const mip_shared_external_time_delta_data_valid_flags MIP_SHARED_EXTERNAL_TIME_DELTA_DATA_VALID_FLAGS_NONE = 0x0000; static const mip_shared_external_time_delta_data_valid_flags MIP_SHARED_EXTERNAL_TIME_DELTA_DATA_VALID_FLAGS_DT_NANOS = 0x0001; ///< +static const mip_shared_external_time_delta_data_valid_flags MIP_SHARED_EXTERNAL_TIME_DELTA_DATA_VALID_FLAGS_ALL = 0x0001; struct mip_shared_external_time_delta_data { diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 7963b132f..fef0ff682 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -171,6 +171,7 @@ struct GpsTimestamp TOW = 0x0001, ///< Whole number seconds TOW has been set WEEK_NUMBER = 0x0002, ///< Week number has been set TIME_VALID = 0x0003, ///< Both TOW and Week Number have been set + ALL = 0x0003, }; uint16_t value = NONE; @@ -188,6 +189,9 @@ struct GpsTimestamp void weekNumber(bool val) { if(val) value |= WEEK_NUMBER; else value &= ~WEEK_NUMBER; } uint16_t timeValid() const { return (value & TIME_VALID) >> 0; } void timeValid(uint16_t val) { value = (value & ~TIME_VALID) | (val << 0); } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; double tow = 0; ///< GPS Time of Week [seconds] @@ -335,6 +339,7 @@ struct ExternalTimestamp { NONE = 0x0000, NANOSECONDS = 0x0001, ///< + ALL = 0x0001, }; uint16_t value = NONE; @@ -348,6 +353,9 @@ struct ExternalTimestamp bool nanoseconds() const { return (value & NANOSECONDS) > 0; } void nanoseconds(bool val) { if(val) value |= NANOSECONDS; else value &= ~NANOSECONDS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint64_t nanoseconds = 0; @@ -395,6 +403,7 @@ struct ExternalTimeDelta { NONE = 0x0000, DT_NANOS = 0x0001, ///< + ALL = 0x0001, }; uint16_t value = NONE; @@ -408,6 +417,9 @@ struct ExternalTimeDelta bool dtNanos() const { return (value & DT_NANOS) > 0; } void dtNanos(bool val) { if(val) value |= DT_NANOS; else value &= ~DT_NANOS; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } }; uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. From 5d2378179dc18675c9786c7b17a5a3cb119612f0 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 28 Mar 2023 13:01:32 -0400 Subject: [PATCH 039/252] Merge integration branch (#58) * Test out bitfield accessors. (#32) * Added git attributes for line endings * Generate MIP definitions from branches/bitfield_accessors@53746. Co-authored-by: Nick DaCosta <25206843+dacuster@users.noreply.github.com> * Generate MIP definitions from branches/dev@53813. * Merge develop into definitions/dev (#39) * Generate MIP definitions from branches/dev@53813. (#37) Co-authored-by: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> * Change extern C functions to callbacks for use in dynamic libraries (#34) * Add remaining_time accessor to pending commands. * Refactor mip_interface and MipDevice: - Use callbacks. - Add wait_time parameter to recv_from_device. * Update serial and TCP connections to include wait_time parameter. * Add nonmember callback setters to DeviceInterface. * Update recording connection. * Update examples. * Fix serial port build on Windows. * Removes extern timestamp function from connections Co-authored-by: Rob Fisher * Update line endings (#38) Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> Co-authored-by: Rob Fisher Co-authored-by: Nick DaCosta <25206843+dacuster@users.noreply.github.com> * Generate MIP definitions from branches/fix_tuple@53851. (#41) * Merge latest from develop (#42) * Added git attributes for line endings * Fix bug in mip_interface_wait_for_reply when update function fails. (#35) * Generate MIP definitions from branches/bitfield_accessors@53746. (#33) * Generate MIP definitions from branches/dev@53813. (#37) Co-authored-by: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> * Change extern C functions to callbacks for use in dynamic libraries (#34) * Add remaining_time accessor to pending commands. * Refactor mip_interface and MipDevice: - Use callbacks. - Add wait_time parameter to recv_from_device. * Update serial and TCP connections to include wait_time parameter. * Add nonmember callback setters to DeviceInterface. * Update recording connection. * Update examples. * Fix serial port build on Windows. * Removes extern timestamp function from connections Co-authored-by: Rob Fisher * Update line endings (#38) * Add diagnostic counters to mip parser. (#36) * Add diagnostic counters to mip parser. * Remove unnecessary #ifdef. * Add MIP_DIAG_ZERO macro to zero diagnostic counters. * Fix CMake not passing diagnostic flags to compiler. * Fix compiler errors related to diagnostics. * Add missing stats increment in mip_parser_process_written. * Print statistics in WatchImu example. * Add diagnostic printout to WatchImuC example. * Add diagnostic counters to command queue. * Change MIP_ENABLE_DIAGNOSTIC_COUNTERS -> MIP_ENABLE_DIAGNOSTICS. * Add MIP_ENABLE_DIAGNOSTICS to readme. * Merge line endings. * Removes semicolon warnings with void cast Co-authored-by: Rob Fisher * MIP SDK Logger (#27) * Checkin * checkin * Adds logging functionality * Reverts spaces for mip_interface * Updates README * Updates based on PR comments * Changes noop from do while to void cast * Updates files to mip_logging, and adds MIP_LOG_INIT macro * Removes mip_logger.c * Renames LoggingLevel to LogLevel and removes test log * Removes C++, and adds ability to set max log level at compile time * Removes namespaces and moves enums to defines in logging * Checkin? * Updates based on PR feedback * Reverts some pointless changes Co-authored-by: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> Co-authored-by: Rob Fisher * Generate MIP definitions from branches/dev@53881. * Generate MIP definitions from branches/dev@53882. * Fix DeviceInterface::update functions to use the wait_time value instead of boolean. * - Add exact_size parameter to Field::extract. - Export INVALID_FIELD_DESCRIPTOR to C++. * Add functions to C++ dispatcher class. * Generate MIP definitions from branches/dev@54061. * Definitions/dev (#45) * Does not cause a build error if the version returned from git is not in the semantic version format (#43) * Does not cause a build error if the version returned from git is not in the semantic version format * Removes debug print * Replaces only the file extension in .cpp and .hpp files (#44) * Generate MIP definitions from branches/dev@54061. Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> * Fix mip::Packet::addField and add mip_serializer_init_new_field. * Fix mip_field_from_header_ptr. * Fix bug in mip_serializer_finish_new_field when serializer->_buffer is NULL (called by Packet::addField). * Definitions/dev (#47) * Does not cause a build error if the version returned from git is not in the semantic version format (#43) * Does not cause a build error if the version returned from git is not in the semantic version format * Removes debug print * Replaces only the file extension in .cpp and .hpp files (#44) * Generate MIP definitions from branches/dev@54061. Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> * Merge Develop (#48) * Does not cause a build error if the version returned from git is not in the semantic version format (#43) * Does not cause a build error if the version returned from git is not in the semantic version format * Removes debug print * Replaces only the file extension in .cpp and .hpp files (#44) Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> * Fix cmake installation of internal header files. * Generate MIP definitions from branches/dev@54243. * Definitions/dev (#49) * Does not cause a build error if the version returned from git is not in the semantic version format (#43) * Does not cause a build error if the version returned from git is not in the semantic version format * Removes debug print * Replaces only the file extension in .cpp and .hpp files (#44) * Generate MIP definitions from branches/dev@54061. * Generate MIP definitions from branches/dev@54243. Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> * Add methods for getting response descriptors. * Merge Definitions/dev (#50) * Does not cause a build error if the version returned from git is not in the semantic version format (#43) * Does not cause a build error if the version returned from git is not in the semantic version format * Removes debug print * Replaces only the file extension in .cpp and .hpp files (#44) * Generate MIP definitions from branches/dev@54061. * Generate MIP definitions from branches/dev@54243. Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> * Packet creation: - Fix sign error in length calculation in mip_packet_realloc_last_field. - Fix field header length being added twice in mip_packet_cancel_last_field. * Improve documentation in mip::Packet and mip::Field in mip.hpp. * Fix mip_is_cmd_descriptor_set for reserved commands. * Fix compilation error wrt Packet::FieldIterator access. * Rename template parameters from "Field" to "FieldType" to avoid ambiguity with mip::Field class. * Develop (#52) * Does not cause a build error if the version returned from git is not in the semantic version format (#43) * Does not cause a build error if the version returned from git is not in the semantic version format * Removes debug print * Replaces only the file extension in .cpp and .hpp files (#44) * Removes the MIP version from the all header (#51) --------- Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> * Generate MIP definitions from branches/dev@54435. * Remove line ending stuff from .gitattributes. * Fix cmake installation of internal header files. * Fix bad merge of gnss definitions. * Fix bad installation paths when not using internal defintions. * Make Connection have a virtual destructor. --------- Co-authored-by: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Co-authored-by: Nick DaCosta Co-authored-by: Rob <87914992+robbiefish@users.noreply.github.com> Co-authored-by: Rob Fisher --- CMakeLists.txt | 4 +- src/mip/definitions/descriptors.c | 2 +- src/mip/definitions/descriptors.h | 3 + src/mip/mip.hpp | 740 ++++++++++--------- src/mip/mip_cmdqueue.c | 1112 +++++++++++++++-------------- src/mip/mip_cmdqueue.h | 2 +- src/mip/mip_device.hpp | 14 +- src/mip/mip_field.c | 9 +- src/mip/mip_packet.c | 922 ++++++++++++------------ src/mip/utils/serialization.c | 500 +++++++------ src/mip/utils/serialization.h | 547 +++++++------- 11 files changed, 2012 insertions(+), 1843 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5542a12ad..45af8210e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -355,7 +355,9 @@ set(ALL_MIP_HEADERS "${ALL_MIP_SOURCES}") list(FILTER ALL_MIP_HEADERS INCLUDE REGEX "^.*\.(h|hpp)$") foreach(MIP_HEADER ${ALL_MIP_HEADERS}) string(REPLACE "${SRC_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER}") - string(REPLACE "${INT_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER_RELATIVE}") + if(INT_DIR) + string(REPLACE "${INT_DIR}/" "" MIP_HEADER_RELATIVE "${MIP_HEADER_RELATIVE}") + endif() set(MIP_HEADER_DESTINATION_FULL "${CMAKE_INSTALL_INCLUDEDIR}/${MIP_HEADER_RELATIVE}") get_filename_component(MIP_HEADER_DESTINATION "${MIP_HEADER_DESTINATION_FULL}" DIRECTORY) install( diff --git a/src/mip/definitions/descriptors.c b/src/mip/definitions/descriptors.c index 9d11708e4..bc32cb598 100644 --- a/src/mip/definitions/descriptors.c +++ b/src/mip/definitions/descriptors.c @@ -42,7 +42,7 @@ bool mip_is_data_descriptor_set(uint8_t descriptor_set) /// bool mip_is_cmd_descriptor_set(uint8_t descriptor_set) { - return (descriptor_set < MIP_DATA_DESCRIPTOR_SET_START); + return !mip_is_data_descriptor_set(descriptor_set); } //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index 5a4c1a3a0..4f6da0545 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -109,6 +109,9 @@ enum class FunctionSelector : uint8_t using DescriptorRate = C::mip_descriptor_rate; +static constexpr uint8_t INVALID_FIELD_DESCRIPTOR = C::MIP_INVALID_FIELD_DESCRIPTOR; +static constexpr uint8_t INVALID_DESCRIPTOR_SET = C::MIP_INVALID_DESCRIPTOR_SET; + inline bool isValidDescriptorSet (uint8_t descriptorSet) { return C::mip_is_valid_descriptor_set(descriptorSet); } inline bool isDataDescriptorSet (uint8_t descriptorSet) { return C::mip_is_data_descriptor_set(descriptorSet); } inline bool isCommandDescriptorSet (uint8_t descriptorSet) { return C::mip_is_cmd_descriptor_set(descriptorSet); } diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 4c4832e63..50052c37e 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -1,326 +1,414 @@ -#pragma once - -#include - -#include -#include - -#include "mip_packet.h" -#include "mip_field.h" -#include "mip_parser.h" -#include "mip_offsets.h" -#include "definitions/descriptors.h" -#include "mip_types.h" - -#include - -//////////////////////////////////////////////////////////////////////////////// -///@defgroup mip_cpp MIP C++ API -/// -///@brief This module contains functions and classes for communicating with a -/// MIP device in C++. -/// -///@see mip namespace -/// - -//////////////////////////////////////////////////////////////////////////////// -///@brief A collection of C++ classes and functions covering the full -/// mip api. -/// -///@see mip_cpp -/// -namespace mip -{ - -using PacketLength = C::packet_length; - -template struct MipFieldInfo; - -//////////////////////////////////////////////////////////////////////////////// -///@addtogroup mip_cpp -///@{ - -//////////////////////////////////////////////////////////////////////////////// -///@brief C++ class representing a MIP field. -/// -class Field : public C::mip_field -{ -public: - /// Construct an empty MIP field. - Field() { C::mip_field::_payload=nullptr; C::mip_field::_payload_length=0; C::mip_field::_field_descriptor=0x00; C::mip_field::_descriptor_set=0x00; C::mip_field::_remaining_length=0; } - ///@copydoc mip_field_init() - Field(uint8_t descriptor_set, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length) { C::mip_field_init(this, descriptor_set, field_descriptor, payload, payload_length); } - ///@copydoc mip_field_from_header_ptr() - Field(const uint8_t* header, uint8_t total_length, uint8_t descriptor_set) { *this = C::mip_field_from_header_ptr(header, total_length, descriptor_set); } - /// Creates a %Field class from the mip_field C struct. - Field(const C::mip_field& other) { std::memcpy(static_cast(this), &other, sizeof(C::mip_field)); } - - ///@copydoc mip_field_descriptor_set - uint8_t descriptorSet() const { return C::mip_field_descriptor_set(this); } - ///@copydoc mip_field_field_descriptor - uint8_t fieldDescriptor() const { return C::mip_field_field_descriptor(this); } - ///@copydoc mip_field_payload_length - uint8_t payloadLength() const { return C::mip_field_payload_length(this); } - ///@copydoc mip_field_payload - const uint8_t* payload() const { return C::mip_field_payload(this); } - - template - bool extract(Field& field) const { return mip::extract(field, payload(), payloadLength(), 0, true); } - - ///@brief Index the payload at the given location. - ///@param index - ///@returns payload byte - uint8_t payload(unsigned int index) const { return payload()[index]; } - - ///@copydoc mip_field_is_valid - bool isValid() const { return C::mip_field_is_valid(this); } - - ///@copydoc mip_field_next_after - Field nextAfter() const { return C::mip_field_next_after(this); } - ///@copydoc mip_field_next - bool next() { return C::mip_field_next(this); } - - - ///@brief Determines if the field is from a command descriptor set (a command, reply, or response field). - bool isCommandSet() const { return isCommandDescriptorSet(descriptorSet()); } - - ///@brief Determines if the field contains a data field. - bool isData() const { return isDataDescriptorSet(descriptorSet()); } - - ///@brief Determines if the field holds a command. - bool isCommand() const { return isCommandSet() && isCommandFieldDescriptor(fieldDescriptor()); } - - ///@brief Determines if the field holds command response data. - bool isResponse() const { return isCommandSet() && isResponseFieldDescriptor(fieldDescriptor()); } - - ///@brief Determines if the field holds an ack/nack reply code. - bool isReply() const { return isCommandSet() && isReplyFieldDescriptor(fieldDescriptor()) && payloadLength()==2; } -}; - - -//////////////////////////////////////////////////////////////////////////////// -///@brief C++ class representing a MIP Packet. -/// -/// Fields may be iterated over using the C-style method or with a range-based -/// for loop: -///@code{.cpp} -/// for(Field field : packet) { ... } -///@endcode -/// -class Packet : public C::mip_packet -{ - class FieldIterator; - -public: - ///@copydoc mip::C::mip_packet_create - Packet(uint8_t* buffer, size_t bufferSize, uint8_t descriptorSet) { C::mip_packet_create(this, buffer, bufferSize, descriptorSet); } - ///@copydoc mip_packet_from_buffer - Packet(uint8_t* buffer, size_t length) { C::mip_packet_from_buffer(this, buffer, length); } - /// Constructs a C++ %Packet class from the base C object. - Packet(const C::mip_packet* other) { std::memcpy(static_cast(this), other, sizeof(*this)); } - /// Constructs a C++ %Packet class from the base C object. - Packet(const C::mip_packet& other) { std::memcpy(static_cast(this), &other, sizeof(*this)); } - - uint8_t descriptorSet() const { return C::mip_packet_descriptor_set(this); } ///<@copydoc mip::C::mip_packet_descriptor_set - PacketLength totalLength() const { return C::mip_packet_total_length(this); } ///<@copydoc mip::C::mip_packet_total_length - uint8_t payloadLength() const { return C::mip_packet_payload_length(this); } ///<@copydoc mip::C::mip_packet_payload_length - - bool isData() const { return C::mip_packet_is_data(this); } - bool isCommand() const { return !C::mip_packet_is_data(this); } - - const uint8_t* pointer() const { return C::mip_packet_pointer(this); } ///<@copydoc mip::C::mip_packet_pointer - const uint8_t* payload() const { return C::mip_packet_payload(this); } ///<@copydoc mip::C::mip_packet_payload - - uint16_t checksumValue() const { return C::mip_packet_checksum_value(this); } ///<@copydoc mip::C::mip_packet_checksum_value - uint16_t computeChecksum() const { return C::mip_packet_compute_checksum(this); } ///<@copydoc mip::C::mip_packet_compute_checksum - - bool isSane() const { return C::mip_packet_is_sane(this); } ///<@copydoc mip::C::mip_packet_is_sane - bool isValid() const { return C::mip_packet_is_valid(this); } ///<@copydoc mip::C::mip_packet_is_valid - bool isEmpty() const { return C::mip_packet_is_empty(this); } ///<@copydoc mip::C::mip_packet_is_empty - - PacketLength bufferSize() const { return C::mip_packet_buffer_size(this); } ///<@copydoc mip::C::mip_packet_buffer_size - RemainingCount remainingSpace() const { return C::mip_packet_remaining_space(this); } ///<@copydoc mip::C::mip_packet_remaining_space - - bool addField(uint8_t fieldDescriptor, const uint8_t* payload, size_t payloadLength) { return C::mip_packet_add_field(this, fieldDescriptor, payload, payloadLength); } ///<@copydoc mip::C::mip_packet_add_field - RemainingCount allocField(uint8_t fieldDescriptor, uint8_t payloadLength, uint8_t** payloadPtr_out) { return C::mip_packet_alloc_field(this, fieldDescriptor, payloadLength, payloadPtr_out); } ///<@copydoc mip::C::mip_packet_alloc_field - RemainingCount reallocLastField(uint8_t* payloadPtr, uint8_t newPayloadLength) { return C::mip_packet_realloc_last_field(this, payloadPtr, newPayloadLength); } ///<@copydoc mip::C::mip_packet_realloc_last_field - RemainingCount cancelLastField(uint8_t* payloadPtr) { return C::mip_packet_cancel_last_field(this, payloadPtr); } ///<@copydoc mip::C::mip_packet_cancel_last_field - - void finalize() { C::mip_packet_finalize(this); } ///<@copydoc mip::C::mip_packet_finalize - - void reset(uint8_t descSet) { C::mip_packet_reset(this, descSet); } ///<@copydoc mip::C::mip_packet_reset - void reset() { reset(descriptorSet()); } ///<@brief Resets the packet using the same descriptor set. - - /// Returns the first field in the packet. - Field firstField() const { return Field(C::mip_field_first_from_packet(this)); } - - /// Returns a forward iterator to the first field in the packet. - ///@internal - FieldIterator begin() const { return firstField(); } - - /// Returns a sentry object representing the end of fields in the packet. - ///@internal -#if __cpp_range_based_for >= 201603 - std::nullptr_t end() const { return nullptr; } -#else - FieldIterator end() const { return Field(); } -#endif - - template - bool addField(const Field& field, uint8_t fieldDescriptor = Field::FIELD_DESCRIPTOR) - { - uint8_t* payload; - size_t available = allocField(fieldDescriptor, 0, &payload); - Serializer serializer(payload, available); - insert(serializer, field); - return reallocLastField(payload, serializer.length()) >= 0; - } - - template - static Packet createFromField(uint8_t* buffer, size_t bufferSize, const Field& field, uint8_t fieldDescriptor=Field::FIELD_DESCRIPTOR) - { - Packet packet(buffer, bufferSize, Field::DESCRIPTOR_SET); - packet.addField(field, fieldDescriptor); - packet.finalize(); - return packet; - } - -private: - /// Iterator class for use with the range-based for loop. - ///@internal - class FieldIterator - { - public: - FieldIterator(const Field& first) : mField(first) {} - FieldIterator() {} - - bool operator==(const FieldIterator& other) const { - // Required to make invalid fields equivalent for range-based for loop - if( !mField.isValid() && !other.mField.isValid() ) - return true; - return mField.descriptorSet() == other.mField.descriptorSet() && mField.fieldDescriptor() == other.mField.fieldDescriptor() && mField.payload() == other.mField.payload(); - } - bool operator!=(const FieldIterator& other) const { return !(*this == other); } - - bool operator==(std::nullptr_t) const { return !mField.isValid(); } - bool operator!=(std::nullptr_t) const { return mField.isValid(); } - - const Field& operator*() const { return mField; } - - FieldIterator& operator++() { mField.next(); return *this; } - private: - Field mField; - }; - -}; - - -//////////////////////////////////////////////////////////////////////////////// -///@brief C++ class representing a MIP parser. -/// -/// See @ref parsing_packets -/// -class Parser : public C::mip_parser -{ -public: - ///@copydoc mip::C::mip_parser_init - Parser(uint8_t* buffer, size_t bufferSize, C::mip_packet_callback callback, void* callbackObject, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, callback, callbackObject, timeout); } - ///@copydoc mip::C::mip_parser_init - Parser(uint8_t* buffer, size_t bufferSize, bool (*callback)(void*,const Packet*,Timestamp), void* callbackObject, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, (C::mip_packet_callback)callback, callbackObject, timeout); } - - Parser(uint8_t* buffer, size_t bufferSize, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, nullptr, nullptr, timeout); } - - template - void setCallback(T& object); - - ///@copydoc mip::C::mip_parser_reset - void reset() { C::mip_parser_reset(this); } - - ///@copydoc mip::C::mip_parser_parse - RemainingCount parse(const uint8_t* inputBuffer, size_t inputCount, Timestamp timestamp, unsigned int maxPackets) { return C::mip_parser_parse(this, inputBuffer, inputCount, timestamp, maxPackets); } - - ///@copydoc mip::C::mip_parser_timeout - Timeout timeout() const { return C::mip_parser_timeout(this); } - ///@copydoc mip::C::mip_parser_set_timeout - void setTimeout(Timeout timeout) { return C::mip_parser_set_timeout(this, timeout); } -}; - - -///@brief Initializes the MIP Parser -/// -/// This version allows binding a member function instead of a C-style callback. -/// Example: -///@code{.cpp} -/// struct MyClass -/// { -/// void handlePacket(const Packet& packet, Timeout timeout); -/// }; -/// MyClass myInstance; -/// Parser parser(myInstance); -///@endcode -/// -///@tparam T Class type containing the member function to be called. -///@tparam Callback A pointer to a member function on a T to be called when a -/// packet is parsed. -/// -///@param object -/// Instance of T to call the callback. -/// -template -void Parser::setCallback(T& object) -{ - C::mip_packet_callback callback = [](void* obj, const C::mip_packet* pkt, Timestamp timestamp)->bool - { - return (static_cast(obj)->*Callback)(Packet(pkt), timestamp); - }; - - C::mip_parser_set_callback(this, callback, &object); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Read data from a source into the internal parsing buffer. -/// -///@tparam Function -/// A function-like object with the following signature: -/// `bool read(size_t maxCount, size_t* count_out, Timestamp* timestamp_out);` -/// The parameters are as follows: -/// @li buffer - Buffer into which to write data. -/// @li maxCount - The maximum number of bytes to read. -/// @li count_out - Updated with the number of bytes actually read. -/// @li timestamp_out - Updated with the timestamp of the data. -/// -///@param parser -/// -///@param reader -/// A callback function, lambda, or similar which will read data into the -/// buffer and capture the timestamp. It should return true if successful -/// or false otherwise. If it returns false, parsing is skipped. The read -/// function may also throw an exception instead of returning false. -/// -///@param maxPackets -/// Maximum number of packets to parse, just like for Parser::parse(). -/// -///@return Same as the return value of reader. -/// -template -bool parseMipDataFromSource(C::mip_parser& parser, Function reader, size_t maxPackets) -{ - uint8_t* ptr; - size_t maxCount = C::mip_parser_get_write_ptr(&parser, &ptr); - - size_t count; - Timestamp timestamp; - if( !reader(ptr, maxCount, &count, ×tamp) ) - return false; - - assert(count <= maxCount); - - C::mip_parser_process_written(&parser, count, timestamp, maxPackets); - - return true; -} - -///@} -//////////////////////////////////////////////////////////////////////////////// - -} // namespace mip +#pragma once + +#include + +#include +#include + +#include "mip_packet.h" +#include "mip_field.h" +#include "mip_parser.h" +#include "mip_offsets.h" +#include "definitions/descriptors.h" +#include "mip_types.h" + +#include + +//////////////////////////////////////////////////////////////////////////////// +///@defgroup mip_cpp MIP C++ API +/// +///@brief This module contains functions and classes for communicating with a +/// MIP device in C++. +/// +///@see mip namespace +/// + +//////////////////////////////////////////////////////////////////////////////// +///@brief A collection of C++ classes and functions covering the full mip api. +/// +///@see mip_cpp +/// +namespace mip +{ + +using PacketLength = C::packet_length; + +template struct MipFieldInfo; + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_cpp +///@{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief C++ class representing a MIP field. +/// +/// This is a thin wrapper around the C mip_field struct. +/// +class Field : public C::mip_field +{ +public: + /// Construct an empty MIP field. + Field() { C::mip_field::_payload=nullptr; C::mip_field::_payload_length=0; C::mip_field::_field_descriptor=0x00; C::mip_field::_descriptor_set=0x00; C::mip_field::_remaining_length=0; } + ///@copydoc mip_field_init() + Field(uint8_t descriptor_set, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length) { C::mip_field_init(this, descriptor_set, field_descriptor, payload, payload_length); } + ///@copydoc mip_field_from_header_ptr() + Field(const uint8_t* header, uint8_t total_length, uint8_t descriptor_set) { *this = C::mip_field_from_header_ptr(header, total_length, descriptor_set); } + /// Creates a %Field class from the mip_field C struct. + Field(const C::mip_field& other) { std::memcpy(static_cast(this), &other, sizeof(C::mip_field)); } + + // + // C function wrappers + // + + ///@copydoc mip_field_descriptor_set + uint8_t descriptorSet() const { return C::mip_field_descriptor_set(this); } + ///@copydoc mip_field_field_descriptor + uint8_t fieldDescriptor() const { return C::mip_field_field_descriptor(this); } + ///@copydoc mip_field_payload_length + uint8_t payloadLength() const { return C::mip_field_payload_length(this); } + ///@copydoc mip_field_payload + const uint8_t* payload() const { return C::mip_field_payload(this); } + + ///@brief Index the payload at the given location. + ///@param index + ///@returns payload byte + uint8_t payload(unsigned int index) const { return payload()[index]; } + + ///@copydoc mip_field_is_valid + bool isValid() const { return C::mip_field_is_valid(this); } + + ///@copydoc mip_field_next_after + Field nextAfter() const { return C::mip_field_next_after(this); } + ///@copydoc mip_field_next + bool next() { return C::mip_field_next(this); } + + // + // Additional functions which have no C equivalent + // + + ///@brief Deserializes the field data to specific field struct. + /// + ///@tparam FieldType Any field class from a file in the mip/definitions directory. + /// + ///@param[out] field A reference to the field struct to be filled out. Valid + /// only if the function returns true. + ///@param exact_size If true, the function fails if any bytes remain after deserialization. + /// + ///@returns True if the field was successfully deserialized, or false if the field contains + /// too few bytes (or to many if exact_size is specified). The field data is not + /// valid unless this function returns true. + template + bool extract(FieldType& field, bool exact_size=true) const { return mip::extract(field, payload(), payloadLength(), 0, exact_size); } + + + ///@brief Determines if the field holds data (and not a command, reply, or response). + bool isData() const { return isDataDescriptorSet(descriptorSet()); } + + ///@brief Determines if the field is from a command descriptor set (a command, reply, or response field). + bool isCommandSet() const { return isCommandDescriptorSet(descriptorSet()); } + + ///@brief Determines if the field holds a command. + bool isCommand() const { return isCommandSet() && isCommandFieldDescriptor(fieldDescriptor()); } + + ///@brief Determines if the field holds an ack/nack reply code. + bool isReply() const { return isCommandSet() && isReplyFieldDescriptor(fieldDescriptor()) && payloadLength()==2; } + + ///@brief Determines if the field holds command response data (not an ack/nack reply). + bool isResponse() const { return isCommandSet() && isResponseFieldDescriptor(fieldDescriptor()); } +}; + + +//////////////////////////////////////////////////////////////////////////////// +///@brief C++ class representing a MIP Packet. +/// +/// This is a thin wrapper around the mip_packet C structure. Like the C +/// version, it does not contain or own the data buffer. Any of the C functions +/// can be used with the C++ packet class because it inherits from the C struct. +/// +/// Fields may be iterated over using the C-style methods, with an iterator, or +/// with a range-based for loop: +///@code{.cpp} +/// for(Packet::Iterator iter = packet.begin(); iter != packet.end(); ++iter) { ... } +/// for(Field field : packet) { ... } +///@endcode +/// +class Packet : public C::mip_packet +{ +public: + class FieldIterator; + +public: + ///@copydoc mip::C::mip_packet_create + Packet(uint8_t* buffer, size_t bufferSize, uint8_t descriptorSet) { C::mip_packet_create(this, buffer, bufferSize, descriptorSet); } + ///@copydoc mip_packet_from_buffer + Packet(uint8_t* buffer, size_t length) { C::mip_packet_from_buffer(this, buffer, length); } + /// Constructs a C++ %Packet class from the base C object. + Packet(const C::mip_packet* other) { std::memcpy(static_cast(this), other, sizeof(*this)); } + /// Constructs a C++ %Packet class from the base C object. + Packet(const C::mip_packet& other) { std::memcpy(static_cast(this), &other, sizeof(*this)); } + + + // + // C function wrappers + // + + uint8_t descriptorSet() const { return C::mip_packet_descriptor_set(this); } ///<@copydoc mip::C::mip_packet_descriptor_set + PacketLength totalLength() const { return C::mip_packet_total_length(this); } ///<@copydoc mip::C::mip_packet_total_length + uint8_t payloadLength() const { return C::mip_packet_payload_length(this); } ///<@copydoc mip::C::mip_packet_payload_length + + bool isData() const { return C::mip_packet_is_data(this); } + + const uint8_t* pointer() const { return C::mip_packet_pointer(this); } ///<@copydoc mip::C::mip_packet_pointer + const uint8_t* payload() const { return C::mip_packet_payload(this); } ///<@copydoc mip::C::mip_packet_payload + + uint16_t checksumValue() const { return C::mip_packet_checksum_value(this); } ///<@copydoc mip::C::mip_packet_checksum_value + uint16_t computeChecksum() const { return C::mip_packet_compute_checksum(this); } ///<@copydoc mip::C::mip_packet_compute_checksum + + bool isSane() const { return C::mip_packet_is_sane(this); } ///<@copydoc mip::C::mip_packet_is_sane + bool isValid() const { return C::mip_packet_is_valid(this); } ///<@copydoc mip::C::mip_packet_is_valid + bool isEmpty() const { return C::mip_packet_is_empty(this); } ///<@copydoc mip::C::mip_packet_is_empty + + PacketLength bufferSize() const { return C::mip_packet_buffer_size(this); } ///<@copydoc mip::C::mip_packet_buffer_size + RemainingCount remainingSpace() const { return C::mip_packet_remaining_space(this); } ///<@copydoc mip::C::mip_packet_remaining_space + + bool addField(uint8_t fieldDescriptor, const uint8_t* payload, size_t payloadLength) { return C::mip_packet_add_field(this, fieldDescriptor, payload, payloadLength); } ///<@copydoc mip::C::mip_packet_add_field + RemainingCount allocField(uint8_t fieldDescriptor, uint8_t payloadLength, uint8_t** payloadPtr_out) { return C::mip_packet_alloc_field(this, fieldDescriptor, payloadLength, payloadPtr_out); } ///<@copydoc mip::C::mip_packet_alloc_field + RemainingCount reallocLastField(uint8_t* payloadPtr, uint8_t newPayloadLength) { return C::mip_packet_realloc_last_field(this, payloadPtr, newPayloadLength); } ///<@copydoc mip::C::mip_packet_realloc_last_field + RemainingCount cancelLastField(uint8_t* payloadPtr) { return C::mip_packet_cancel_last_field(this, payloadPtr); } ///<@copydoc mip::C::mip_packet_cancel_last_field + + void finalize() { C::mip_packet_finalize(this); } ///<@copydoc mip::C::mip_packet_finalize + + void reset(uint8_t descSet) { C::mip_packet_reset(this, descSet); } ///<@copydoc mip::C::mip_packet_reset + void reset() { reset(descriptorSet()); } ///<@brief Resets the packet using the same descriptor set. + + + // + // Additional functions which have no C equivalent + // + + /// Returns a forward iterator to the first field in the packet. + /// + FieldIterator begin() const { return firstField(); } + + /// Returns a sentry object representing the end of fields in the packet. + /// +#if __cpp_range_based_for >= 201603 + // After 201603, for loops allow different clases for begin and end. + // Using nullptr is simpler and more efficient than creating an end iterator. + std::nullptr_t end() const { return nullptr; } +#else + FieldIterator end() const { return Field(); } +#endif + + ///@brief Returns the first field in the packet. + /// + /// Subsequent fields can be obtained via the returned Field class, + /// but iteration is best done with begin()/end() or the range-based for loop. + /// + ///@note Packets can be empty, so make sure that the returned field is + /// valid before using it. + /// + ///@returns A Field instance representing the first field (if any). + /// + Field firstField() const { return Field(C::mip_field_first_from_packet(this)); } + + ///@brief Adds a field of the given type to the packet. + /// + ///@tparam FieldType Any field class from a file in the mip/definitions directory. + /// + ///@param field Instance of the field to add to the packet. + ///@param fieldDescriptor If specified, overrides the field descriptor. + /// + ///@returns True if the field was added, false if the packet has insufficient space. + /// + template + bool addField(const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR) + { + if( fieldDescriptor == INVALID_FIELD_DESCRIPTOR ) + fieldDescriptor = FieldType::FIELD_DESCRIPTOR; + Serializer serializer(*this, fieldDescriptor); + insert(serializer, field); + C::mip_serializer_finish_new_field(&serializer, this); + return serializer.isOk(); + } + + ///@brief Creates a new Packet containing a single MIP field from an instance of the field type. + /// + /// This works just like the addField() function but also initializes and finalizes the packet. + /// It is assumed that the field will fit in an empty packet; otherwise the field can't ever be used. + /// The field classes are predefined so this doesn't need runtime checking. + /// + ///@tparam FieldType Any field class from a file in the mip/definitions directory. + /// + ///@param buffer Buffer to hold the packet bytes. + ///@param bufferSize Size of buffer in bytes. + ///@param field Instance of the field to add to the packet. + ///@param fieldDescriptor If specified, overrides the field descriptor. + /// + ///@returns A Packet object containing the field. + /// + template + static Packet createFromField(uint8_t* buffer, size_t bufferSize, const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR) + { + if( fieldDescriptor == INVALID_FIELD_DESCRIPTOR ) + fieldDescriptor = FieldType::FIELD_DESCRIPTOR; + Packet packet(buffer, bufferSize, FieldType::DESCRIPTOR_SET); + packet.addField(field, fieldDescriptor); + packet.finalize(); + return packet; + } + + + /// Iterator class for use with the range-based for loop or iterators. + /// + /// You should generally use the begin()/end() functions on the Packet + /// class instead of using this class directly. + /// + class FieldIterator + { + public: + /// Empty iterator, which represents the "end" iterator of a packet. + FieldIterator() {} + + /// Create an iterator given the first field to iterate in a packet. + /// Technically this can be any field, not just the first field. + FieldIterator(const Field& first) : mField(first) {} + + /// Comparison between any two iterators. + /// This works even for iterators over different packets, which will + /// never be the same (except all null/end iterators are equivalent). + bool operator==(const FieldIterator& other) const { + // Required to make invalid fields equivalent for range-based for loop + if( !mField.isValid() && !other.mField.isValid() ) + return true; + return mField.descriptorSet() == other.mField.descriptorSet() && mField.fieldDescriptor() == other.mField.fieldDescriptor() && mField.payload() == other.mField.payload(); + } + bool operator!=(const FieldIterator& other) const { return !(*this == other); } + + /// Comparison with std::nullptr is checking if the iterator points to + /// a valid field (i.e. not the end). + bool operator==(std::nullptr_t) const { return !mField.isValid(); } + bool operator!=(std::nullptr_t) const { return mField.isValid(); } + + /// Dereference the iterator as a Field instance. + const Field& operator*() const { return mField; } + + /// Advance to the next field. + FieldIterator& operator++() { mField.next(); return *this; } + + private: + Field mField; + }; + +}; + + +//////////////////////////////////////////////////////////////////////////////// +///@brief C++ class representing a MIP parser. +/// +/// See @ref parsing_packets +/// +class Parser : public C::mip_parser +{ +public: + ///@copydoc mip::C::mip_parser_init + Parser(uint8_t* buffer, size_t bufferSize, C::mip_packet_callback callback, void* callbackObject, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, callback, callbackObject, timeout); } + ///@copydoc mip::C::mip_parser_init + Parser(uint8_t* buffer, size_t bufferSize, bool (*callback)(void*,const Packet*,Timestamp), void* callbackObject, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, (C::mip_packet_callback)callback, callbackObject, timeout); } + + Parser(uint8_t* buffer, size_t bufferSize, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, nullptr, nullptr, timeout); } + + template + void setCallback(T& object); + + ///@copydoc mip::C::mip_parser_reset + void reset() { C::mip_parser_reset(this); } + + ///@copydoc mip::C::mip_parser_parse + RemainingCount parse(const uint8_t* inputBuffer, size_t inputCount, Timestamp timestamp, unsigned int maxPackets=0) { return C::mip_parser_parse(this, inputBuffer, inputCount, timestamp, maxPackets); } + + ///@copydoc mip::C::mip_parser_timeout + Timeout timeout() const { return C::mip_parser_timeout(this); } + ///@copydoc mip::C::mip_parser_set_timeout + void setTimeout(Timeout timeout) { return C::mip_parser_set_timeout(this, timeout); } +}; + + +///@brief Initializes the MIP Parser +/// +/// This version allows binding a member function instead of a C-style callback. +/// Example: +///@code{.cpp} +/// struct MyClass +/// { +/// void handlePacket(const Packet& packet, Timeout timeout); +/// }; +/// MyClass myInstance; +/// Parser parser(myInstance); +///@endcode +/// +///@tparam T Class type containing the member function to be called. +///@tparam Callback A pointer to a member function on a T to be called when a +/// packet is parsed. +/// +///@param object +/// Instance of T to call the callback. +/// +template +void Parser::setCallback(T& object) +{ + C::mip_packet_callback callback = [](void* obj, const C::mip_packet* pkt, Timestamp timestamp)->bool + { + return (static_cast(obj)->*Callback)(Packet(pkt), timestamp); + }; + + C::mip_parser_set_callback(this, callback, &object); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Read data from a source into the internal parsing buffer. +/// +///@tparam Function +/// A function-like object with the following signature: +/// `bool read(size_t maxCount, size_t* count_out, Timestamp* timestamp_out);` +/// The parameters are as follows: +/// @li buffer - Buffer into which to write data. +/// @li maxCount - The maximum number of bytes to read. +/// @li count_out - Updated with the number of bytes actually read. +/// @li timestamp_out - Updated with the timestamp of the data. +/// +///@param parser +/// +///@param reader +/// A callback function, lambda, or similar which will read data into the +/// buffer and capture the timestamp. It should return true if successful +/// or false otherwise. If it returns false, parsing is skipped. The read +/// function may also throw an exception instead of returning false. +/// +///@param maxPackets +/// Maximum number of packets to parse, just like for Parser::parse(). +/// +///@return Same as the return value of reader. +/// +template +bool parseMipDataFromSource(C::mip_parser& parser, Function reader, size_t maxPackets) +{ + uint8_t* ptr; + size_t maxCount = C::mip_parser_get_write_ptr(&parser, &ptr); + + size_t count; + Timestamp timestamp; + if( !reader(ptr, maxCount, &count, ×tamp) ) + return false; + + assert(count <= maxCount); + + C::mip_parser_process_written(&parser, count, timestamp, maxPackets); + + return true; +} + +///@} +//////////////////////////////////////////////////////////////////////////////// + +} // namespace mip diff --git a/src/mip/mip_cmdqueue.c b/src/mip/mip_cmdqueue.c index 82f911b57..068378262 100644 --- a/src/mip/mip_cmdqueue.c +++ b/src/mip/mip_cmdqueue.c @@ -1,552 +1,560 @@ - -#include "mip_cmdqueue.h" - -#include "mip_field.h" -#include "mip_packet.h" - -#include -#include - - -#define MIP_REPLY_DESC_GLOBAL_ACK_NACK 0xF1 - -#define MIP_INDEX_REPLY_DESCRIPTOR 0 -#define MIP_INDEX_REPLY_ACK_CODE 1 - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending command with no reponse data or additional time. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -/// -void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, 0); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with extra timeout time. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param additional_time -/// Additional time on top of the base reply timeout for this specific command. -/// -void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with expected response data. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param response_descriptor -/// Optional response data descriptor. Use 0x00 if no data is expected. -///@param response_buffer -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Size of the response buffer. The response will be limited to this size. -/// -void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size) -{ - mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, response_descriptor, response_buffer, response_buffer_size, 0); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a pending mip commmand with all parameters. -/// -///@param cmd -///@param descriptor_set -/// Command descriptor set. -///@param field_descriptor -/// Command field descriptor. -///@param response_descriptor -/// Optional response data descriptor. Use 0x00 if no data is expected. -///@param response_buffer -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size -/// Size of the response buffer. The response will be limited to this size. -///@param additional_time -/// Additional time on top of the base reply timeout for this specific command. -/// -void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time) -{ - cmd->_next = NULL; - cmd->_response_buffer = NULL; - cmd->_extra_timeout = additional_time; - cmd->_descriptor_set = descriptor_set; - cmd->_field_descriptor = field_descriptor; - cmd->_response_descriptor = response_descriptor; - cmd->_response_buffer = response_buffer; - cmd->_response_buffer_size = response_buffer_size; - // cmd->_ack_code = 0xFF; // invalid - cmd->_status = MIP_STATUS_NONE; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the status of the pending command. -/// -///@see mip_cmd_status -/// -enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd) -{ - return cmd->_status; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the response payload pointer. -/// -/// This function may only be called after the command finishes with an ACK. -/// -const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd) -{ - assert(mip_cmd_result_is_finished(cmd->_status)); - - return cmd->_response_buffer; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the length of the response data. -/// -/// This function may only be called after the command finishes. -/// If the command completed with a NACK, or if it timed out, the response -/// length will be zero. -/// -uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) -{ - assert(mip_cmd_result_is_finished(cmd->_status)); - - return cmd->_response_length; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines how much time is remaining before the command times out. -/// -/// For most cases you should instead call mip_pending_cmd_check_timeout() to -/// know if the command has timed out or not. -/// -///@param cmd The command to check. Must be in MIP_STATUS_WAITING state. -///@param now The current timestamp. -/// -///@warning The command must be in the MIP_STATUS_WAITING state, otherwise the -/// result is unspecified. -/// -///@returns The time remaining before the command times out. The value will be -/// negative if the timeout time has passed. -/// -int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now) -{ - assert(cmd->_status == MIP_STATUS_WAITING); - - // result <= 0 if timed out. - // Note: this still works with unsigned overflow. - return (int)(now - cmd->_timeout_time); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Checks if the command should time out. -/// -///@param cmd -///@param now Current time -/// -///@returns true if the command should time out. Only possible for MIP_STATUS_WAITING. -///@returns false if the command should not time out. -/// -bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now) -{ - if( cmd->_status == MIP_STATUS_WAITING ) - { - if( mip_pending_cmd_remaining_time(cmd, now) > 0 ) - { - return true; - } - } - - return false; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initializes a command queue. -/// -///@param queue -///@param base_reply_timeout -/// The minimum timeout given to all MIP commands. Additional time may be -/// given to specific commands which take longer. This is intended to be -/// used to accommodate long communication latencies, such as when using -/// a TCP connection. -/// -void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) -{ - queue->_first_pending_cmd = NULL; - queue->_base_timeout = base_reply_timeout; - - MIP_DIAG_ZERO(queue->_diag_cmds_queued); - MIP_DIAG_ZERO(queue->_diag_cmds_acked); - MIP_DIAG_ZERO(queue->_diag_cmds_nacked); - MIP_DIAG_ZERO(queue->_diag_cmds_timedout); - MIP_DIAG_ZERO(queue->_diag_cmds_failed); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Queue a command to wait for replies. -/// -///@param queue -///@param cmd Listens for replies to this command. -/// -///@warning The command must not be deallocated or go out of scope while the -/// mip_cmd_status_is_finished returns false. -/// -void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd) -{ - // For now only one command can be queued at a time. - if( queue->_first_pending_cmd ) - { - cmd->_status = MIP_STATUS_CANCELLED; - return; - } - - MIP_DIAG_INC(queue->_diag_cmds_queued, 1); - - cmd->_status = MIP_STATUS_PENDING; - queue->_first_pending_cmd = cmd; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Removes a pending command from the queue. -/// -///@internal -/// -///@param queue -///@param cmd -/// -void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd) -{ - if( queue->_first_pending_cmd == cmd ) - { - queue->_first_pending_cmd = NULL; - cmd->_status = MIP_STATUS_CANCELLED; - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Iterate over a packet, checking for replies to the pending command. -/// -///@internal -/// -///@param pending -/// Pending command which is awaiting replies -///@param packet -///@param base_timeout -///@param timestamp -/// -///@returns The new status of the pending command (the command status field is -/// not updated). The caller should set pending->_status to this value -/// after doing any additional processing requiring the pending struct. -/// -static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp) -{ - assert( pending->_status != MIP_STATUS_NONE ); // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here. - assert( !mip_cmd_result_is_finished(pending->_status) ); // Command shouldn't be finished yet - make sure the queue is processed properly. - - if( pending->_status == MIP_STATUS_PENDING ) - { - // Update the timeout to the timestamp of the timeout time. - pending->_timeout_time = timestamp + base_timeout + pending->_extra_timeout; - pending->_status = MIP_STATUS_WAITING; - } - - // ------+------+------+------+------+------+------+------+------+------------------------ - // ... | 0x02 | 0xF1 | cmd1 | nack | 0x02 | 0xF1 | cmd2 | ack | response field ... - // ------+------+------+------+------+------+------+------+------+------------------------ - - if( mip_packet_descriptor_set(packet) == pending->_descriptor_set ) - { - mip_field field = {0}; - while( mip_field_next_in_packet(&field, packet) ) - { - // Not an ack/nack reply field, skip it. - if( mip_field_field_descriptor(&field) != MIP_REPLY_DESC_GLOBAL_ACK_NACK ) - continue; - - // Sanity check payload length before accessing it. - if( mip_field_payload_length(&field) != 2 ) - continue; - - const uint8_t* const payload = mip_field_payload(&field); - - const uint8_t cmd_descriptor = payload[MIP_INDEX_REPLY_DESCRIPTOR]; - const uint8_t ack_code = payload[MIP_INDEX_REPLY_ACK_CODE]; - - // Is this the right command reply? - if( pending->_field_descriptor != cmd_descriptor ) - continue; - - // Descriptor matches! - - uint8_t response_length = 0; - mip_field response_field; - - // If the command was ACK'd, check if response data is expected. - if( pending->_response_descriptor != 0x00 && ack_code == MIP_ACK_OK ) - { - // Look ahead one field for response data. - response_field = mip_field_next_after(&field); - if( mip_field_is_valid(&response_field) ) - { - const uint8_t response_descriptor = mip_field_field_descriptor(&response_field); - - // This is a wildcard to accept any response data descriptor. - // Needed when the response descriptor is not known or is wrong. - if( pending->_response_descriptor == MIP_REPLY_DESC_GLOBAL_ACK_NACK ) - pending->_response_descriptor = response_descriptor; - - // Make sure the response descriptor matches what is expected. - if( response_descriptor == pending->_response_descriptor ) - { - // Update the response_size field to reflect the actual size. - response_length = mip_field_payload_length(&response_field); - - // Skip this field when iterating for next ack/nack reply. - field = response_field; - } - } - } - - // Limit response data size to lesser of buffer size or actual response length. - pending->_response_length = (response_length < pending->_response_buffer_size) ? response_length : pending->_response_buffer_size; - - // Copy response data to the pending buffer (skip if response_field is invalid). - if( pending->_response_length > 0 ) - memcpy(pending->_response_buffer, mip_field_payload(&response_field), pending->_response_length); - - // pending->_ack_code = ack_code; - pending->_reply_time = timestamp; // Completion time - - return (enum mip_cmd_result)ack_code; - } - } - - // No matching reply descriptors in this packet. - - // Check for timeout - if( mip_pending_cmd_check_timeout(pending, timestamp) ) - { - pending->_response_length = 0; - // pending->_ack_code = MIP_NACK_COMMAND_TIMEOUT; - - // Must be last! - return MIP_STATUS_TIMEDOUT; - } - - return pending->_status; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Process an incoming packet and check for replies to pending commands. -/// -/// Call this from the Mip_parser callback, passing the arguments directly. -/// -///@param queue -///@param packet The received MIP packet. Assumed to be valid. -///@param timestamp The time the packet was received -/// -void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp) -{ - // Check if the packet is a command descriptor set. - const uint8_t descriptor_set = mip_packet_descriptor_set(packet); - if( descriptor_set >= 0x80 && descriptor_set < 0xF0 ) - return; - - if( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - - const enum mip_cmd_result status = process_fields_for_pending_cmd(pending, packet, queue->_base_timeout, timestamp); - - if( mip_cmd_result_is_finished(status) ) - { - queue->_first_pending_cmd = queue->_first_pending_cmd->_next; - - // This must be done last b/c it may trigger the thread which queued the command. - // The command could go out of scope or its attributes inspected. - pending->_status = status; - -#ifdef MIP_ENABLE_DIAGNOSTICS - if( mip_cmd_result_is_ack(status) ) - MIP_DIAG_INC(queue->_diag_cmds_acked, 1); - else if( mip_cmd_result_is_reply(status) ) - MIP_DIAG_INC(queue->_diag_cmds_nacked, 1); - else if( status == MIP_STATUS_TIMEDOUT ) - MIP_DIAG_INC(queue->_diag_cmds_timedout, 1); - else - MIP_DIAG_INC(queue->_diag_cmds_failed, 1); -#endif // MIP_ENABLE_DIAGNOSTICS - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Clears the command queue. -/// -/// This must be called from the same thread context as the update function. -/// -///@param queue -/// -void mip_cmd_queue_clear(mip_cmd_queue* queue) -{ - while( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - queue->_first_pending_cmd = pending->_next; - - // This may deallocate the pending command in another thread (make sure to fetch the next cmd first). - pending->_status = MIP_STATUS_CANCELLED; - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Call periodically to make sure commands time out if no packets are -/// received. -/// -/// Call this during the device update if no calls to mip_cmd_queue_process_packet -/// are made (e.g. because no packets were received). It is safe to call this -/// in either case. -/// -///@param queue -///@param now -/// -void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) -{ - if( queue->_first_pending_cmd ) - { - mip_pending_cmd* pending = queue->_first_pending_cmd; - - if( pending->_status == MIP_STATUS_PENDING ) - { - // Update the timeout to the timestamp of the timeout time. - pending->_timeout_time = now + queue->_base_timeout + pending->_extra_timeout; - pending->_status = MIP_STATUS_WAITING; - } - else if( mip_pending_cmd_check_timeout(pending, now) ) - { - queue->_first_pending_cmd = queue->_first_pending_cmd->_next; - - // Clear response length and mark when it timed out. - pending->_response_length = 0; - pending->_reply_time = now; - - // This must be last! - pending->_status = MIP_STATUS_TIMEDOUT; - - MIP_DIAG_INC(queue->_diag_cmds_timedout, 1); - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Sets the base reply timeout for all commands. -/// -/// The base reply timeout is the minimum time to wait for a reply. -/// Takes effect for any commands queued after this function call. -/// -///@param queue -///@param timeout -/// -void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout) -{ - queue->_base_timeout = timeout; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the base reply timeout for all commands. -/// -///@returns The minimum time to wait for a reply to any command. -/// -timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) -{ - return queue->_base_timeout; -} - - -#ifdef MIP_ENABLE_DIAGNOSTICS - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the number of commands ever put into the queue. -/// -/// In most cases this is the number of commands sent to the device. -/// -uint16_t mip_cmd_queue_diagnostic_cmds_queued(const mip_cmd_queue* queue) -{ - return queue->_diag_cmds_queued; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the number of commands that have failed for any reason. -/// -uint16_t mip_cmd_queue_diagnostic_cmds_failed(const mip_cmd_queue* queue) -{ - return (uint16_t)queue->_diag_cmds_nacked + queue->_diag_cmds_failed + queue->_diag_cmds_timedout; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the number of successful commands. -/// -/// Same as mip_cmd_queue_diagnostic_cmd_acks(). -/// -uint16_t mip_cmd_queue_diagnostic_cmds_successful(const mip_cmd_queue* queue) -{ - return queue->_diag_cmds_acked; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the number of successful commands. -/// -/// Same as mip_cmd_queue_diagnostic_cmds_successful(). -/// -uint16_t mip_cmd_queue_diagnostic_cmd_acks(const mip_cmd_queue* queue) -{ - return queue->_diag_cmds_acked; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the number of commands nack'd by the device. -/// -uint16_t mip_cmd_queue_diagnostic_cmd_nacks(const mip_cmd_queue* queue) -{ - return queue->_diag_cmds_nacked; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the number of commands that did not receive a reply within the -/// time limit. -/// -uint16_t mip_cmd_queue_diagnostic_cmd_timeouts(const mip_cmd_queue* queue) -{ - return queue->_diag_cmds_timedout; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Gets the number of command errors not caused by the device. -/// -uint16_t mip_cmd_queue_diagnostic_cmd_errors(const mip_cmd_queue* queue) -{ - return queue->_diag_cmds_failed; -} - -#endif // MIP_ENABLE_DIAGNOSTICS + +#include "mip_cmdqueue.h" + +#include "mip_field.h" +#include "mip_packet.h" + +#include +#include + + +#define MIP_REPLY_DESC_GLOBAL_ACK_NACK 0xF1 + +#define MIP_INDEX_REPLY_DESCRIPTOR 0 +#define MIP_INDEX_REPLY_ACK_CODE 1 + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending command with no reponse data or additional time. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +/// +void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, 0); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with extra timeout time. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param additional_time +/// Additional time on top of the base reply timeout for this specific command. +/// +void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with expected response data. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param response_descriptor +/// Optional response data descriptor. Use 0x00 if no data is expected. +///@param response_buffer +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Size of the response buffer. The response will be limited to this size. +/// +void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size) +{ + mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, response_descriptor, response_buffer, response_buffer_size, 0); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a pending mip commmand with all parameters. +/// +///@param cmd +///@param descriptor_set +/// Command descriptor set. +///@param field_descriptor +/// Command field descriptor. +///@param response_descriptor +/// Optional response data descriptor. Use 0x00 if no data is expected. +///@param response_buffer +/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. +///@param response_buffer_size +/// Size of the response buffer. The response will be limited to this size. +///@param additional_time +/// Additional time on top of the base reply timeout for this specific command. +/// +void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time) +{ + cmd->_next = NULL; + cmd->_response_buffer = NULL; + cmd->_extra_timeout = additional_time; + cmd->_descriptor_set = descriptor_set; + cmd->_field_descriptor = field_descriptor; + cmd->_response_descriptor = response_descriptor; + cmd->_response_buffer = response_buffer; + cmd->_response_buffer_size = response_buffer_size; + // cmd->_ack_code = 0xFF; // invalid + cmd->_status = MIP_STATUS_NONE; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the status of the pending command. +/// +///@see mip_cmd_status +/// +enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd) +{ + return cmd->_status; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the response descriptor. +/// +uint8_t mip_pending_cmd_response_descriptor(const mip_pending_cmd* cmd) +{ + return cmd->_response_descriptor; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the response payload pointer. +/// +/// This function may only be called after the command finishes with an ACK. +/// +const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd) +{ + assert(mip_cmd_result_is_finished(cmd->_status)); + + return cmd->_response_buffer; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the length of the response data. +/// +/// This function may only be called after the command finishes. +/// If the command completed with a NACK, or if it timed out, the response +/// length will be zero. +/// +uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) +{ + assert(mip_cmd_result_is_finished(cmd->_status)); + + return cmd->_response_length; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines how much time is remaining before the command times out. +/// +/// For most cases you should instead call mip_pending_cmd_check_timeout() to +/// know if the command has timed out or not. +/// +///@param cmd The command to check. Must be in MIP_STATUS_WAITING state. +///@param now The current timestamp. +/// +///@warning The command must be in the MIP_STATUS_WAITING state, otherwise the +/// result is unspecified. +/// +///@returns The time remaining before the command times out. The value will be +/// negative if the timeout time has passed. +/// +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now) +{ + assert(cmd->_status == MIP_STATUS_WAITING); + + // result <= 0 if timed out. + // Note: this still works with unsigned overflow. + return (int)(now - cmd->_timeout_time); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Checks if the command should time out. +/// +///@param cmd +///@param now Current time +/// +///@returns true if the command should time out. Only possible for MIP_STATUS_WAITING. +///@returns false if the command should not time out. +/// +bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now) +{ + if( cmd->_status == MIP_STATUS_WAITING ) + { + if( mip_pending_cmd_remaining_time(cmd, now) > 0 ) + { + return true; + } + } + + return false; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initializes a command queue. +/// +///@param queue +///@param base_reply_timeout +/// The minimum timeout given to all MIP commands. Additional time may be +/// given to specific commands which take longer. This is intended to be +/// used to accommodate long communication latencies, such as when using +/// a TCP connection. +/// +void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) +{ + queue->_first_pending_cmd = NULL; + queue->_base_timeout = base_reply_timeout; + + MIP_DIAG_ZERO(queue->_diag_cmds_queued); + MIP_DIAG_ZERO(queue->_diag_cmds_acked); + MIP_DIAG_ZERO(queue->_diag_cmds_nacked); + MIP_DIAG_ZERO(queue->_diag_cmds_timedout); + MIP_DIAG_ZERO(queue->_diag_cmds_failed); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Queue a command to wait for replies. +/// +///@param queue +///@param cmd Listens for replies to this command. +/// +///@warning The command must not be deallocated or go out of scope while the +/// mip_cmd_status_is_finished returns false. +/// +void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd) +{ + // For now only one command can be queued at a time. + if( queue->_first_pending_cmd ) + { + cmd->_status = MIP_STATUS_CANCELLED; + return; + } + + MIP_DIAG_INC(queue->_diag_cmds_queued, 1); + + cmd->_status = MIP_STATUS_PENDING; + queue->_first_pending_cmd = cmd; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Removes a pending command from the queue. +/// +///@internal +/// +///@param queue +///@param cmd +/// +void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd) +{ + if( queue->_first_pending_cmd == cmd ) + { + queue->_first_pending_cmd = NULL; + cmd->_status = MIP_STATUS_CANCELLED; + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Iterate over a packet, checking for replies to the pending command. +/// +///@internal +/// +///@param pending +/// Pending command which is awaiting replies +///@param packet +///@param base_timeout +///@param timestamp +/// +///@returns The new status of the pending command (the command status field is +/// not updated). The caller should set pending->_status to this value +/// after doing any additional processing requiring the pending struct. +/// +static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp) +{ + assert( pending->_status != MIP_STATUS_NONE ); // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here. + assert( !mip_cmd_result_is_finished(pending->_status) ); // Command shouldn't be finished yet - make sure the queue is processed properly. + + if( pending->_status == MIP_STATUS_PENDING ) + { + // Update the timeout to the timestamp of the timeout time. + pending->_timeout_time = timestamp + base_timeout + pending->_extra_timeout; + pending->_status = MIP_STATUS_WAITING; + } + + // ------+------+------+------+------+------+------+------+------+------------------------ + // ... | 0x02 | 0xF1 | cmd1 | nack | 0x02 | 0xF1 | cmd2 | ack | response field ... + // ------+------+------+------+------+------+------+------+------+------------------------ + + if( mip_packet_descriptor_set(packet) == pending->_descriptor_set ) + { + mip_field field = {0}; + while( mip_field_next_in_packet(&field, packet) ) + { + // Not an ack/nack reply field, skip it. + if( mip_field_field_descriptor(&field) != MIP_REPLY_DESC_GLOBAL_ACK_NACK ) + continue; + + // Sanity check payload length before accessing it. + if( mip_field_payload_length(&field) != 2 ) + continue; + + const uint8_t* const payload = mip_field_payload(&field); + + const uint8_t cmd_descriptor = payload[MIP_INDEX_REPLY_DESCRIPTOR]; + const uint8_t ack_code = payload[MIP_INDEX_REPLY_ACK_CODE]; + + // Is this the right command reply? + if( pending->_field_descriptor != cmd_descriptor ) + continue; + + // Descriptor matches! + + uint8_t response_length = 0; + mip_field response_field; + + // If the command was ACK'd, check if response data is expected. + if( pending->_response_descriptor != 0x00 && ack_code == MIP_ACK_OK ) + { + // Look ahead one field for response data. + response_field = mip_field_next_after(&field); + if( mip_field_is_valid(&response_field) ) + { + const uint8_t response_descriptor = mip_field_field_descriptor(&response_field); + + // This is a wildcard to accept any response data descriptor. + // Needed when the response descriptor is not known or is wrong. + if( pending->_response_descriptor == MIP_REPLY_DESC_GLOBAL_ACK_NACK ) + pending->_response_descriptor = response_descriptor; + + // Make sure the response descriptor matches what is expected. + if( response_descriptor == pending->_response_descriptor ) + { + // Update the response_size field to reflect the actual size. + response_length = mip_field_payload_length(&response_field); + + // Skip this field when iterating for next ack/nack reply. + field = response_field; + } + } + } + + // Limit response data size to lesser of buffer size or actual response length. + pending->_response_length = (response_length < pending->_response_buffer_size) ? response_length : pending->_response_buffer_size; + + // Copy response data to the pending buffer (skip if response_field is invalid). + if( pending->_response_length > 0 ) + memcpy(pending->_response_buffer, mip_field_payload(&response_field), pending->_response_length); + + // pending->_ack_code = ack_code; + pending->_reply_time = timestamp; // Completion time + + return (enum mip_cmd_result)ack_code; + } + } + + // No matching reply descriptors in this packet. + + // Check for timeout + if( mip_pending_cmd_check_timeout(pending, timestamp) ) + { + pending->_response_length = 0; + // pending->_ack_code = MIP_NACK_COMMAND_TIMEOUT; + + // Must be last! + return MIP_STATUS_TIMEDOUT; + } + + return pending->_status; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Process an incoming packet and check for replies to pending commands. +/// +/// Call this from the Mip_parser callback, passing the arguments directly. +/// +///@param queue +///@param packet The received MIP packet. Assumed to be valid. +///@param timestamp The time the packet was received +/// +void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp) +{ + // Check if the packet is a command descriptor set. + const uint8_t descriptor_set = mip_packet_descriptor_set(packet); + if( descriptor_set >= 0x80 && descriptor_set < 0xF0 ) + return; + + if( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + + const enum mip_cmd_result status = process_fields_for_pending_cmd(pending, packet, queue->_base_timeout, timestamp); + + if( mip_cmd_result_is_finished(status) ) + { + queue->_first_pending_cmd = queue->_first_pending_cmd->_next; + + // This must be done last b/c it may trigger the thread which queued the command. + // The command could go out of scope or its attributes inspected. + pending->_status = status; + +#ifdef MIP_ENABLE_DIAGNOSTICS + if( mip_cmd_result_is_ack(status) ) + MIP_DIAG_INC(queue->_diag_cmds_acked, 1); + else if( mip_cmd_result_is_reply(status) ) + MIP_DIAG_INC(queue->_diag_cmds_nacked, 1); + else if( status == MIP_STATUS_TIMEDOUT ) + MIP_DIAG_INC(queue->_diag_cmds_timedout, 1); + else + MIP_DIAG_INC(queue->_diag_cmds_failed, 1); +#endif // MIP_ENABLE_DIAGNOSTICS + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Clears the command queue. +/// +/// This must be called from the same thread context as the update function. +/// +///@param queue +/// +void mip_cmd_queue_clear(mip_cmd_queue* queue) +{ + while( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + queue->_first_pending_cmd = pending->_next; + + // This may deallocate the pending command in another thread (make sure to fetch the next cmd first). + pending->_status = MIP_STATUS_CANCELLED; + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Call periodically to make sure commands time out if no packets are +/// received. +/// +/// Call this during the device update if no calls to mip_cmd_queue_process_packet +/// are made (e.g. because no packets were received). It is safe to call this +/// in either case. +/// +///@param queue +///@param now +/// +void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) +{ + if( queue->_first_pending_cmd ) + { + mip_pending_cmd* pending = queue->_first_pending_cmd; + + if( pending->_status == MIP_STATUS_PENDING ) + { + // Update the timeout to the timestamp of the timeout time. + pending->_timeout_time = now + queue->_base_timeout + pending->_extra_timeout; + pending->_status = MIP_STATUS_WAITING; + } + else if( mip_pending_cmd_check_timeout(pending, now) ) + { + queue->_first_pending_cmd = queue->_first_pending_cmd->_next; + + // Clear response length and mark when it timed out. + pending->_response_length = 0; + pending->_reply_time = now; + + // This must be last! + pending->_status = MIP_STATUS_TIMEDOUT; + + MIP_DIAG_INC(queue->_diag_cmds_timedout, 1); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Sets the base reply timeout for all commands. +/// +/// The base reply timeout is the minimum time to wait for a reply. +/// Takes effect for any commands queued after this function call. +/// +///@param queue +///@param timeout +/// +void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout) +{ + queue->_base_timeout = timeout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the base reply timeout for all commands. +/// +///@returns The minimum time to wait for a reply to any command. +/// +timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) +{ + return queue->_base_timeout; +} + + +#ifdef MIP_ENABLE_DIAGNOSTICS + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands ever put into the queue. +/// +/// In most cases this is the number of commands sent to the device. +/// +uint16_t mip_cmd_queue_diagnostic_cmds_queued(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_queued; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands that have failed for any reason. +/// +uint16_t mip_cmd_queue_diagnostic_cmds_failed(const mip_cmd_queue* queue) +{ + return (uint16_t)queue->_diag_cmds_nacked + queue->_diag_cmds_failed + queue->_diag_cmds_timedout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of successful commands. +/// +/// Same as mip_cmd_queue_diagnostic_cmd_acks(). +/// +uint16_t mip_cmd_queue_diagnostic_cmds_successful(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_acked; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of successful commands. +/// +/// Same as mip_cmd_queue_diagnostic_cmds_successful(). +/// +uint16_t mip_cmd_queue_diagnostic_cmd_acks(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_acked; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands nack'd by the device. +/// +uint16_t mip_cmd_queue_diagnostic_cmd_nacks(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_nacked; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of commands that did not receive a reply within the +/// time limit. +/// +uint16_t mip_cmd_queue_diagnostic_cmd_timeouts(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_timedout; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Gets the number of command errors not caused by the device. +/// +uint16_t mip_cmd_queue_diagnostic_cmd_errors(const mip_cmd_queue* queue) +{ + return queue->_diag_cmds_failed; +} + +#endif // MIP_ENABLE_DIAGNOSTICS diff --git a/src/mip/mip_cmdqueue.h b/src/mip/mip_cmdqueue.h index 99ff096a1..fc0110935 100644 --- a/src/mip/mip_cmdqueue.h +++ b/src/mip/mip_cmdqueue.h @@ -63,7 +63,7 @@ void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_size, timeout_type additional_time); enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd); - +uint8_t mip_pending_cmd_response_descriptor(const mip_pending_cmd* cmd); const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd); uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd); diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 1db0813c3..e8e21ca91 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -21,6 +21,11 @@ struct Dispatcher : public C::mip_dispatcher ANY_DATA_SET = C::MIP_DISPATCH_ANY_DATA_SET, ANY_DESCRIPTOR = C::MIP_DISPATCH_ANY_DESCRIPTOR, }; + + void addHandler(DispatchHandler& handler) { C::mip_dispatcher_add_handler(this, &handler); } + void removeHandler(DispatchHandler& handler) { C::mip_dispatcher_remove_handler(this, &handler); } + + void removeAllHandlers() { C::mip_dispatcher_remove_all_handlers(this); } }; @@ -109,6 +114,9 @@ struct PendingCmd : public C::mip_pending_cmd /// CmdResult status() const { return C::mip_pending_cmd_status(this); } + ///@copydoc mip::C::mip_pending_cmd_response_descriptor + uint8_t responseDescriptor() const { return C::mip_pending_cmd_response_descriptor(this); } + ///@copydoc mip::C::mip_pending_cmd_response const uint8_t* response() const { return C::mip_pending_cmd_response(this); } @@ -135,6 +143,8 @@ template bool startCommand(C::mip_interface& device, C::mip_pending_c class Connection { public: + virtual ~Connection() {} + virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) = 0; @@ -243,8 +253,8 @@ class DeviceInterface : public C::mip_interface bool sendToDevice(const uint8_t* data, size_t length) { return C::mip_interface_send_to_device(this, data, length); } bool sendToDevice(const C::mip_packet& packet) { return sendToDevice(C::mip_packet_pointer(&packet), C::mip_packet_total_length(&packet)); } bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp) { return C::mip_interface_recv_from_device(this, buffer, max_length, wait_time, length_out, timestamp); } - bool update(bool blocking=false) { return C::mip_interface_update(this, blocking); } - bool defaultUpdate(bool blocking=false) { return C::mip_interface_default_update(this, blocking); } + bool update(Timeout wait_time=0) { return C::mip_interface_update(this, wait_time); } + bool defaultUpdate(Timeout wait_time=0) { return C::mip_interface_default_update(this, wait_time); } RemainingCount receiveBytes(const uint8_t* data, size_t length, Timestamp timestamp) { return C::mip_interface_receive_bytes(this, data, length, timestamp); } void receivePacket(const C::mip_packet& packet, Timestamp timestamp) { C::mip_interface_receive_packet(this, &packet, timestamp); } diff --git a/src/mip/mip_field.c b/src/mip/mip_field.c index e95336486..de7548430 100644 --- a/src/mip/mip_field.c +++ b/src/mip/mip_field.c @@ -117,10 +117,9 @@ mip_field mip_field_from_header_ptr(const uint8_t* header, uint8_t total_length, { mip_field field; - field._payload = header + MIP_INDEX_FIELD_PAYLOAD; - field._descriptor_set = descriptor_set; - // Default invalid values. + field._payload = NULL; + field._descriptor_set = descriptor_set; field._payload_length = 0; field._field_descriptor = 0x00; // This makes the field invalid. field._remaining_length = 0; @@ -139,6 +138,7 @@ mip_field mip_field_from_header_ptr(const uint8_t* header, uint8_t total_length, { field._field_descriptor = header[MIP_INDEX_FIELD_DESC]; field._payload_length = field_length - MIP_FIELD_HEADER_LENGTH; + field._payload = header + MIP_INDEX_FIELD_PAYLOAD; field._remaining_length = total_length - field_length; } } @@ -178,6 +178,9 @@ mip_field mip_field_first_from_packet(const mip_packet* packet) /// mip_field mip_field_next_after(const mip_field* field) { + // Payload length must be zero if payload is NULL. + assert(!(field->_payload == NULL) || (field->_payload_length == 0)); + const uint8_t* next_header = field->_payload + field->_payload_length; return mip_field_from_header_ptr(next_header, field->_remaining_length, field->_descriptor_set); diff --git a/src/mip/mip_packet.c b/src/mip/mip_packet.c index 92295f403..0f8bbc974 100644 --- a/src/mip/mip_packet.c +++ b/src/mip/mip_packet.c @@ -1,461 +1,461 @@ - -#include "mip_packet.h" -#include "mip_offsets.h" - -#include "definitions/descriptors.h" - -#include -#include - - -// -// Initialization -// - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initializes a MIP packet from an existing buffer. -/// -/// Use this when receiving or parsing MIP packets. -/// -/// The data in the buffer should be a valid or suspected MIP packet. -/// -///@param packet -///@param buffer -/// The data buffer containing the bytes for a MIP packet. Must be at -/// least MIP_PACKET_LENGTH_MIN bytes in size. -///@param length -/// The length of the data pointed to by buffer. -/// -///@note The data does not need to be a valid MIP packet, for instance to use -/// the mip_packet_is_sane() or mip_packet_is_valid() functions. However, if -/// it is NOT a valid MIP packet, the result of calling any accessor -/// function is unpredictable. In particular, if length is less than -/// MIP_PACKET_LENGTH_MIN bytes, calling the accessor functions is undefined -/// behavior. -/// -void mip_packet_from_buffer(mip_packet* packet, uint8_t* buffer, size_t length) -{ - assert(buffer != NULL); - - // Limit the length in case it's longer than a mip packer (or worse, longer than the buffer size field can hold) - if( length > MIP_PACKET_LENGTH_MAX ) - length = MIP_PACKET_LENGTH_MAX; - - packet->_buffer = buffer; - packet->_buffer_length = length; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Create a brand-new MIP packet in the given buffer. -/// -/// Use this along with the packet building functions to create MIP packets. -/// -///@param packet -///@param buffer -/// This is where the packet bytes will be stored. Must be at least -/// MIP_PACKET_LENGTH_MIN bytes in size. -///@param buffer_size -/// The size of buffer, in bytes. -///@param descriptor_set -/// The MIP descriptor set for the packet. -/// -void mip_packet_create(mip_packet* packet, uint8_t* buffer, size_t buffer_size, uint8_t descriptor_set) -{ - mip_packet_from_buffer(packet, buffer, buffer_size); - - if( buffer_size < MIP_PACKET_LENGTH_MIN ) - { - assert(false); // Buffer too small! - return; - } - - packet->_buffer[MIP_INDEX_SYNC1] = MIP_SYNC1; - packet->_buffer[MIP_INDEX_SYNC2] = MIP_SYNC2; - packet->_buffer[MIP_INDEX_DESCSET] = descriptor_set; - packet->_buffer[MIP_INDEX_LENGTH] = 0; -} - - - -// -// Accessors -// - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the MIP descriptor set for this packet. -/// -uint8_t mip_packet_descriptor_set(const mip_packet* packet) -{ - return packet->_buffer[MIP_INDEX_DESCSET]; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the length of the payload (MIP fields). -/// -uint8_t mip_packet_payload_length(const mip_packet* packet) -{ - return packet->_buffer[MIP_INDEX_LENGTH]; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the total length of the packet, in bytes. -/// -///@returns The length of the packet. Always at least MIP_PACKET_LENGTH_MIN. -/// -packet_length mip_packet_total_length(const mip_packet* packet) -{ - return mip_packet_payload_length(packet) + MIP_PACKET_LENGTH_MIN; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns a writable pointer to the data buffer. -/// -uint8_t* mip_packet_buffer(mip_packet* packet) -{ - return packet->_buffer; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns a pointer to the data buffer containing the packet. -/// -const uint8_t* mip_packet_pointer(const mip_packet* packet) -{ - return packet->_buffer; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns a pointer to the packet's payload (the first field). -/// -const uint8_t* mip_packet_payload(const mip_packet* packet) -{ - return packet->_buffer + MIP_INDEX_PAYLOAD; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the value of the checksum as written in the packet. -/// -/// This function does not compute the checksum. To do so, use -/// mip_packet_compute_checksum(). -/// -uint16_t mip_packet_checksum_value(const mip_packet* packet) -{ - const packet_length index = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; - - return ((uint16_t)(packet->_buffer[index+0]) << 8) | (uint16_t)(packet->_buffer[index+1]); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Computes the checksum of the MIP packet. -/// -///@returns The computed checksum value. -/// -uint16_t mip_packet_compute_checksum(const mip_packet* packet) -{ - uint8_t a = 0; - uint8_t b = 0; - - // mip_packet_total_length always returns at least MIP_PACKET_LENGTH_MIN so this - // subtraction is guaranteed to be safe. - const packet_length length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; - - for(packet_length i=0; i_buffer[i]; - b += a; - } - - return ((uint16_t)(a) << 8) | (uint16_t)(b); -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns true if the packet buffer is not NULL and is at least the -/// minimum size (MIP_PACKET_LENGTH_MIN). -/// -/// If the packet is not 'sane', then none of the mip_packet_* functions may be -/// used to access it (to do so is undefined behavior). This should never occur -/// in normal circumstances. -/// -bool mip_packet_is_sane(const mip_packet* packet) -{ - return packet->_buffer && (mip_packet_buffer_size(packet) >= MIP_PACKET_LENGTH_MIN); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns true if the packet is valid. -/// -/// A packet is valid if: -/// * mip_packet_is_sane() returns true, -/// * The descriptor set is not 0x00, and -/// * The checksum is valid. -/// -bool mip_packet_is_valid(const mip_packet* packet) -{ - if( !mip_packet_is_sane(packet) || (mip_packet_descriptor_set(packet) == 0x00) ) - return false; - - const uint16_t listed_checksum = mip_packet_checksum_value(packet); - const uint16_t computed_checksum = mip_packet_compute_checksum(packet); - - return listed_checksum == computed_checksum; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns true if the mip packet contains no payload. -/// -///@param packet -/// -///@returns true if the packet has a payload length of 0. -/// -bool mip_packet_is_empty(const mip_packet* packet) -{ - if( !mip_packet_is_sane(packet) ) - return true; - - return mip_packet_payload_length(packet) == 0; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the size of the buffer backing the MIP packet. -/// -///@note This is the BUFFER SIZE and not the packet length. -/// -packet_length mip_packet_buffer_size(const mip_packet* packet) -{ - return packet->_buffer_length; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns the remaining space available for more payload data. -/// -/// This is equal to the buffer size less the total packet length. -/// -remaining_count mip_packet_remaining_space(const mip_packet* packet) -{ - return mip_packet_buffer_size(packet) - mip_packet_total_length(packet); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Returns true if the packet is from a data descriptor set. -/// -///@see is_data_descriptor_set -/// -///@returns true if the packet contains data. -///@returns false if it contains commands or replies. -/// -bool mip_packet_is_data(const mip_packet* packet) -{ - return mip_is_data_descriptor_set(mip_packet_descriptor_set(packet)); -} - -// -// Packet Building -// - -//////////////////////////////////////////////////////////////////////////////// -///@brief Adds a pre-constructed MIP field to the packet. -/// -///~~~ -/// +--------------------+ -/// | Payload Bytes | -/// Len Desc +--------------------+ -/// | | | copy -/// Packet Buffer V V V -/// ---------------+------------+- -+- -+-- -+- -/// ... Header | Field | Len | Desc | Payload | -/// ---------------+------------+-----+------+---------------------+---------- -/// | | -/// End of last field ---------------> End of new field -///~~~ -/// -/// -///@param packet -///@param field_descriptor -/// The MIP field descriptor (e.g. command or data descriptor). -///@param payload -/// A pointer to the field payload data (without the header). -/// Can be NULL if payload_length is 0. -///@param payload_length -/// The length of the payload data. Must be less than or equal to -/// MIP_FIELD_PAYLOAD_LENGTH_MAX. Does not include the header. -/// -///@returns true if the field was added, or false if there was not enough space. -/// -bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length) -{ - uint8_t* payload_buffer; - remaining_count remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer); - if( remaining < 0 ) - return false; - - memcpy(payload_buffer, payload, payload_length); - - return true; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Allocate a MIP field within the packet and return the payload pointer. -/// -///~~~ -/// Len Desc .---> Payload ptr out -/// | | | -/// Packet Buffer V V | -/// ---------------+------------+- -+- -+---------------------+---------- -/// ... Header | Field | Len | Desc | (unwritten payload) | -/// ---------------+------------+-----+------+---------------------+---------- -/// | | -/// End of last field ---------------> End of new field -///~~~ -/// -///@param packet -///@param field_descriptor -/// The MIP field descriptor (e.g. command or data descriptor). -///@param payload_length -/// The requested length of the field payload (not including the header). -/// If the size is not known ahead of time, pass 0 and inspect the return -/// value to see how much payload data can be written. Then call -/// mip_packet_realloc_field() with the used size and same payload pointer. -///@param payload_ptr_out -/// A pointer to a pointer to the field payload. This will receive the -/// payload pointer into which data should be written. -/// -///@returns The amount of space remaining after allocating this field. If this -/// is negative, the field could not be allocated and the payload must -/// not be written. -/// -remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out) -{ - assert(payload_ptr_out != NULL); - // assert( payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); - - const remaining_count remaining = mip_packet_remaining_space(packet); - - const packet_length field_length = MIP_FIELD_HEADER_LENGTH + (packet_length)payload_length; - - *payload_ptr_out = NULL; - - if( field_length <= remaining ) - { - packet_length field_index = MIP_HEADER_LENGTH + mip_packet_payload_length(packet); - - packet->_buffer[MIP_INDEX_LENGTH] += field_length; - - packet->_buffer[field_index+MIP_INDEX_FIELD_LEN] = field_length; - packet->_buffer[field_index+MIP_INDEX_FIELD_DESC] = field_descriptor; - - *payload_ptr_out = &packet->_buffer[field_index + MIP_INDEX_FIELD_PAYLOAD]; - } - - return remaining - field_length; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Changes the size of the last field in the packet. -/// -/// Use this in conjunction with mip_packet_alloc_field() when the size of the -/// field is not known in advance. Pass a payload size of 0 to alloc_field and -/// check that the returned available space is sufficient, then write the -/// payload and call this function with the actual space used. -/// -///@param packet -///@param payload_ptr -/// Pointer to the field payload. This must be the same value returned -/// from alloc_field and must point to the last field. -///@param new_payload_length -/// Length of payload written. Generally it is an error for this to -/// exceed the actual amount of space available in the packet. In this -/// case, the packet is left with just the empty field and the return -/// value will be negative. -/// -///@returns The space remaining in the packet after changing the field size. -/// This will be negative if the new length did not fit. -/// -remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length) -{ - assert(payload_ptr != NULL); - assert( new_payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); - - uint8_t* field_ptr = payload_ptr - MIP_INDEX_FIELD_PAYLOAD; - const uint8_t old_field_length = field_ptr[MIP_INDEX_FIELD_LEN]; - const uint8_t new_field_length = new_payload_length + MIP_FIELD_HEADER_LENGTH; - - const remaining_count delta_length = new_field_length - old_field_length; - - remaining_count remaining = mip_packet_remaining_space(packet) + delta_length; - - if( remaining >= 0 ) - { - field_ptr[MIP_INDEX_FIELD_LEN] = new_field_length; - packet->_buffer[MIP_INDEX_LENGTH] += delta_length; - } - - return remaining; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Removes the last field from the packet after having allocated it. -/// -/// Use only after allocating a field with mip_packet_alloc_field to cancel it. -/// E.g. if it turns out that there isn't enough buffer space to write the -/// payload. -/// -///@param packet -///@param payload_ptr -/// Pointer to the field payload. This must be the same value returned -/// from alloc_field and must point to the last field. -/// -///@returns The remaining space in the packet after removing the field. -/// -remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr) -{ - assert(payload_ptr != NULL); - - uint8_t* field_ptr = payload_ptr - MIP_INDEX_FIELD_PAYLOAD; - const uint8_t old_payload_length = field_ptr[MIP_INDEX_FIELD_LEN]; - - packet->_buffer[MIP_INDEX_LENGTH] -= MIP_FIELD_HEADER_LENGTH + old_payload_length; - - return mip_packet_remaining_space(packet); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Prepares the packet for transmission by adding the checksum. -/// -/// This does not increase the total packet length since the checksum is always -/// implicitly included. -/// -///~~~ -/// Checksum -/// VVVV -/// ---------------+------------+------------+-----//-----+-- --+---- -/// ... Header | Field | Field | ... | (empty) | -/// ---------------+------------+------------+-----//-----+------------+---- -/// | | -/// End of last field | -/// Total Length -///~~~ -/// -void mip_packet_finalize(mip_packet* packet) -{ - uint16_t checksum = mip_packet_compute_checksum(packet); - packet_length length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; - - packet->_buffer[length+0] = checksum >> 8; - packet->_buffer[length+1] = checksum & 0xFF; -} - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Reinitialize the packet with the given descriptor set. -/// -/// This clears out all of the fields but keeps the same buffer. -/// -///@param packet -///@param descriptor_set New descriptor set. -/// -void mip_packet_reset(mip_packet* packet, uint8_t descriptor_set) -{ - mip_packet_create(packet, mip_packet_buffer(packet), mip_packet_buffer_size(packet), descriptor_set); -} - + +#include "mip_packet.h" +#include "mip_offsets.h" + +#include "definitions/descriptors.h" + +#include +#include + + +// +// Initialization +// + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initializes a MIP packet from an existing buffer. +/// +/// Use this when receiving or parsing MIP packets. +/// +/// The data in the buffer should be a valid or suspected MIP packet. +/// +///@param packet +///@param buffer +/// The data buffer containing the bytes for a MIP packet. Must be at +/// least MIP_PACKET_LENGTH_MIN bytes in size. +///@param length +/// The length of the data pointed to by buffer. +/// +///@note The data does not need to be a valid MIP packet, for instance to use +/// the mip_packet_is_sane() or mip_packet_is_valid() functions. However, if +/// it is NOT a valid MIP packet, the result of calling any accessor +/// function is unpredictable. In particular, if length is less than +/// MIP_PACKET_LENGTH_MIN bytes, calling the accessor functions is undefined +/// behavior. +/// +void mip_packet_from_buffer(mip_packet* packet, uint8_t* buffer, size_t length) +{ + assert(buffer != NULL); + + // Limit the length in case it's longer than a mip packer (or worse, longer than the buffer size field can hold) + if( length > MIP_PACKET_LENGTH_MAX ) + length = MIP_PACKET_LENGTH_MAX; + + packet->_buffer = buffer; + packet->_buffer_length = length; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Create a brand-new MIP packet in the given buffer. +/// +/// Use this along with the packet building functions to create MIP packets. +/// +///@param packet +///@param buffer +/// This is where the packet bytes will be stored. Must be at least +/// MIP_PACKET_LENGTH_MIN bytes in size. +///@param buffer_size +/// The size of buffer, in bytes. +///@param descriptor_set +/// The MIP descriptor set for the packet. +/// +void mip_packet_create(mip_packet* packet, uint8_t* buffer, size_t buffer_size, uint8_t descriptor_set) +{ + mip_packet_from_buffer(packet, buffer, buffer_size); + + if( buffer_size < MIP_PACKET_LENGTH_MIN ) + { + assert(false); // Buffer too small! + return; + } + + packet->_buffer[MIP_INDEX_SYNC1] = MIP_SYNC1; + packet->_buffer[MIP_INDEX_SYNC2] = MIP_SYNC2; + packet->_buffer[MIP_INDEX_DESCSET] = descriptor_set; + packet->_buffer[MIP_INDEX_LENGTH] = 0; +} + + + +// +// Accessors +// + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the MIP descriptor set for this packet. +/// +uint8_t mip_packet_descriptor_set(const mip_packet* packet) +{ + return packet->_buffer[MIP_INDEX_DESCSET]; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the length of the payload (MIP fields). +/// +uint8_t mip_packet_payload_length(const mip_packet* packet) +{ + return packet->_buffer[MIP_INDEX_LENGTH]; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the total length of the packet, in bytes. +/// +///@returns The length of the packet. Always at least MIP_PACKET_LENGTH_MIN. +/// +packet_length mip_packet_total_length(const mip_packet* packet) +{ + return mip_packet_payload_length(packet) + MIP_PACKET_LENGTH_MIN; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns a writable pointer to the data buffer. +/// +uint8_t* mip_packet_buffer(mip_packet* packet) +{ + return packet->_buffer; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns a pointer to the data buffer containing the packet. +/// +const uint8_t* mip_packet_pointer(const mip_packet* packet) +{ + return packet->_buffer; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns a pointer to the packet's payload (the first field). +/// +const uint8_t* mip_packet_payload(const mip_packet* packet) +{ + return packet->_buffer + MIP_INDEX_PAYLOAD; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the value of the checksum as written in the packet. +/// +/// This function does not compute the checksum. To do so, use +/// mip_packet_compute_checksum(). +/// +uint16_t mip_packet_checksum_value(const mip_packet* packet) +{ + const packet_length index = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; + + return ((uint16_t)(packet->_buffer[index+0]) << 8) | (uint16_t)(packet->_buffer[index+1]); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Computes the checksum of the MIP packet. +/// +///@returns The computed checksum value. +/// +uint16_t mip_packet_compute_checksum(const mip_packet* packet) +{ + uint8_t a = 0; + uint8_t b = 0; + + // mip_packet_total_length always returns at least MIP_PACKET_LENGTH_MIN so this + // subtraction is guaranteed to be safe. + const packet_length length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; + + for(packet_length i=0; i_buffer[i]; + b += a; + } + + return ((uint16_t)(a) << 8) | (uint16_t)(b); +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns true if the packet buffer is not NULL and is at least the +/// minimum size (MIP_PACKET_LENGTH_MIN). +/// +/// If the packet is not 'sane', then none of the mip_packet_* functions may be +/// used to access it (to do so is undefined behavior). This should never occur +/// in normal circumstances. +/// +bool mip_packet_is_sane(const mip_packet* packet) +{ + return packet->_buffer && (mip_packet_buffer_size(packet) >= MIP_PACKET_LENGTH_MIN); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns true if the packet is valid. +/// +/// A packet is valid if: +/// * mip_packet_is_sane() returns true, +/// * The descriptor set is not 0x00, and +/// * The checksum is valid. +/// +bool mip_packet_is_valid(const mip_packet* packet) +{ + if( !mip_packet_is_sane(packet) || (mip_packet_descriptor_set(packet) == 0x00) ) + return false; + + const uint16_t listed_checksum = mip_packet_checksum_value(packet); + const uint16_t computed_checksum = mip_packet_compute_checksum(packet); + + return listed_checksum == computed_checksum; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns true if the mip packet contains no payload. +/// +///@param packet +/// +///@returns true if the packet has a payload length of 0. +/// +bool mip_packet_is_empty(const mip_packet* packet) +{ + if( !mip_packet_is_sane(packet) ) + return true; + + return mip_packet_payload_length(packet) == 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the size of the buffer backing the MIP packet. +/// +///@note This is the BUFFER SIZE and not the packet length. +/// +packet_length mip_packet_buffer_size(const mip_packet* packet) +{ + return packet->_buffer_length; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns the remaining space available for more payload data. +/// +/// This is equal to the buffer size less the total packet length. +/// +remaining_count mip_packet_remaining_space(const mip_packet* packet) +{ + return mip_packet_buffer_size(packet) - mip_packet_total_length(packet); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Returns true if the packet is from a data descriptor set. +/// +///@see is_data_descriptor_set +/// +///@returns true if the packet contains data. +///@returns false if it contains commands or replies. +/// +bool mip_packet_is_data(const mip_packet* packet) +{ + return mip_is_data_descriptor_set(mip_packet_descriptor_set(packet)); +} + +// +// Packet Building +// + +//////////////////////////////////////////////////////////////////////////////// +///@brief Adds a pre-constructed MIP field to the packet. +/// +///~~~ +/// +--------------------+ +/// | Payload Bytes | +/// Len Desc +--------------------+ +/// | | | copy +/// Packet Buffer V V V +/// ---------------+------------+- -+- -+-- -+- +/// ... Header | Field | Len | Desc | Payload | +/// ---------------+------------+-----+------+---------------------+---------- +/// | | +/// End of last field ---------------> End of new field +///~~~ +/// +/// +///@param packet +///@param field_descriptor +/// The MIP field descriptor (e.g. command or data descriptor). +///@param payload +/// A pointer to the field payload data (without the header). +/// Can be NULL if payload_length is 0. +///@param payload_length +/// The length of the payload data. Must be less than or equal to +/// MIP_FIELD_PAYLOAD_LENGTH_MAX. Does not include the header. +/// +///@returns true if the field was added, or false if there was not enough space. +/// +bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length) +{ + uint8_t* payload_buffer; + remaining_count remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer); + if( remaining < 0 ) + return false; + + memcpy(payload_buffer, payload, payload_length); + + return true; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Allocate a MIP field within the packet and return the payload pointer. +/// +///~~~ +/// Len Desc .---> Payload ptr out +/// | | | +/// Packet Buffer V V | +/// ---------------+------------+- -+- -+---------------------+---------- +/// ... Header | Field | Len | Desc | (unwritten payload) | +/// ---------------+------------+-----+------+---------------------+---------- +/// | | +/// End of last field ---------------> End of new field +///~~~ +/// +///@param packet +///@param field_descriptor +/// The MIP field descriptor (e.g. command or data descriptor). +///@param payload_length +/// The requested length of the field payload (not including the header). +/// If the size is not known ahead of time, pass 0 and inspect the return +/// value to see how much payload data can be written. Then call +/// mip_packet_realloc_field() with the used size and same payload pointer. +///@param payload_ptr_out +/// A pointer to a pointer to the field payload. This will receive the +/// payload pointer into which data should be written. +/// +///@returns The amount of space remaining after allocating this field. If this +/// is negative, the field could not be allocated and the payload must +/// not be written. +/// +remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out) +{ + assert(payload_ptr_out != NULL); + // assert( payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); + + const remaining_count remaining = mip_packet_remaining_space(packet); + + const packet_length field_length = MIP_FIELD_HEADER_LENGTH + (packet_length)payload_length; + + *payload_ptr_out = NULL; + + if( field_length <= remaining ) + { + packet_length field_index = MIP_HEADER_LENGTH + mip_packet_payload_length(packet); + + packet->_buffer[MIP_INDEX_LENGTH] += field_length; + + packet->_buffer[field_index+MIP_INDEX_FIELD_LEN] = field_length; + packet->_buffer[field_index+MIP_INDEX_FIELD_DESC] = field_descriptor; + + *payload_ptr_out = &packet->_buffer[field_index + MIP_INDEX_FIELD_PAYLOAD]; + } + + return remaining - field_length; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Changes the size of the last field in the packet. +/// +/// Use this in conjunction with mip_packet_alloc_field() when the size of the +/// field is not known in advance. Pass a payload size of 0 to alloc_field and +/// check that the returned available space is sufficient, then write the +/// payload and call this function with the actual space used. +/// +///@param packet +///@param payload_ptr +/// Pointer to the field payload. This must be the same value returned +/// from alloc_field and must point to the last field. +///@param new_payload_length +/// Length of payload written. Generally it is an error for this to +/// exceed the actual amount of space available in the packet. In this +/// case, the packet is left with just the empty field and the return +/// value will be negative. +/// +///@returns The space remaining in the packet after changing the field size. +/// This will be negative if the new length did not fit. +/// +remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length) +{ + assert(payload_ptr != NULL); + assert( new_payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); + + uint8_t* field_ptr = payload_ptr - MIP_INDEX_FIELD_PAYLOAD; + const uint8_t old_field_length = field_ptr[MIP_INDEX_FIELD_LEN]; + const uint8_t new_field_length = new_payload_length + MIP_FIELD_HEADER_LENGTH; + + const remaining_count delta_length = new_field_length - old_field_length; + + const remaining_count remaining = mip_packet_remaining_space(packet) - delta_length; + + if( remaining >= 0 ) + { + field_ptr[MIP_INDEX_FIELD_LEN] = new_field_length; + packet->_buffer[MIP_INDEX_LENGTH] += delta_length; + } + + return remaining; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Removes the last field from the packet after having allocated it. +/// +/// Use only after allocating a field with mip_packet_alloc_field to cancel it. +/// E.g. if it turns out that there isn't enough buffer space to write the +/// payload. +/// +///@param packet +///@param payload_ptr +/// Pointer to the field payload. This must be the same value returned +/// from alloc_field and must point to the last field. +/// +///@returns The remaining space in the packet after removing the field. +/// +remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr) +{ + assert(payload_ptr != NULL); + + uint8_t* field_ptr = payload_ptr - MIP_INDEX_FIELD_PAYLOAD; + const uint8_t old_field_length = field_ptr[MIP_INDEX_FIELD_LEN]; + + packet->_buffer[MIP_INDEX_LENGTH] -= old_field_length; + + return mip_packet_remaining_space(packet); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Prepares the packet for transmission by adding the checksum. +/// +/// This does not increase the total packet length since the checksum is always +/// implicitly included. +/// +///~~~ +/// Checksum +/// VVVV +/// ---------------+------------+------------+-----//-----+-- --+---- +/// ... Header | Field | Field | ... | (empty) | +/// ---------------+------------+------------+-----//-----+------------+---- +/// | | +/// End of last field | +/// Total Length +///~~~ +/// +void mip_packet_finalize(mip_packet* packet) +{ + uint16_t checksum = mip_packet_compute_checksum(packet); + packet_length length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; + + packet->_buffer[length+0] = checksum >> 8; + packet->_buffer[length+1] = checksum & 0xFF; +} + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Reinitialize the packet with the given descriptor set. +/// +/// This clears out all of the fields but keeps the same buffer. +/// +///@param packet +///@param descriptor_set New descriptor set. +/// +void mip_packet_reset(mip_packet* packet, uint8_t descriptor_set) +{ + mip_packet_create(packet, mip_packet_buffer(packet), mip_packet_buffer_size(packet), descriptor_set); +} + diff --git a/src/mip/utils/serialization.c b/src/mip/utils/serialization.c index 4bb897a13..b6026b3e9 100644 --- a/src/mip/utils/serialization.c +++ b/src/mip/utils/serialization.c @@ -1,225 +1,275 @@ - -#include "serialization.h" -#include "../mip_field.h" - -#include - -#ifdef __cplusplus -namespace mip { -#endif - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a serialization struct for insertion into a buffer. -/// -///@param serializer -///@param buffer -/// Buffer into which data will be written. Can be NULL if buffer_size==0. -///@param buffer_size -/// Size of the buffer. Data will not be written beyond this size. -/// -void mip_serializer_init_insertion(mip_serializer* serializer, uint8_t* buffer, size_t buffer_size) -{ - serializer->_buffer = buffer; - serializer->_buffer_size = buffer_size; - serializer->_offset = 0; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a serialization struct for extract from a buffer. -/// -///@param serializer -///@param buffer -/// A pointer from which data will be read. -///@param buffer_size -/// Maximum number of bytes to be read from the buffer. -/// -void mip_serializer_init_extraction(mip_serializer* serializer, const uint8_t* buffer, size_t buffer_size) -{ - serializer->_buffer = (uint8_t*)buffer; - serializer->_buffer_size = buffer_size; - serializer->_offset = 0; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Initialize a serialization struct from a MIP field payload. -/// -///@param serializer -///@param field -/// -void mip_serializer_init_from_field(mip_serializer* serializer, const mip_field* field) -{ - mip_serializer_init_extraction(serializer, mip_field_payload(field), mip_field_payload_length(field)); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines the total length the buffer. -/// -///@param serializer -/// -///@returns The buffer size. -/// -size_t mip_serializer_capacity(const mip_serializer* serializer) -{ - return serializer->_buffer_size; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines the length of the data in the buffer. -/// -///@param serializer -/// -/// For insertion, returns how many bytes have been written. -/// For extraction, returns how many bytes have been read. -/// -///@note This may exceed the buffer size. Check mip_serializer_is_ok() before using -/// the data. -/// -size_t mip_serializer_length(const mip_serializer* serializer) -{ - return serializer->_offset; -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines the difference between the length and buffer size. -/// -///@param serializer -/// -/// For insertion, returns how many unwritten bytes remain in the buffer. -/// For extraction, returns how many bytes have not been read. -/// -///@note This can be a negative number if the application attempted to write -/// or read more data than contained in the buffer. This is not a bug and -/// it can be detected with the mip_serializer_is_ok() function. -/// -remaining_count mip_serializer_remaining(const mip_serializer* serializer) -{ - return mip_serializer_capacity(serializer) - mip_serializer_length(serializer); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the data read/written is less than the buffer size. -/// -/// If the application attempts to read or write beyond the end of the buffer -/// (as defined by the buffer_size passed to the init function), the read or -/// write will be a no-op but the offset will still be advanced. This allows -/// the condition to be detected. -/// -///@param serializer -/// -///@returns true if mip_serializer_remaining() >= 0. -/// -bool mip_serializer_is_ok(const mip_serializer* serializer) -{ - return mip_serializer_length(serializer) <= mip_serializer_capacity(serializer); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Determines if the number of remaining bytes is 0. -/// -/// Use this to determine if the entire buffer has been extracted. It is not -/// particularly useful for insertion. -/// -///@param serializer -/// -///@returns true if mip_serializer_remaining() == 0. -/// -bool mip_serializer_is_complete(const mip_serializer* serializer) -{ - return serializer->_offset == serializer->_buffer_size; -} - - -static void pack(uint8_t* buffer, const void* value, size_t size) -{ - for(size_t i=0; i_offset + sizeof(type); \ - if( offset <= serializer->_buffer_size ) \ - pack(&serializer->_buffer[serializer->_offset], &value, sizeof(type)); \ - serializer->_offset = offset; \ -} - -INSERT_MACRO(bool, bool ) -INSERT_MACRO(char, char ) -INSERT_MACRO(u8, uint8_t ) -INSERT_MACRO(u16, uint16_t) -INSERT_MACRO(u32, uint32_t) -INSERT_MACRO(u64, uint64_t) -INSERT_MACRO(s8, int8_t ) -INSERT_MACRO(s16, int16_t ) -INSERT_MACRO(s32, int32_t ) -INSERT_MACRO(s64, int64_t ) -INSERT_MACRO(float, float ) -INSERT_MACRO(double, double ) - - - -static void unpack(const uint8_t* buffer, void* value, size_t size) -{ - for(size_t i=0; i_offset + sizeof(type); \ - if( offset <= serializer->_buffer_size ) \ - unpack(&serializer->_buffer[serializer->_offset], value, sizeof(type)); \ - serializer->_offset = offset; \ -} - -EXTRACT_MACRO(bool, bool ) -EXTRACT_MACRO(char, char ) -EXTRACT_MACRO(u8, uint8_t ) -EXTRACT_MACRO(u16, uint16_t) -EXTRACT_MACRO(u32, uint32_t) -EXTRACT_MACRO(u64, uint64_t) -EXTRACT_MACRO(s8, int8_t ) -EXTRACT_MACRO(s16, int16_t ) -EXTRACT_MACRO(s32, int32_t ) -EXTRACT_MACRO(s64, int64_t ) -EXTRACT_MACRO(float, float ) -EXTRACT_MACRO(double, double ) - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Similar to extract_u8 but allows a maximum value to be specified. -/// -/// If the maximum count would be exceeded, an error is generated which causes -/// further extraction to fail. -/// -///@param serializer -///@param count_out -/// The counter value read from the buffer. -///@param max_count -/// The maximum value of the counter. If the count exceeds this, it is -/// set to 0 and the serializer is put into an error state. -/// -void extract_count(mip_serializer* serializer, uint8_t* count_out, uint8_t max_count) -{ - *count_out = 0; // Default to zero if extraction fails. - extract_u8(serializer, count_out); - if( *count_out > max_count ) - { - // This is an error condition which can occur if the device sends - // more array entries than the receiving structure expected. - // This does not imply any sort of protocol violation, only that - // the receiving array was not large enough. - // Either way, deserialization cannot continue because the following - // array extraction would leave some elements in the input buffer. - *count_out = 0; - serializer->_offset = SIZE_MAX; - } -} - -#ifdef __cplusplus -} // namespace mip -#endif + +#include "serialization.h" +#include "../mip_field.h" +#include "../mip_packet.h" +#include "../mip_offsets.h" + +#include +#include + +#ifdef __cplusplus +namespace mip { +#endif + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a serialization struct for insertion into a buffer. +/// +///@param serializer +///@param buffer +/// Buffer into which data will be written. Can be NULL if buffer_size==0. +///@param buffer_size +/// Size of the buffer. Data will not be written beyond this size. +/// +void mip_serializer_init_insertion(mip_serializer* serializer, uint8_t* buffer, size_t buffer_size) +{ + serializer->_buffer = buffer; + serializer->_buffer_size = buffer_size; + serializer->_offset = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a serialization struct for extraction from a buffer. +/// +///@param serializer +///@param buffer +/// A pointer from which data will be read. +///@param buffer_size +/// Maximum number of bytes to be read from the buffer. +/// +void mip_serializer_init_extraction(mip_serializer* serializer, const uint8_t* buffer, size_t buffer_size) +{ + serializer->_buffer = (uint8_t*)buffer; + serializer->_buffer_size = buffer_size; + serializer->_offset = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initializer a serialization struct for creation of a new field at the +/// end of the packet. +/// +///@note Call mip_serializer_finiish_new_field after the data has been serialized. +/// +///@note Only one new field per packet can be in progress at a time. +/// +///@param serializer +///@param packet +/// Allocate the new field on the end of this packet. +///@param field_descriptor +/// Field descriptor of the new field. +/// +void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet* packet, uint8_t field_descriptor) +{ + assert(packet); + + serializer->_buffer = NULL; + serializer->_buffer_size = 0; + serializer->_offset = 0; + + const remaining_count length = mip_packet_alloc_field(packet, field_descriptor, 0, &serializer->_buffer); + + if( length >= 0 ) + serializer->_buffer_size = length; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Call this after a new field allocated by mip_serializer_init_new_field +/// has been written. +/// +/// This will either finish the field, or abort it if the serializer overflowed. +/// +///@param serializer Must be created from mip_serializer_init_new_field. +///@param packet Must be the original packet. +/// +void mip_serializer_finish_new_field(const mip_serializer* serializer, mip_packet* packet) +{ + assert(packet); + + if( mip_serializer_is_ok(serializer) ) + mip_packet_realloc_last_field(packet, serializer->_buffer, serializer->_offset); + else if( serializer->_buffer ) + mip_packet_cancel_last_field(packet, serializer->_buffer); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Initialize a serialization struct from a MIP field payload. +/// +///@param serializer +///@param field +/// +void mip_serializer_init_from_field(mip_serializer* serializer, const mip_field* field) +{ + mip_serializer_init_extraction(serializer, mip_field_payload(field), mip_field_payload_length(field)); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines the total length the buffer. +/// +///@param serializer +/// +///@returns The buffer size. +/// +size_t mip_serializer_capacity(const mip_serializer* serializer) +{ + return serializer->_buffer_size; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines the length of the data in the buffer. +/// +///@param serializer +/// +/// For insertion, returns how many bytes have been written. +/// For extraction, returns how many bytes have been read. +/// +///@note This may exceed the buffer size. Check mip_serializer_is_ok() before using +/// the data. +/// +size_t mip_serializer_length(const mip_serializer* serializer) +{ + return serializer->_offset; +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines the difference between the length and buffer size. +/// +///@param serializer +/// +/// For insertion, returns how many unwritten bytes remain in the buffer. +/// For extraction, returns how many bytes have not been read. +/// +///@note This can be a negative number if the application attempted to write +/// or read more data than contained in the buffer. This is not a bug and +/// it can be detected with the mip_serializer_is_ok() function. +/// +remaining_count mip_serializer_remaining(const mip_serializer* serializer) +{ + return mip_serializer_capacity(serializer) - mip_serializer_length(serializer); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the data read/written is less than the buffer size. +/// +/// If the application attempts to read or write beyond the end of the buffer +/// (as defined by the buffer_size passed to the init function), the read or +/// write will be a no-op but the offset will still be advanced. This allows +/// the condition to be detected. +/// +///@param serializer +/// +///@returns true if mip_serializer_remaining() >= 0. +/// +bool mip_serializer_is_ok(const mip_serializer* serializer) +{ + return mip_serializer_length(serializer) <= mip_serializer_capacity(serializer); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the number of remaining bytes is 0. +/// +/// Use this to determine if the entire buffer has been extracted. It is not +/// particularly useful for insertion. +/// +///@param serializer +/// +///@returns true if mip_serializer_remaining() == 0. +/// +bool mip_serializer_is_complete(const mip_serializer* serializer) +{ + return serializer->_offset == serializer->_buffer_size; +} + + +static void pack(uint8_t* buffer, const void* value, size_t size) +{ + for(size_t i=0; i_offset + sizeof(type); \ + if( offset <= serializer->_buffer_size ) \ + pack(&serializer->_buffer[serializer->_offset], &value, sizeof(type)); \ + serializer->_offset = offset; \ +} + +INSERT_MACRO(bool, bool ) +INSERT_MACRO(char, char ) +INSERT_MACRO(u8, uint8_t ) +INSERT_MACRO(u16, uint16_t) +INSERT_MACRO(u32, uint32_t) +INSERT_MACRO(u64, uint64_t) +INSERT_MACRO(s8, int8_t ) +INSERT_MACRO(s16, int16_t ) +INSERT_MACRO(s32, int32_t ) +INSERT_MACRO(s64, int64_t ) +INSERT_MACRO(float, float ) +INSERT_MACRO(double, double ) + + + +static void unpack(const uint8_t* buffer, void* value, size_t size) +{ + for(size_t i=0; i_offset + sizeof(type); \ + if( offset <= serializer->_buffer_size ) \ + unpack(&serializer->_buffer[serializer->_offset], value, sizeof(type)); \ + serializer->_offset = offset; \ +} + +EXTRACT_MACRO(bool, bool ) +EXTRACT_MACRO(char, char ) +EXTRACT_MACRO(u8, uint8_t ) +EXTRACT_MACRO(u16, uint16_t) +EXTRACT_MACRO(u32, uint32_t) +EXTRACT_MACRO(u64, uint64_t) +EXTRACT_MACRO(s8, int8_t ) +EXTRACT_MACRO(s16, int16_t ) +EXTRACT_MACRO(s32, int32_t ) +EXTRACT_MACRO(s64, int64_t ) +EXTRACT_MACRO(float, float ) +EXTRACT_MACRO(double, double ) + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Similar to extract_u8 but allows a maximum value to be specified. +/// +/// If the maximum count would be exceeded, an error is generated which causes +/// further extraction to fail. +/// +///@param serializer +///@param count_out +/// The counter value read from the buffer. +///@param max_count +/// The maximum value of the counter. If the count exceeds this, it is +/// set to 0 and the serializer is put into an error state. +/// +void extract_count(mip_serializer* serializer, uint8_t* count_out, uint8_t max_count) +{ + *count_out = 0; // Default to zero if extraction fails. + extract_u8(serializer, count_out); + if( *count_out > max_count ) + { + // This is an error condition which can occur if the device sends + // more array entries than the receiving structure expected. + // This does not imply any sort of protocol violation, only that + // the receiving array was not large enough. + // Either way, deserialization cannot continue because the following + // array extraction would leave some elements in the input buffer. + *count_out = 0; + serializer->_offset = SIZE_MAX; + } +} + +#ifdef __cplusplus +} // namespace mip +#endif diff --git a/src/mip/utils/serialization.h b/src/mip/utils/serialization.h index 204dba1e4..076a98c5a 100644 --- a/src/mip/utils/serialization.h +++ b/src/mip/utils/serialization.h @@ -1,272 +1,277 @@ -#pragma once - -#include -#include - -#include "../mip_types.h" -#include "../mip_field.h" - -//////////////////////////////////////////////////////////////////////////////// -///@defgroup mip_serialization MIP Serialization -/// -///@brief Serialization Functions for reading and writing to byte buffers. -///@{ -///@defgroup mip_serialization_c MIP Serialization [C] -///@defgroup mip_serialization_cpp MIP Serialization [CPP] -///@} - -#ifdef __cplusplus -#include - -namespace mip { -namespace C { -extern "C" { -#endif // __cplusplus - - -//////////////////////////////////////////////////////////////////////////////// -///@addtogroup mip_serialization_c -/// -///@brief (De)serialization in C. -/// -///@{ - - -//////////////////////////////////////////////////////////////////////////////// -///@brief Structure used for serialization. -/// -///@note This should be considered an "opaque" structure; its members should be -/// considered an internal implementation detail. Avoid accessing them directly -/// as they are subject to change in future versions of this software. -/// -/// -typedef struct mip_serializer -{ - uint8_t* _buffer; ///<@private Pointer to data for serialization. - size_t _buffer_size; ///<@private Size of the buffer. - size_t _offset; ///<@private Current offset into the buffer (can exceed buffer_size!). -} mip_serializer; - -void mip_serializer_init_insertion(mip_serializer* serializer, uint8_t* buffer, size_t buffer_size); -void mip_serializer_init_extraction(mip_serializer* serializer, const uint8_t* buffer, size_t buffer_size); -void mip_serializer_init_from_field(mip_serializer* serializer, const mip_field* field); - -size_t mip_serializer_capacity(const mip_serializer* serializer); -size_t mip_serializer_length(const mip_serializer* serializer); -remaining_count mip_serializer_remaining(const mip_serializer* serializer); - -bool mip_serializer_is_ok(const mip_serializer* serializer); -bool mip_serializer_is_complete(const mip_serializer* serializer); - - -void insert_bool(mip_serializer* serializer, bool value); -void insert_char(mip_serializer* serializer, char value); - -void insert_u8 (mip_serializer* serializer, uint8_t value); -void insert_u16(mip_serializer* serializer, uint16_t value); -void insert_u32(mip_serializer* serializer, uint32_t value); -void insert_u64(mip_serializer* serializer, uint64_t value); - -void insert_s8 (mip_serializer* serializer, int8_t value); -void insert_s16(mip_serializer* serializer, int16_t value); -void insert_s32(mip_serializer* serializer, int32_t value); -void insert_s64(mip_serializer* serializer, int64_t value); - -void insert_float (mip_serializer* serializer, float value); -void insert_double(mip_serializer* serializer, double value); - - -void extract_bool(mip_serializer* serializer, bool* value); -void extract_char(mip_serializer* serializer, char* value); - -void extract_u8 (mip_serializer* serializer, uint8_t* value); -void extract_u16(mip_serializer* serializer, uint16_t* value); -void extract_u32(mip_serializer* serializer, uint32_t* value); -void extract_u64(mip_serializer* serializer, uint64_t* value); - -void extract_s8 (mip_serializer* serializer, int8_t* value); -void extract_s16(mip_serializer* serializer, int16_t* value); -void extract_s32(mip_serializer* serializer, int32_t* value); -void extract_s64(mip_serializer* serializer, int64_t* value); - -void extract_float (mip_serializer* serializer, float* value); -void extract_double(mip_serializer* serializer, double* value); - -void extract_count(mip_serializer* serializer, uint8_t* count_out, uint8_t max_count); - -///@} -//////////////////////////////////////////////////////////////////////////////// - -#ifdef __cplusplus -} // extern "C" -} // namespace C - -//////////////////////////////////////////////////////////////////////////////// -///@addtogroup mip_serialization_cpp -/// -///@brief (De)serialization in C++. -/// -/// There are two overloaded functions defined in the mip namespace, insert and -/// extract. The signature for each is as follows: -///@code{.cpp} -/// void mip::insert(Serializer& serializer, Type value); -/// voie mip::extract(Serializer& serializer, Type value); -///@endcode -/// Where `Type` is a struct or numeric type. -/// -/// There are overloads for all of the MIP definition types: -///@li Command, response, and data fields -///@li Enums, bitfields, and nested structs for commands -/// -/// Additionally, there are overloads with a different signature which allow -/// one to avoid creating a Serializer object every time. These overloads -/// create a serializer internally and pass it on to the regular version. -///@code{.cpp} -/// template bool mip::insert(const T& value, uint8_t* buffer, size_t bufferSize); -/// template bool mip::Field::extract(T& value); -///@endcode -/// This makes it easy to extract data from a field: -///@code{.cpp} -/// data_sensor::ScaledAccel accel; -/// MipField field(...); -/// if( field.extract(accel) ) -/// { -/// // Do something with accel data -/// printf("Accel X=%f\n", accel.scaled_accel[0]); -/// } -///@endcode -/// -///@{ - -//////////////////////////////////////////////////////////////////////////////// -///@brief Serialization class. -/// -class Serializer : public C::mip_serializer -{ -public: - Serializer(uint8_t* buffer, size_t size, size_t offset=0) { C::mip_serializer_init_insertion(this, buffer, size); this->_offset = offset; } - Serializer(const uint8_t* buffer, size_t size, size_t offset=0) { C::mip_serializer_init_extraction(this, const_cast(buffer), size); this->_offset = offset; } - - size_t capacity() const { return C::mip_serializer_capacity(this); } - size_t length() const { return C::mip_serializer_length(this); } - RemainingCount remaining() const { return C::mip_serializer_remaining(this); } - - bool isOk() const { return C::mip_serializer_is_ok(this); } - bool isComplete() const { return C::mip_serializer_is_complete(this); } - - operator const void*() const { return isOk() ? this : nullptr; } - bool operator!() const { return !isOk(); } -}; - - - -inline void insert(Serializer& serializer, bool value) { return C::insert_bool (&serializer, value); } -inline void insert(Serializer& serializer, char value) { return C::insert_char (&serializer, value); } -inline void insert(Serializer& serializer, uint8_t value) { return C::insert_u8 (&serializer, value); } -inline void insert(Serializer& serializer, uint16_t value) { return C::insert_u16 (&serializer, value); } -inline void insert(Serializer& serializer, uint32_t value) { return C::insert_u32 (&serializer, value); } -inline void insert(Serializer& serializer, uint64_t value) { return C::insert_u64 (&serializer, value); } -inline void insert(Serializer& serializer, int8_t value) { return C::insert_s8 (&serializer, value); } -inline void insert(Serializer& serializer, int16_t value) { return C::insert_s16 (&serializer, value); } -inline void insert(Serializer& serializer, int32_t value) { return C::insert_s32 (&serializer, value); } -inline void insert(Serializer& serializer, int64_t value) { return C::insert_s64 (&serializer, value); } -inline void insert(Serializer& serializer, float value) { return C::insert_float (&serializer, value); } -inline void insert(Serializer& serializer, double value) { return C::insert_double(&serializer, value); } - -//////////////////////////////////////////////////////////////////////////////// -///@brief Inserts an enum into the buffer. -/// -///@tparam Enum The type of the enum to serialize. Must be a c++ typed enum. -/// -///@param serializer The serialization instance. -///@param value The enum to insert. -/// -template -typename std::enable_if< std::is_enum::value, void>::type -/*void*/ insert(Serializer& serializer, Enum value) { return insert(serializer, static_cast< typename std::underlying_type::type >(value) ); } - -//////////////////////////////////////////////////////////////////////////////// -///@brief Insert the given value into the buffer. -/// -/// If the buffer has insufficient space, this function returns false and the -/// contents of buffer may be partially modified. -/// -///@param value Value to insert. -///@param buffer Buffer to udpate with the value. -///@param bufferSize Size of the buffer. -/// -///@returns true if sufficient space was available, false otherwise. -/// -template -bool insert(const T& value, uint8_t* buffer, size_t bufferSize) -{ - Serializer serializer(buffer, bufferSize); - insert(serializer, value); - return !!serializer; -} - -inline void extract(Serializer& serializer, bool& value) { return C::extract_bool (&serializer, &value); } -inline void extract(Serializer& serializer, char& value) { return C::extract_char (&serializer, &value); } -inline void extract(Serializer& serializer, uint8_t& value) { return C::extract_u8 (&serializer, &value); } -inline void extract(Serializer& serializer, uint16_t& value) { return C::extract_u16 (&serializer, &value); } -inline void extract(Serializer& serializer, uint32_t& value) { return C::extract_u32 (&serializer, &value); } -inline void extract(Serializer& serializer, uint64_t& value) { return C::extract_u64 (&serializer, &value); } -inline void extract(Serializer& serializer, int8_t& value) { return C::extract_s8 (&serializer, &value); } -inline void extract(Serializer& serializer, int16_t& value) { return C::extract_s16 (&serializer, &value); } -inline void extract(Serializer& serializer, int32_t& value) { return C::extract_s32 (&serializer, &value); } -inline void extract(Serializer& serializer, int64_t& value) { return C::extract_s64 (&serializer, &value); } -inline void extract(Serializer& serializer, float& value) { return C::extract_float (&serializer, &value); } -inline void extract(Serializer& serializer, double& value) { return C::extract_double(&serializer, &value); } - -//////////////////////////////////////////////////////////////////////////////// -///@brief Extract an enum from the buffer. -/// -///@tparam Enum The type of the enum to deserialize. Must be a c++ typed enum. -/// -///@param serializer The serialization instance. -///@param[out] value The enum to populate. -/// -template -typename std::enable_if< std::is_enum::value, void>::type -/*void*/ extract(Serializer& serializer, Enum& value) { - typename std::underlying_type::type tmp; - extract(serializer, tmp); - value = static_cast(tmp); -} - -//////////////////////////////////////////////////////////////////////////////// -///@brief Extract the value given a buffer, size, and starting offset. -/// -///@param[out] value_out -/// This parameter will be filled with the extracted value. -///@param buffer -/// A pointer to the raw data. -///@param bufferSize -/// Length of the buffer, or the relevant data within the buffer. -///@param offset -/// Start reading from this offset in the buffer. Default 0. -///@param exact_size -/// If true, exactly bufferSize bytes must be used in order for the return -/// value to be true. -/// -///@returns True if the extraction was successful, false otherwise. "Success" -/// means the supplied data was sufficient. If exact_size is true, then -/// this function only returns true if exactly bufferSize bytes were -/// consumed. -/// -template -bool extract(T& value_out, const uint8_t* buffer, size_t bufferSize, size_t offset=0, bool exact_size=false) -{ - Serializer serializer(buffer, bufferSize, offset); - extract(serializer, value_out); - return exact_size ? serializer.isComplete() : serializer.isOk(); -} - -///@} -///@} -//////////////////////////////////////////////////////////////////////////////// - -} // namespace mip -#endif // __cplusplus - +#pragma once + +#include +#include + +#include "../mip_field.h" +#include "../mip_packet.h" +#include "../mip_types.h" + +//////////////////////////////////////////////////////////////////////////////// +///@defgroup mip_serialization MIP Serialization +/// +///@brief Serialization Functions for reading and writing to byte buffers. +///@{ +///@defgroup mip_serialization_c MIP Serialization [C] +///@defgroup mip_serialization_cpp MIP Serialization [CPP] +///@} + +#ifdef __cplusplus +#include + +namespace mip { +namespace C { +extern "C" { +#endif // __cplusplus + + + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_serialization_c +/// +///@brief (De)serialization in C. +/// +///@{ + + +//////////////////////////////////////////////////////////////////////////////// +///@brief Structure used for serialization. +/// +///@note This should be considered an "opaque" structure; its members should be +/// considered an internal implementation detail. Avoid accessing them directly +/// as they are subject to change in future versions of this software. +/// +/// +typedef struct mip_serializer +{ + uint8_t* _buffer; ///<@private Pointer to data for serialization. + size_t _buffer_size; ///<@private Size of the buffer. + size_t _offset; ///<@private Current offset into the buffer (can exceed buffer_size!). +} mip_serializer; + +void mip_serializer_init_insertion(mip_serializer* serializer, uint8_t* buffer, size_t buffer_size); +void mip_serializer_init_extraction(mip_serializer* serializer, const uint8_t* buffer, size_t buffer_size); +void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet* packet, uint8_t field_descriptor); +void mip_serializer_finish_new_field(const mip_serializer* serializer, mip_packet* packet); +void mip_serializer_init_from_field(mip_serializer* serializer, const mip_field* field); + +size_t mip_serializer_capacity(const mip_serializer* serializer); +size_t mip_serializer_length(const mip_serializer* serializer); +remaining_count mip_serializer_remaining(const mip_serializer* serializer); + +bool mip_serializer_is_ok(const mip_serializer* serializer); +bool mip_serializer_is_complete(const mip_serializer* serializer); + + +void insert_bool(mip_serializer* serializer, bool value); +void insert_char(mip_serializer* serializer, char value); + +void insert_u8 (mip_serializer* serializer, uint8_t value); +void insert_u16(mip_serializer* serializer, uint16_t value); +void insert_u32(mip_serializer* serializer, uint32_t value); +void insert_u64(mip_serializer* serializer, uint64_t value); + +void insert_s8 (mip_serializer* serializer, int8_t value); +void insert_s16(mip_serializer* serializer, int16_t value); +void insert_s32(mip_serializer* serializer, int32_t value); +void insert_s64(mip_serializer* serializer, int64_t value); + +void insert_float (mip_serializer* serializer, float value); +void insert_double(mip_serializer* serializer, double value); + + +void extract_bool(mip_serializer* serializer, bool* value); +void extract_char(mip_serializer* serializer, char* value); + +void extract_u8 (mip_serializer* serializer, uint8_t* value); +void extract_u16(mip_serializer* serializer, uint16_t* value); +void extract_u32(mip_serializer* serializer, uint32_t* value); +void extract_u64(mip_serializer* serializer, uint64_t* value); + +void extract_s8 (mip_serializer* serializer, int8_t* value); +void extract_s16(mip_serializer* serializer, int16_t* value); +void extract_s32(mip_serializer* serializer, int32_t* value); +void extract_s64(mip_serializer* serializer, int64_t* value); + +void extract_float (mip_serializer* serializer, float* value); +void extract_double(mip_serializer* serializer, double* value); + +void extract_count(mip_serializer* serializer, uint8_t* count_out, uint8_t max_count); + +///@} +//////////////////////////////////////////////////////////////////////////////// + +#ifdef __cplusplus +} // extern "C" +} // namespace C + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_serialization_cpp +/// +///@brief (De)serialization in C++. +/// +/// There are two overloaded functions defined in the mip namespace, insert and +/// extract. The signature for each is as follows: +///@code{.cpp} +/// void mip::insert(Serializer& serializer, Type value); +/// voie mip::extract(Serializer& serializer, Type value); +///@endcode +/// Where `Type` is a struct or numeric type. +/// +/// There are overloads for all of the MIP definition types: +///@li Command, response, and data fields +///@li Enums, bitfields, and nested structs for commands +/// +/// Additionally, there are overloads with a different signature which allow +/// one to avoid creating a Serializer object every time. These overloads +/// create a serializer internally and pass it on to the regular version. +///@code{.cpp} +/// template bool mip::insert(const T& value, uint8_t* buffer, size_t bufferSize); +/// template bool mip::Field::extract(T& value); +///@endcode +/// This makes it easy to extract data from a field: +///@code{.cpp} +/// data_sensor::ScaledAccel accel; +/// MipField field(...); +/// if( field.extract(accel) ) +/// { +/// // Do something with accel data +/// printf("Accel X=%f\n", accel.scaled_accel[0]); +/// } +///@endcode +/// +///@{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief Serialization class. +/// +class Serializer : public C::mip_serializer +{ +public: + Serializer(C::mip_packet& packet, uint8_t newFieldDescriptor) { C::mip_serializer_init_new_field(this, &packet, newFieldDescriptor); } + Serializer(uint8_t* buffer, size_t size, size_t offset=0) { C::mip_serializer_init_insertion(this, buffer, size); this->_offset = offset; } + Serializer(const uint8_t* buffer, size_t size, size_t offset=0) { C::mip_serializer_init_extraction(this, const_cast(buffer), size); this->_offset = offset; } + + size_t capacity() const { return C::mip_serializer_capacity(this); } + size_t length() const { return C::mip_serializer_length(this); } + RemainingCount remaining() const { return C::mip_serializer_remaining(this); } + + bool isOk() const { return C::mip_serializer_is_ok(this); } + bool isComplete() const { return C::mip_serializer_is_complete(this); } + + operator const void*() const { return isOk() ? this : nullptr; } + bool operator!() const { return !isOk(); } +}; + + + +inline void insert(Serializer& serializer, bool value) { return C::insert_bool (&serializer, value); } +inline void insert(Serializer& serializer, char value) { return C::insert_char (&serializer, value); } +inline void insert(Serializer& serializer, uint8_t value) { return C::insert_u8 (&serializer, value); } +inline void insert(Serializer& serializer, uint16_t value) { return C::insert_u16 (&serializer, value); } +inline void insert(Serializer& serializer, uint32_t value) { return C::insert_u32 (&serializer, value); } +inline void insert(Serializer& serializer, uint64_t value) { return C::insert_u64 (&serializer, value); } +inline void insert(Serializer& serializer, int8_t value) { return C::insert_s8 (&serializer, value); } +inline void insert(Serializer& serializer, int16_t value) { return C::insert_s16 (&serializer, value); } +inline void insert(Serializer& serializer, int32_t value) { return C::insert_s32 (&serializer, value); } +inline void insert(Serializer& serializer, int64_t value) { return C::insert_s64 (&serializer, value); } +inline void insert(Serializer& serializer, float value) { return C::insert_float (&serializer, value); } +inline void insert(Serializer& serializer, double value) { return C::insert_double(&serializer, value); } + +//////////////////////////////////////////////////////////////////////////////// +///@brief Inserts an enum into the buffer. +/// +///@tparam Enum The type of the enum to serialize. Must be a c++ typed enum. +/// +///@param serializer The serialization instance. +///@param value The enum to insert. +/// +template +typename std::enable_if< std::is_enum::value, void>::type +/*void*/ insert(Serializer& serializer, Enum value) { return insert(serializer, static_cast< typename std::underlying_type::type >(value) ); } + +//////////////////////////////////////////////////////////////////////////////// +///@brief Insert the given value into the buffer. +/// +/// If the buffer has insufficient space, this function returns false and the +/// contents of buffer may be partially modified. +/// +///@param value Value to insert. +///@param buffer Buffer to udpate with the value. +///@param bufferSize Size of the buffer. +/// +///@returns true if sufficient space was available, false otherwise. +/// +template +bool insert(const T& value, uint8_t* buffer, size_t bufferSize) +{ + Serializer serializer(buffer, bufferSize); + insert(serializer, value); + return !!serializer; +} + +inline void extract(Serializer& serializer, bool& value) { return C::extract_bool (&serializer, &value); } +inline void extract(Serializer& serializer, char& value) { return C::extract_char (&serializer, &value); } +inline void extract(Serializer& serializer, uint8_t& value) { return C::extract_u8 (&serializer, &value); } +inline void extract(Serializer& serializer, uint16_t& value) { return C::extract_u16 (&serializer, &value); } +inline void extract(Serializer& serializer, uint32_t& value) { return C::extract_u32 (&serializer, &value); } +inline void extract(Serializer& serializer, uint64_t& value) { return C::extract_u64 (&serializer, &value); } +inline void extract(Serializer& serializer, int8_t& value) { return C::extract_s8 (&serializer, &value); } +inline void extract(Serializer& serializer, int16_t& value) { return C::extract_s16 (&serializer, &value); } +inline void extract(Serializer& serializer, int32_t& value) { return C::extract_s32 (&serializer, &value); } +inline void extract(Serializer& serializer, int64_t& value) { return C::extract_s64 (&serializer, &value); } +inline void extract(Serializer& serializer, float& value) { return C::extract_float (&serializer, &value); } +inline void extract(Serializer& serializer, double& value) { return C::extract_double(&serializer, &value); } + +//////////////////////////////////////////////////////////////////////////////// +///@brief Extract an enum from the buffer. +/// +///@tparam Enum The type of the enum to deserialize. Must be a c++ typed enum. +/// +///@param serializer The serialization instance. +///@param[out] value The enum to populate. +/// +template +typename std::enable_if< std::is_enum::value, void>::type +/*void*/ extract(Serializer& serializer, Enum& value) { + typename std::underlying_type::type tmp; + extract(serializer, tmp); + value = static_cast(tmp); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Extract the value given a buffer, size, and starting offset. +/// +///@param[out] value_out +/// This parameter will be filled with the extracted value. +///@param buffer +/// A pointer to the raw data. +///@param bufferSize +/// Length of the buffer, or the relevant data within the buffer. +///@param offset +/// Start reading from this offset in the buffer. Default 0. +///@param exact_size +/// If true, exactly bufferSize bytes must be used in order for the return +/// value to be true. +/// +///@returns True if the extraction was successful, false otherwise. "Success" +/// means the supplied data was sufficient. If exact_size is true, then +/// this function only returns true if exactly bufferSize bytes were +/// consumed. +/// +template +bool extract(T& value_out, const uint8_t* buffer, size_t bufferSize, size_t offset=0, bool exact_size=false) +{ + Serializer serializer(buffer, bufferSize, offset); + extract(serializer, value_out); + return exact_size ? serializer.isComplete() : serializer.isOk(); +} + +///@} +///@} +//////////////////////////////////////////////////////////////////////////////// + +} // namespace mip +#endif // __cplusplus + //////////////////////////////////////////////////////////////////////////////// \ No newline at end of file From e48ae0f4d5f183e974d936d967fd5a86934f7c79 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 12:23:23 -0400 Subject: [PATCH 040/252] Fix compiler warnings. --- test/mip/test_mip.cpp | 2 +- test/mip/test_mip_fields.c | 2 +- test/mip/test_mip_random.c | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/test/mip/test_mip.cpp b/test/mip/test_mip.cpp index f00ce3287..190ae84b6 100644 --- a/test/mip/test_mip.cpp +++ b/test/mip/test_mip.cpp @@ -9,7 +9,7 @@ using namespace mip; - +using namespace mip::C; uint8_t packetBuffer[PACKET_LENGTH_MAX]; uint8_t parseBuffer[1024]; diff --git a/test/mip/test_mip_fields.c b/test/mip/test_mip_fields.c index e33dcac32..156c7a4be 100644 --- a/test/mip/test_mip_fields.c +++ b/test/mip/test_mip_fields.c @@ -61,7 +61,7 @@ int main(int argc, const char* argv[]) // Now iterate the fields and verify they match. unsigned int scanned_fields = 0; - for(struct mip_field field = mip_field_from_packet(&packet); mip_field_is_valid(&field); mip_field_next(&field)) + for(struct mip_field field = mip_field_first_from_packet(&packet); mip_field_is_valid(&field); mip_field_next(&field)) { const uint8_t test_field_desc = mip_field_field_descriptor(&field); const uint8_t test_desc_set = mip_field_descriptor_set(&field); diff --git a/test/mip/test_mip_random.c b/test/mip/test_mip_random.c index 6449a40f3..492659b71 100644 --- a/test/mip/test_mip_random.c +++ b/test/mip/test_mip_random.c @@ -166,13 +166,13 @@ int main(int argc, const char* argv[]) { num_errors++; error = true; - fprintf(stderr, "Parsed packet has wrong timestamp %d\n", parsed_packet_timestamp); + fprintf(stderr, "Parsed packet has wrong timestamp %ld\n", parsed_packet_timestamp); } last_parsed = num_packets_parsed; if( error ) { - fprintf(stderr, " packet_size=%ld, last_count=%ld, extra=%ld, start_time=%d\n", packet_size, count, extra, start_time); + fprintf(stderr, " packet_size=%ld, last_count=%ld, extra=%ld, start_time=%ld\n", packet_size, count, extra, start_time); fprintf(stderr, " Sent chunks:"); for(unsigned int d=0; d Date: Mon, 10 Apr 2023 12:31:14 -0400 Subject: [PATCH 041/252] TCP socket open/close implementations. --- src/mip/utils/tcp_socket.c | 105 +++++++++++++++++++++++++++++++------ src/mip/utils/tcp_socket.h | 17 +++++- 2 files changed, 105 insertions(+), 17 deletions(-) diff --git a/src/mip/utils/tcp_socket.c b/src/mip/utils/tcp_socket.c index 16db78dd1..c2b9e268c 100644 --- a/src/mip/utils/tcp_socket.c +++ b/src/mip/utils/tcp_socket.c @@ -2,7 +2,18 @@ #include "tcp_socket.h" #ifdef WIN32 + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif + +#include +#include + +#include "../mip_loggging.h" + #else + #include #include #include @@ -10,13 +21,15 @@ #include #include #include + +#define INVALID_SOCKET -1 + #endif -bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port, size_t timeout_ms) +static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, uint16_t port, unsigned int timeout_ms) { -#ifdef WIN32 - return false; // TODO: Windows -#else + socket_ptr->handle = INVALID_SOCKET; + // https://man7.org/linux/man-pages/man3/getaddrinfo.3.html struct addrinfo hints, *info; memset(&hints, 0, sizeof(hints)); @@ -35,19 +48,19 @@ bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port for(struct addrinfo* addr=info; addr!=NULL; addr=addr->ai_next) { socket_ptr->handle = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); //socket(addr->ai_family, addr->ai_socktype, addr->ai_protocol); - if( socket_ptr->handle == -1 ) + if( socket_ptr->handle == INVALID_SOCKET ) continue; if( connect(socket_ptr->handle, addr->ai_addr, addr->ai_addrlen) == 0 ) break; close(socket_ptr->handle); - socket_ptr->handle = -1; + socket_ptr->handle = INVALID_SOCKET; } freeaddrinfo(info); - if( socket_ptr->handle == -1 ) + if( socket_ptr->handle == INVALID_SOCKET ) return false; struct timeval timeout_option; @@ -61,23 +74,83 @@ bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port return false; return true; -#endif } -bool tcp_socket_close(tcp_socket* socket_ptr) +bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port, unsigned int timeout_ms) { #ifdef WIN32 - return false; // TODO: Windows -#else - if( socket_ptr->handle != -1 ) + + // Initialize winsock for each connection since there's no global init function. + // This is safe to do multiple times, as long as it's shutdown the same number of times. + WSAData wsaData; + int result = WSAStartup(MAKEWORD(2,2), &wsaData); + if(result != 0) { - close(socket_ptr->handle); - socket_ptr->handle = -1; - return true; + MIP_LOG_ERROR("WSAStartup() failed: %d\n", result); + return false; } - else + +// struct addrinfo* address = NULL; +// struct addrinfo* ptr = NULL; +// struct addrinfo hints; +// +// ZeroMemory(&hints, sizeof(hints)); +// hints.ai_family = AF_UNSPEC; +// hints.ai_socktype = SOCK_STREAM; +// hints.ai_protocol = IPPROTO_TCP; +// +// result = getaddrinfo(hostname, port, &hints, &address); +// if(result != 0) +// { +// MIP_LOG_WARNING("getaddrinfo() failed for hostname=%s, port=%d: %d\n", hostname, port, result); +// WSACleanup(); +// return false; +// } +// +// *socket_ptr = socket(address->ai_family, address->ai_socktype, address->ai_protocol); +// if(*socket_ptr == INVALID_SOCKET) +// { +// MIP_LOG_WARNING("socket() failed for hostname=%s, port=%d: %d\n", hostname, port, WSAGetLastError()); +// freeaddrinfo(address); +// WSACleanup(); +// return false; +// } +// +// +// +// result = connect(*socket_ptr, address->ai_addr, (int)address->ai_addrlen); +// +// freeaddrinfo(address); +// +// if(result == SOCKET_ERROR) +// { +// closesocket(*socket_ptr); +// *socket_ptr = INVALID_SOCKET; +// WSACleanup(); +// return false; +// } +// +// return true; + +#endif + + return tcp_socket_open_common(socket_ptr, hostname, port ,timeout_ms); +} + +bool tcp_socket_close(tcp_socket* socket_ptr) +{ + if( socket_ptr->handle == INVALID_SOCKET ) return false; + +#ifdef WIN32 + closesocket(socket_ptr->handle); + WSACleanup(); // See tcp_socket_open +#else + close(socket_ptr->handle); #endif + + socket_ptr->handle = INVALID_SOCKET; + return true; } bool tcp_socket_send(tcp_socket* socket_ptr, const void* buffer, size_t num_bytes, size_t* bytes_written) diff --git a/src/mip/utils/tcp_socket.h b/src/mip/utils/tcp_socket.h index f2dc86f5c..7260cce88 100644 --- a/src/mip/utils/tcp_socket.h +++ b/src/mip/utils/tcp_socket.h @@ -1,5 +1,15 @@ #pragma once +#ifdef WIN32 + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif + +#include + +#endif + #include #include #include @@ -22,11 +32,16 @@ extern "C" { typedef struct tcp_socket { +#ifdef WIN32 + SOCKET handle; +#else int handle; +#endif } tcp_socket; -bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port, size_t timeout_ms); + +bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port, unsigned int timeout_ms); bool tcp_socket_close(tcp_socket* socket_ptr); bool tcp_socket_send(tcp_socket* socket_ptr, const void* buffer, size_t num_bytes, size_t* bytes_written); bool tcp_socket_recv(tcp_socket* socket_ptr, void* buffer, size_t num_bytes, size_t* bytes_read); From 567650236a483d02d5e539bb4a7dd4e45c239b90 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 13:04:23 -0400 Subject: [PATCH 042/252] Fix warnings in mip_logging.h when no additional parameters are used. --- src/mip/mip_logging.h | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/mip/mip_logging.h b/src/mip/mip_logging.h index 5182d4810..b847b729a 100644 --- a/src/mip/mip_logging.h +++ b/src/mip/mip_logging.h @@ -50,7 +50,7 @@ void* mip_logging_user_data(); void mip_logging_log(mip_log_level level, const char* fmt, ...); //////////////////////////////////////////////////////////////////////////////// -///@brief Helper macro used to initialize the MIP logger. +///@brief Helper macro used to initialize the MIP logger. /// This function does not need to be called unless the user wants logging /// ///@param callback The callback to execute when there is data to log @@ -69,9 +69,9 @@ void mip_logging_log(mip_log_level level, const char* fmt, ...); ///@copydetails mip::C::mip_log_callback /// #ifdef MIP_ENABLE_LOGGING -#define MIP_LOG_LOG(level, fmt, ...) mip_logging_log(level, fmt, __VA_ARGS__) +#define MIP_LOG_LOG(level, ...) mip_logging_log(level, __VA_ARGS__) #else -#define MIP_LOG_LOG(level, fmt, ...) (void)0 +#define MIP_LOG_LOG(level, ...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// @@ -82,54 +82,54 @@ void mip_logging_log(mip_log_level level, const char* fmt, ...); ///@param ... Variadic args used to populate the fmt string /// #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_FATAL -#define MIP_LOG_FATAL(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_FATAL, fmt, __VA_ARGS__) +#define MIP_LOG_FATAL(...) MIP_LOG_LOG(MIP_LOG_LEVEL_FATAL, __VA_ARGS__) #else -#define MIP_LOG_FATAL(fmt, ...) (void)0 +#define MIP_LOG_FATAL(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at error level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_ERROR -#define MIP_LOG_ERROR(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_ERROR, fmt, __VA_ARGS__) +#define MIP_LOG_ERROR(...) MIP_LOG_LOG(MIP_LOG_LEVEL_ERROR, __VA_ARGS__) #else -#define MIP_LOG_ERROR(fmt, ...) (void)0 +#define MIP_LOG_ERROR(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at warn level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_WARN -#define MIP_LOG_WARN(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_WARN, fmt, __VA_ARGS__) +#define MIP_LOG_WARN(...) MIP_LOG_LOG(MIP_LOG_LEVEL_WARN, __VA_ARGS__) #else -#define MIP_LOG_WARN(fmt, ...) (void)0 +#define MIP_LOG_WARN(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at info level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_INFO -#define MIP_LOG_INFO(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_INFO, fmt, __VA_ARGS__) +#define MIP_LOG_INFO(...) MIP_LOG_LOG(MIP_LOG_LEVEL_INFO, __VA_ARGS__) #else -#define MIP_LOG_INFO(fmt, ...) (void)0 +#define MIP_LOG_INFO(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at debug level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_DEBUG -#define MIP_LOG_DEBUG(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_DEBUG, fmt, __VA_ARGS__) +#define MIP_LOG_DEBUG(...) MIP_LOG_LOG(MIP_LOG_LEVEL_DEBUG, __VA_ARGS__) #else -#define MIP_LOG_DEBUG(fmt, ...) (void)0 +#define MIP_LOG_DEBUG(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at trace level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_TRACE -#define MIP_LOG_TRACE(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_TRACE, fmt, __VA_ARGS__) +#define MIP_LOG_TRACE(...) MIP_LOG_LOG(MIP_LOG_LEVEL_TRACE, __VA_ARGS__) #else -#define MIP_LOG_TRACE(fmt, ...) (void)0 +#define MIP_LOG_TRACE(...) (void)0 #endif ///@} From 86446432ca0b1ccf7f786d486276df7d25442cff Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 14:11:27 -0400 Subject: [PATCH 043/252] Generate MIP definitions from branches/cv7_ins@54708. --- src/mip/definitions/commands_3dm.h | 32 +++++++++++++++++----------- src/mip/definitions/commands_3dm.hpp | 8 ++++++- 2 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 45fe8ec8a..0bc2ad40d 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -956,21 +956,27 @@ static const mip_3dm_gpio_config_command_feature MIP_3DM_GPIO_CONFIG_COMMAND_FEA static const mip_3dm_gpio_config_command_feature MIP_3DM_GPIO_CONFIG_COMMAND_FEATURE_PPS = 2; ///< Pulse per second input or output. static const mip_3dm_gpio_config_command_feature MIP_3DM_GPIO_CONFIG_COMMAND_FEATURE_ENCODER = 3; ///< Motor encoder/odometer input. static const mip_3dm_gpio_config_command_feature MIP_3DM_GPIO_CONFIG_COMMAND_FEATURE_TIMESTAMP = 4; ///< Precision Timestamping. Use with Event Trigger Configuration (0x0C,0x2E). -static const mip_3dm_gpio_config_command_feature MIP_3DM_GPIO_CONFIG_COMMAND_FEATURE_POWER = 5; ///< Controls the device power state (e.g. enter low power mode). +static const mip_3dm_gpio_config_command_feature MIP_3DM_GPIO_CONFIG_COMMAND_FEATURE_UART = 5; ///< UART data or control lines. typedef uint8_t mip_3dm_gpio_config_command_behavior; -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UNUSED = 0; ///< Use 0 unless otherwise specified. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_GPIO_INPUT = 1; ///< Pin will be an input. This can be used to stream or poll the value and is the default setting. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_GPIO_OUTPUT_LOW = 2; ///< Pin is an output initially in the LOW state. This state will be restored during system startup if the configuration is saved. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_GPIO_OUTPUT_HIGH = 3; ///< Pin is an output initially in the HIGH state. This state will be restored during system startup if the configuration is saved. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_PPS_INPUT = 1; ///< Pin will receive the pulse-per-second signal. Only one pin can have this behavior. This will only work if the PPS Source command is configured to GPIO. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_PPS_OUTPUT = 2; ///< Pin will transmit the pulse-per-second signal from the device. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_ENCODER_A = 1; ///< Encoder "A" quadrature input. Only one pin can have this behavior. The last command to set this behavior will take precedence. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_ENCODER_B = 2; ///< Encoder "B" quadrature input. Only one pin can have this behavior. The last command to set this behavior will take precedence. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_RISING = 1; ///< Rising edges will be timestamped. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_FALLING = 2; ///< Falling edges will be timestamped. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_EITHER = 3; ///< Both rising and falling edges will be timestamped. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_POWER_SHUTDOWN = 1; ///< A logic 1 applied to the pin will place the device in low-power mode. A full restart is executed after the signal is removed. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UNUSED = 0; ///< Use 0 unless otherwise specified. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_GPIO_INPUT = 1; ///< Pin will be an input. This can be used to stream or poll the value and is the default setting. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_GPIO_OUTPUT_LOW = 2; ///< Pin is an output initially in the LOW state. This state will be restored during system startup if the configuration is saved. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_GPIO_OUTPUT_HIGH = 3; ///< Pin is an output initially in the HIGH state. This state will be restored during system startup if the configuration is saved. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_PPS_INPUT = 1; ///< Pin will receive the pulse-per-second signal. Only one pin can have this behavior. This will only work if the PPS Source command is configured to GPIO. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_PPS_OUTPUT = 2; ///< Pin will transmit the pulse-per-second signal from the device. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_ENCODER_A = 1; ///< Encoder "A" quadrature input. Only one pin can have this behavior. The last command to set this behavior will take precedence. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_ENCODER_B = 2; ///< Encoder "B" quadrature input. Only one pin can have this behavior. The last command to set this behavior will take precedence. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_RISING = 1; ///< Rising edges will be timestamped. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_FALLING = 2; ///< Falling edges will be timestamped. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_EITHER = 3; ///< Both rising and falling edges will be timestamped. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_DEFAULT = 1; ///< UART transmit line (auto-select port). +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_DEFAULT = 2; ///< UART receive line (auto-select port). +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_AUX1 = 33; ///< UART transmit line, port 2 (aux port 1). +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_AUX1 = 34; ///< UART receive line, port 2 (aux port 1). +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_AUX2 = 49; ///< UART transmit line, port 3 (aux port 2). +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_AUX2 = 50; ///< UART receive line, port 3 (aux port 2). +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_POWER_SHUTDOWN = 1; ///< A logic 1 applied to the pin will place the device in low-power mode. A full restart is executed after the signal is removed. typedef uint8_t mip_3dm_gpio_config_command_pin_mode; static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_NONE = 0x00; diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 845637b38..afe519d41 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -1172,7 +1172,7 @@ struct GpioConfig PPS = 2, ///< Pulse per second input or output. ENCODER = 3, ///< Motor encoder/odometer input. TIMESTAMP = 4, ///< Precision Timestamping. Use with Event Trigger Configuration (0x0C,0x2E). - POWER = 5, ///< Controls the device power state (e.g. enter low power mode). + UART = 5, ///< UART data or control lines. }; enum class Behavior : uint8_t @@ -1188,6 +1188,12 @@ struct GpioConfig TIMESTAMP_RISING = 1, ///< Rising edges will be timestamped. TIMESTAMP_FALLING = 2, ///< Falling edges will be timestamped. TIMESTAMP_EITHER = 3, ///< Both rising and falling edges will be timestamped. + UART_TX_DEFAULT = 1, ///< UART transmit line (auto-select port). + UART_RX_DEFAULT = 2, ///< UART receive line (auto-select port). + UART_TX_AUX1 = 33, ///< UART transmit line, port 2 (aux port 1). + UART_RX_AUX1 = 34, ///< UART receive line, port 2 (aux port 1). + UART_TX_AUX2 = 49, ///< UART transmit line, port 3 (aux port 2). + UART_RX_AUX2 = 50, ///< UART receive line, port 3 (aux port 2). POWER_SHUTDOWN = 1, ///< A logic 1 applied to the pin will place the device in low-power mode. A full restart is executed after the signal is removed. }; From 65ad7c48e470860ba96f5bcd5bb1416693bc55cf Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 16:17:17 -0400 Subject: [PATCH 044/252] Generate MIP definitions from branches/cv7_ins@54768. --- src/mip/definitions/commands_filter.c | 65 +++++++++++++++++++++++++ src/mip/definitions/commands_filter.cpp | 64 ++++++++++++++++++++++++ src/mip/definitions/commands_filter.h | 25 ++++++++++ src/mip/definitions/commands_filter.hpp | 29 +++++++++++ src/mip/definitions/data_filter.c | 50 +++++++++++++++++++ src/mip/definitions/data_filter.cpp | 41 ++++++++++++++++ src/mip/definitions/data_filter.h | 25 ++++++++++ src/mip/definitions/data_filter.hpp | 33 +++++++++++++ 8 files changed, 332 insertions(+) diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index ccbca3b69..df37cf95b 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -5058,6 +5058,71 @@ mip_cmd_result mip_filter_set_initial_heading(struct mip_interface* device, floa return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SET_INITIAL_HEADING, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self) +{ + insert_u8(serializer, self->source_id); + + insert_double(serializer, self->gps_time); + + insert_u16(serializer, self->gps_week); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + insert_u8(serializer, self->reserved[i]); + +} +void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self) +{ + extract_u8(serializer, &self->source_id); + + extract_double(serializer, &self->gps_time); + + extract_u16(serializer, &self->gps_week); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + extract_u8(serializer, &self->reserved[i]); + +} + +mip_cmd_result mip_filter_vehicle_frame_vel_with_unc_and_timestamp(struct mip_interface* device, uint8_t source_id, double gps_time, uint16_t gps_week, const float* velocity, const float* velocity_uncertainty, const uint8_t* reserved) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_u8(&serializer, source_id); + + insert_double(&serializer, gps_time); + + insert_u16(&serializer, gps_week); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, velocity[i]); + + assert(velocity_uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, velocity_uncertainty[i]); + + assert(reserved || (4 == 0)); + for(unsigned int i=0; i < 4; i++) + insert_u8(&serializer, reserved[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 7048f63d0..c26e36d99 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -4581,6 +4581,70 @@ CmdResult setInitialHeading(C::mip_interface& device, float heading) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SET_INITIAL_HEADING, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self) +{ + insert(serializer, self.source_id); + + insert(serializer, self.gps_time); + + insert(serializer, self.gps_week); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + insert(serializer, self.reserved[i]); + +} +void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self) +{ + extract(serializer, self.source_id); + + extract(serializer, self.gps_time); + + extract(serializer, self.gps_week); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + extract(serializer, self.reserved[i]); + +} + +CmdResult vehicleFrameVelWithUncAndTimestamp(C::mip_interface& device, uint8_t sourceId, double gpsTime, uint16_t gpsWeek, const float* velocity, const float* velocityUncertainty, const uint8_t* reserved) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, sourceId); + + insert(serializer, gpsTime); + + insert(serializer, gpsWeek); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, velocity[i]); + + assert(velocityUncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, velocityUncertainty[i]); + + assert(reserved || (4 == 0)); + for(unsigned int i=0; i < 4; i++) + insert(serializer, reserved[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} } // namespace commands_filter } // namespace mip diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 32f1eb72f..65e774d58 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -95,6 +95,7 @@ enum MIP_CMD_DESC_WHEELED_VEHICLE_CONSTRAINT_CONTROL = 0x63, MIP_CMD_DESC_GNSS_ANTENNA_CALIBRATION_CONTROL = 0x64, MIP_CMD_DESC_SENSOR_TO_VEHICLE_CALIBRATION_CONTROL = 0x65, + MIP_CMD_DESC_FILTER_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x66, MIP_REPLY_DESC_FILTER_VEHICLE_DYNAMICS_MODE = 0x80, MIP_REPLY_DESC_FILTER_SENSOR2VEHICLE_ROTATION_EULER = 0x81, @@ -1969,6 +1970,30 @@ void extract_mip_filter_set_initial_heading_command(struct mip_serializer* seria mip_cmd_result mip_filter_set_initial_heading(struct mip_interface* device, float heading); ///@} /// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_vehicle_frame_vel_with_unc_and_timestamp (0x0D,0x66) Vehicle Frame Vel With Unc And Timestamp [C] +/// Send and estimate of velocity in the vehicle frame at specified time, with associated uncertainties, to be incorporated by the Navigation Filter in the state estimation process. +/// The source_id field is used to differentiate between different physical measurement sources; note that source_id == 0 is reserved, and will result an invalid command response. +/// +///@{ + +struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command +{ + uint8_t source_id; ///< Source ID for this estimate + double gps_time; ///< [seconds] + uint16_t gps_week; ///< [GPS week number, not modulus 1024] + float velocity[3]; ///< [meters/second] + float velocity_uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + uint8_t reserved[4]; + +}; +typedef struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command; +void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(struct mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self); +void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(struct mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self); + +mip_cmd_result mip_filter_vehicle_frame_vel_with_unc_and_timestamp(struct mip_interface* device, uint8_t source_id, double gps_time, uint16_t gps_week, const float* velocity, const float* velocity_uncertainty, const uint8_t* reserved); +///@} +/// ///@} ///@} diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 06bebc646..0abc5773a 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -94,6 +94,7 @@ enum CMD_VEHICLE_CONSTRAINT_CONTROL = 0x63, CMD_ANTENNA_CALIBRATION_CONTROL = 0x64, CMD_TO_VEHICLE_CALIBRATION_CONTROL = 0x65, + CMD_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x66, REPLY_VEHICLE_DYNAMICS_MODE = 0x80, REPLY_SENSOR2VEHICLE_ROTATION_EULER = 0x81, @@ -2452,6 +2453,34 @@ void extract(Serializer& serializer, SetInitialHeading& self); CmdResult setInitialHeading(C::mip_interface& device, float heading); ///@} /// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_vehicle_frame_vel_with_unc_and_timestamp (0x0D,0x66) Vehicle Frame Vel With Unc And Timestamp [CPP] +/// Send and estimate of velocity in the vehicle frame at specified time, with associated uncertainties, to be incorporated by the Navigation Filter in the state estimation process. +/// The source_id field is used to differentiate between different physical measurement sources; note that source_id == 0 is reserved, and will result an invalid command response. +/// +///@{ + +struct VehicleFrameVelWithUncAndTimestamp +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP; + + static const bool HAS_FUNCTION_SELECTOR = false; + + uint8_t source_id = 0; ///< Source ID for this estimate + double gps_time = 0; ///< [seconds] + uint16_t gps_week = 0; ///< [GPS week number, not modulus 1024] + float velocity[3] = {0}; ///< [meters/second] + float velocity_uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + uint8_t reserved[4] = {0}; + +}; +void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self); +void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self); + +CmdResult vehicleFrameVelWithUncAndTimestamp(C::mip_interface& device, uint8_t sourceId, double gpsTime, uint16_t gpsWeek, const float* velocity, const float* velocityUncertainty, const uint8_t* reserved); +///@} +/// ///@} ///@} diff --git a/src/mip/definitions/data_filter.c b/src/mip/definitions/data_filter.c index f719207eb..634bdae87 100644 --- a/src/mip/definitions/data_filter.c +++ b/src/mip/definitions/data_filter.c @@ -1687,6 +1687,56 @@ void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags( *self = tmp; } +void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self) +{ + insert_u8(serializer, self->source_id); + + insert_double(serializer, self->gps_time); + + insert_u16(serializer, self->gps_week); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + insert_u8(serializer, self->reserved[i]); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self) +{ + extract_u8(serializer, &self->source_id); + + extract_double(serializer, &self->gps_time); + + extract_u16(serializer, &self->gps_week); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + extract_u8(serializer, &self->reserved[i]); + + extract_u16(serializer, &self->valid_flags); + +} +bool extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data_from_field(const mip_field* field, void* ptr) +{ + assert(ptr); + mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self = ptr; + struct mip_serializer serializer; + mip_serializer_init_from_field(&serializer, field); + extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(&serializer, self); + return mip_serializer_is_complete(&serializer); +} + #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/data_filter.cpp b/src/mip/definitions/data_filter.cpp index fdb85f32b..87eaca530 100644 --- a/src/mip/definitions/data_filter.cpp +++ b/src/mip/definitions/data_filter.cpp @@ -1084,6 +1084,47 @@ void extract(Serializer& serializer, GnssDualAntennaStatus& self) } +void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self) +{ + insert(serializer, self.source_id); + + insert(serializer, self.gps_time); + + insert(serializer, self.gps_week); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + insert(serializer, self.reserved[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self) +{ + extract(serializer, self.source_id); + + extract(serializer, self.gps_time); + + extract(serializer, self.gps_week); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity_uncertainty[i]); + + for(unsigned int i=0; i < 4; i++) + extract(serializer, self.reserved[i]); + + extract(serializer, self.valid_flags); + +} + } // namespace data_filter } // namespace mip diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index 9dfe6eedd..633132e2c 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -92,6 +92,7 @@ enum MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR = 0x47, MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, MIP_DATA_DESC_FILTER_GNSS_DUAL_ANTENNA_STATUS = 0x49, + MIP_DATA_DESC_FILTER_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x50, }; @@ -1369,6 +1370,30 @@ void extract_mip_filter_gnss_dual_antenna_status_data_fix_type(struct mip_serial void insert_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags self); void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags* self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_vehicle_frame_vel_with_unc_and_timestamp (0x82,0x50) Vehicle Frame Vel With Unc And Timestamp [C] +/// Estimate of velocity in the vehicle frame at specified time, with associated uncertainties. +/// +///@{ + +struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data +{ + uint8_t source_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + double gps_time; ///< [seconds] + uint16_t gps_week; ///< [GPS week number, not modulus 1024] + float velocity[3]; ///< [meters/second] + float velocity_uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + uint8_t reserved[4]; + uint16_t valid_flags; ///< 0 - invalid, 1 - valid + +}; +typedef struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data; +void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(struct mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self); +void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(struct mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self); +bool extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 78bf3fd2b..2782c16a7 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -91,6 +91,7 @@ enum DATA_ODOMETER_SCALE_FACTOR_ERROR = 0x47, DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, DATA_GNSS_DUAL_ANTENNA_STATUS = 0x49, + DATA_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x50, }; @@ -1968,6 +1969,38 @@ struct GnssDualAntennaStatus void insert(Serializer& serializer, const GnssDualAntennaStatus& self); void extract(Serializer& serializer, GnssDualAntennaStatus& self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_vehicle_frame_vel_with_unc_and_timestamp (0x82,0x50) Vehicle Frame Vel With Unc And Timestamp [CPP] +/// Estimate of velocity in the vehicle frame at specified time, with associated uncertainties. +/// +///@{ + +struct VehicleFrameVelWithUncAndTimestamp +{ + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP; + + static const bool HAS_FUNCTION_SELECTOR = false; + + auto as_tuple() const + { + return std::make_tuple(source_id,gps_time,gps_week,velocity[0],velocity[1],velocity[2],velocity_uncertainty[0],velocity_uncertainty[1],velocity_uncertainty[2],reserved,valid_flags); + } + + uint8_t source_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + double gps_time = 0; ///< [seconds] + uint16_t gps_week = 0; ///< [GPS week number, not modulus 1024] + float velocity[3] = {0}; ///< [meters/second] + float velocity_uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + uint8_t reserved[4] = {0}; + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + +}; +void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self); +void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self); + ///@} /// From afcaebb2db9777a4ebb4bbdeb140bb3a04a139e7 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 17:16:52 -0400 Subject: [PATCH 045/252] Working TCP implementation on Windows. --- CMakeLists.txt | 10 ++++++- src/mip/utils/tcp_socket.c | 57 +++++++++++++++++++------------------- 2 files changed, 38 insertions(+), 29 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 45af8210e..772df2bf7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -236,8 +236,9 @@ if(${MIP_ENABLE_DIAGNOSTICS}) endif() # Disable windows defined min/max +# Set Windows header version (0x0501 is _WIN32_WINNT_WINXP, required for TCP) if(WIN32) - list(APPEND MIP_DEFINES "NOMINMAX=1") + list(APPEND MIP_DEFINES "NOMINMAX=1" "_WIN32_WINNT=0x0501") endif() if(MSVC) @@ -248,6 +249,13 @@ endif() target_compile_definitions(mip PUBLIC "${MIP_DEFINES}") +# +# Libraries +# +if(WIN32 AND MIP_USE_TCP) + target_link_libraries(mip PUBLIC "ws2_32") +endif() + # # TESTING # diff --git a/src/mip/utils/tcp_socket.c b/src/mip/utils/tcp_socket.c index c2b9e268c..9bff38539 100644 --- a/src/mip/utils/tcp_socket.c +++ b/src/mip/utils/tcp_socket.c @@ -1,16 +1,15 @@ #include "tcp_socket.h" -#ifdef WIN32 +#include "mip/mip_logging.h" -#ifndef WIN32_LEAN_AND_MEAN -#define WIN32_LEAN_AND_MEAN -#endif +#ifdef _WIN32 #include #include -#include "../mip_loggging.h" +static const int SEND_FLAGS = 0; +typedef int ssize_t; #else @@ -20,12 +19,14 @@ #include #include #include -#include -#define INVALID_SOCKET -1 +static const int INVALID_SOCKET -1 +static const int SEND_FLAGS = MSG_NOSIGNAL; #endif +#include + static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, uint16_t port, unsigned int timeout_ms) { socket_ptr->handle = INVALID_SOCKET; @@ -54,7 +55,11 @@ static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, if( connect(socket_ptr->handle, addr->ai_addr, addr->ai_addrlen) == 0 ) break; +#ifdef WIN32 + closesocket(socket_ptr->handle); +#else close(socket_ptr->handle); +#endif socket_ptr->handle = INVALID_SOCKET; } @@ -63,15 +68,15 @@ static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, if( socket_ptr->handle == INVALID_SOCKET ) return false; - struct timeval timeout_option; - timeout_option.tv_sec = timeout_ms / 1000; - timeout_option.tv_usec = (timeout_ms % 1000) * 1000; - - if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_RCVTIMEO, &timeout_option, sizeof(timeout_option)) != 0 ) - return false; - - if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_SNDTIMEO, &timeout_option, sizeof(timeout_option)) != 0 ) - return false; +// struct timeval timeout_option; +// timeout_option.tv_sec = timeout_ms / 1000; +// timeout_option.tv_usec = (timeout_ms % 1000) * 1000; +// +// if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_RCVTIMEO, (void*)&timeout_option, sizeof(timeout_option)) != 0 ) +// return false; +// +// if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_SNDTIMEO, (void*)&timeout_option, sizeof(timeout_option)) != 0 ) +// return false; return true; } @@ -82,7 +87,7 @@ bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port // Initialize winsock for each connection since there's no global init function. // This is safe to do multiple times, as long as it's shutdown the same number of times. - WSAData wsaData; + struct WSAData wsaData; int result = WSAStartup(MAKEWORD(2,2), &wsaData); if(result != 0) { @@ -155,34 +160,31 @@ bool tcp_socket_close(tcp_socket* socket_ptr) bool tcp_socket_send(tcp_socket* socket_ptr, const void* buffer, size_t num_bytes, size_t* bytes_written) { -#ifdef WIN32 - return false; // TODO: Windows -#else for(*bytes_written = 0; *bytes_written < num_bytes; ) { - ssize_t sent = send(socket_ptr->handle, buffer, num_bytes, MSG_NOSIGNAL); + ssize_t sent = send(socket_ptr->handle, buffer, num_bytes, SEND_FLAGS); if(sent < 0) return false; *bytes_written += sent; } return true; -#endif } bool tcp_socket_recv(tcp_socket* socket_ptr, void* buffer, size_t num_bytes, size_t* bytes_read) { -#ifdef WIN32 - return false; // TODO: Windows -#else - ssize_t local_bytes_read = recv(socket_ptr->handle, buffer, num_bytes, MSG_NOSIGNAL); + ssize_t local_bytes_read = recv(socket_ptr->handle, buffer, num_bytes, SEND_FLAGS); - if( local_bytes_read == -1 ) + if( local_bytes_read < 0 ) { +#ifdef WIN32 + return false; +#else if(errno != EAGAIN && errno != EWOULDBLOCK) return false; else return true; +#endif } // Throw an error if the connection has been closed by the other side. else if( local_bytes_read == 0 ) @@ -190,5 +192,4 @@ bool tcp_socket_recv(tcp_socket* socket_ptr, void* buffer, size_t num_bytes, siz *bytes_read = local_bytes_read; return true; -#endif } From d7c5b290fb99d95ce5efb1b751e6d96a3ad4c1b9 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 17:21:16 -0400 Subject: [PATCH 046/252] Fix compilation of tcp_socket.c on Linux. --- src/mip/utils/tcp_socket.c | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/mip/utils/tcp_socket.c b/src/mip/utils/tcp_socket.c index 9bff38539..93bbce383 100644 --- a/src/mip/utils/tcp_socket.c +++ b/src/mip/utils/tcp_socket.c @@ -20,7 +20,7 @@ typedef int ssize_t; #include #include -static const int INVALID_SOCKET -1 +static const int INVALID_SOCKET = -1; static const int SEND_FLAGS = MSG_NOSIGNAL; #endif @@ -68,15 +68,18 @@ static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, if( socket_ptr->handle == INVALID_SOCKET ) return false; -// struct timeval timeout_option; -// timeout_option.tv_sec = timeout_ms / 1000; -// timeout_option.tv_usec = (timeout_ms % 1000) * 1000; -// -// if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_RCVTIMEO, (void*)&timeout_option, sizeof(timeout_option)) != 0 ) -// return false; -// -// if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_SNDTIMEO, (void*)&timeout_option, sizeof(timeout_option)) != 0 ) -// return false; +#ifndef WIN32 + // Todo: timeouts on windows + struct timeval timeout_option; + timeout_option.tv_sec = timeout_ms / 1000; + timeout_option.tv_usec = (timeout_ms % 1000) * 1000; + + if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_RCVTIMEO, &timeout_option, sizeof(timeout_option)) != 0 ) + return false; + + if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_SNDTIMEO, &timeout_option, sizeof(timeout_option)) != 0 ) + return false; +#endif return true; } From a1300d55c9499ecdb881064c19a09621e041a899 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 17:45:23 -0400 Subject: [PATCH 047/252] Remove unused commented code. --- src/mip/utils/tcp_socket.c | 42 -------------------------------------- 1 file changed, 42 deletions(-) diff --git a/src/mip/utils/tcp_socket.c b/src/mip/utils/tcp_socket.c index 93bbce383..fe8d77620 100644 --- a/src/mip/utils/tcp_socket.c +++ b/src/mip/utils/tcp_socket.c @@ -98,48 +98,6 @@ bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port return false; } -// struct addrinfo* address = NULL; -// struct addrinfo* ptr = NULL; -// struct addrinfo hints; -// -// ZeroMemory(&hints, sizeof(hints)); -// hints.ai_family = AF_UNSPEC; -// hints.ai_socktype = SOCK_STREAM; -// hints.ai_protocol = IPPROTO_TCP; -// -// result = getaddrinfo(hostname, port, &hints, &address); -// if(result != 0) -// { -// MIP_LOG_WARNING("getaddrinfo() failed for hostname=%s, port=%d: %d\n", hostname, port, result); -// WSACleanup(); -// return false; -// } -// -// *socket_ptr = socket(address->ai_family, address->ai_socktype, address->ai_protocol); -// if(*socket_ptr == INVALID_SOCKET) -// { -// MIP_LOG_WARNING("socket() failed for hostname=%s, port=%d: %d\n", hostname, port, WSAGetLastError()); -// freeaddrinfo(address); -// WSACleanup(); -// return false; -// } -// -// -// -// result = connect(*socket_ptr, address->ai_addr, (int)address->ai_addrlen); -// -// freeaddrinfo(address); -// -// if(result == SOCKET_ERROR) -// { -// closesocket(*socket_ptr); -// *socket_ptr = INVALID_SOCKET; -// WSACleanup(); -// return false; -// } -// -// return true; - #endif return tcp_socket_open_common(socket_ptr, hostname, port ,timeout_ms); From 39ae8e4b1e73169c77985053ff53d4ccb90aff66 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 11 Apr 2023 13:05:30 -0400 Subject: [PATCH 048/252] Add payload length check to mip_packet_is_sane. --- src/mip/mip_packet.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/mip/mip_packet.c b/src/mip/mip_packet.c index 0f8bbc974..d7851e3a9 100644 --- a/src/mip/mip_packet.c +++ b/src/mip/mip_packet.c @@ -41,7 +41,7 @@ void mip_packet_from_buffer(mip_packet* packet, uint8_t* buffer, size_t length) if( length > MIP_PACKET_LENGTH_MAX ) length = MIP_PACKET_LENGTH_MAX; - packet->_buffer = buffer; + packet->_buffer = buffer; packet->_buffer_length = length; } @@ -69,10 +69,10 @@ void mip_packet_create(mip_packet* packet, uint8_t* buffer, size_t buffer_size, return; } - packet->_buffer[MIP_INDEX_SYNC1] = MIP_SYNC1; - packet->_buffer[MIP_INDEX_SYNC2] = MIP_SYNC2; + packet->_buffer[MIP_INDEX_SYNC1] = MIP_SYNC1; + packet->_buffer[MIP_INDEX_SYNC2] = MIP_SYNC2; packet->_buffer[MIP_INDEX_DESCSET] = descriptor_set; - packet->_buffer[MIP_INDEX_LENGTH] = 0; + packet->_buffer[MIP_INDEX_LENGTH] = 0; } @@ -178,7 +178,7 @@ uint16_t mip_packet_compute_checksum(const mip_packet* packet) /// bool mip_packet_is_sane(const mip_packet* packet) { - return packet->_buffer && (mip_packet_buffer_size(packet) >= MIP_PACKET_LENGTH_MIN); + return packet->_buffer && (packet->_buffer_length >= MIP_PACKET_LENGTH_MIN) && (packet->_buffer_length >= mip_packet_payload_length(packet)+MIP_PACKET_LENGTH_MIN); } //////////////////////////////////////////////////////////////////////////////// From 9b97dc4e4ce4439f4ec52829718cf1800bbc37e5 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 11 Apr 2023 13:06:46 -0400 Subject: [PATCH 049/252] Add operator[] to mip::Packet and mip::Field. --- src/mip/mip.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 50052c37e..07c1dbd62 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -74,6 +74,8 @@ class Field : public C::mip_field ///@returns payload byte uint8_t payload(unsigned int index) const { return payload()[index]; } + uint8_t operator[](unsigned int index) const { return payload(index); } + ///@copydoc mip_field_is_valid bool isValid() const { return C::mip_field_is_valid(this); } @@ -181,6 +183,7 @@ class Packet : public C::mip_packet void reset(uint8_t descSet) { C::mip_packet_reset(this, descSet); } ///<@copydoc mip::C::mip_packet_reset void reset() { reset(descriptorSet()); } ///<@brief Resets the packet using the same descriptor set. + uint8_t operator[](unsigned int index) const { return pointer()[index]; } // // Additional functions which have no C equivalent From dfa37f64971782f227379911b4bee9a8ec2bf14c Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 10 Apr 2023 12:23:23 -0400 Subject: [PATCH 050/252] Fix compiler warnings. --- test/mip/test_mip.cpp | 2 +- test/mip/test_mip_fields.c | 2 +- test/mip/test_mip_random.c | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/test/mip/test_mip.cpp b/test/mip/test_mip.cpp index f00ce3287..190ae84b6 100644 --- a/test/mip/test_mip.cpp +++ b/test/mip/test_mip.cpp @@ -9,7 +9,7 @@ using namespace mip; - +using namespace mip::C; uint8_t packetBuffer[PACKET_LENGTH_MAX]; uint8_t parseBuffer[1024]; diff --git a/test/mip/test_mip_fields.c b/test/mip/test_mip_fields.c index e33dcac32..156c7a4be 100644 --- a/test/mip/test_mip_fields.c +++ b/test/mip/test_mip_fields.c @@ -61,7 +61,7 @@ int main(int argc, const char* argv[]) // Now iterate the fields and verify they match. unsigned int scanned_fields = 0; - for(struct mip_field field = mip_field_from_packet(&packet); mip_field_is_valid(&field); mip_field_next(&field)) + for(struct mip_field field = mip_field_first_from_packet(&packet); mip_field_is_valid(&field); mip_field_next(&field)) { const uint8_t test_field_desc = mip_field_field_descriptor(&field); const uint8_t test_desc_set = mip_field_descriptor_set(&field); diff --git a/test/mip/test_mip_random.c b/test/mip/test_mip_random.c index 6449a40f3..492659b71 100644 --- a/test/mip/test_mip_random.c +++ b/test/mip/test_mip_random.c @@ -166,13 +166,13 @@ int main(int argc, const char* argv[]) { num_errors++; error = true; - fprintf(stderr, "Parsed packet has wrong timestamp %d\n", parsed_packet_timestamp); + fprintf(stderr, "Parsed packet has wrong timestamp %ld\n", parsed_packet_timestamp); } last_parsed = num_packets_parsed; if( error ) { - fprintf(stderr, " packet_size=%ld, last_count=%ld, extra=%ld, start_time=%d\n", packet_size, count, extra, start_time); + fprintf(stderr, " packet_size=%ld, last_count=%ld, extra=%ld, start_time=%ld\n", packet_size, count, extra, start_time); fprintf(stderr, " Sent chunks:"); for(unsigned int d=0; d Date: Mon, 10 Apr 2023 13:04:23 -0400 Subject: [PATCH 051/252] Fix warnings in mip_logging.h when no additional parameters are used. --- src/mip/mip_logging.h | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/mip/mip_logging.h b/src/mip/mip_logging.h index 5182d4810..b847b729a 100644 --- a/src/mip/mip_logging.h +++ b/src/mip/mip_logging.h @@ -50,7 +50,7 @@ void* mip_logging_user_data(); void mip_logging_log(mip_log_level level, const char* fmt, ...); //////////////////////////////////////////////////////////////////////////////// -///@brief Helper macro used to initialize the MIP logger. +///@brief Helper macro used to initialize the MIP logger. /// This function does not need to be called unless the user wants logging /// ///@param callback The callback to execute when there is data to log @@ -69,9 +69,9 @@ void mip_logging_log(mip_log_level level, const char* fmt, ...); ///@copydetails mip::C::mip_log_callback /// #ifdef MIP_ENABLE_LOGGING -#define MIP_LOG_LOG(level, fmt, ...) mip_logging_log(level, fmt, __VA_ARGS__) +#define MIP_LOG_LOG(level, ...) mip_logging_log(level, __VA_ARGS__) #else -#define MIP_LOG_LOG(level, fmt, ...) (void)0 +#define MIP_LOG_LOG(level, ...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// @@ -82,54 +82,54 @@ void mip_logging_log(mip_log_level level, const char* fmt, ...); ///@param ... Variadic args used to populate the fmt string /// #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_FATAL -#define MIP_LOG_FATAL(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_FATAL, fmt, __VA_ARGS__) +#define MIP_LOG_FATAL(...) MIP_LOG_LOG(MIP_LOG_LEVEL_FATAL, __VA_ARGS__) #else -#define MIP_LOG_FATAL(fmt, ...) (void)0 +#define MIP_LOG_FATAL(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at error level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_ERROR -#define MIP_LOG_ERROR(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_ERROR, fmt, __VA_ARGS__) +#define MIP_LOG_ERROR(...) MIP_LOG_LOG(MIP_LOG_LEVEL_ERROR, __VA_ARGS__) #else -#define MIP_LOG_ERROR(fmt, ...) (void)0 +#define MIP_LOG_ERROR(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at warn level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_WARN -#define MIP_LOG_WARN(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_WARN, fmt, __VA_ARGS__) +#define MIP_LOG_WARN(...) MIP_LOG_LOG(MIP_LOG_LEVEL_WARN, __VA_ARGS__) #else -#define MIP_LOG_WARN(fmt, ...) (void)0 +#define MIP_LOG_WARN(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at info level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_INFO -#define MIP_LOG_INFO(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_INFO, fmt, __VA_ARGS__) +#define MIP_LOG_INFO(...) MIP_LOG_LOG(MIP_LOG_LEVEL_INFO, __VA_ARGS__) #else -#define MIP_LOG_INFO(fmt, ...) (void)0 +#define MIP_LOG_INFO(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at debug level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_DEBUG -#define MIP_LOG_DEBUG(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_DEBUG, fmt, __VA_ARGS__) +#define MIP_LOG_DEBUG(...) MIP_LOG_LOG(MIP_LOG_LEVEL_DEBUG, __VA_ARGS__) #else -#define MIP_LOG_DEBUG(fmt, ...) (void)0 +#define MIP_LOG_DEBUG(...) (void)0 #endif //////////////////////////////////////////////////////////////////////////////// ///@brief Helper macro used to log data inside the MIP SDK at trace level ///@copydetails mip::C::MIP_LOG_FATAL #if !defined(MIP_LOGGING_MAX_LEVEL) || MIP_LOGGING_MAX_LEVEL >= MIP_LOG_LEVEL_TRACE -#define MIP_LOG_TRACE(fmt, ...) MIP_LOG_LOG(MIP_LOG_LEVEL_TRACE, fmt, __VA_ARGS__) +#define MIP_LOG_TRACE(...) MIP_LOG_LOG(MIP_LOG_LEVEL_TRACE, __VA_ARGS__) #else -#define MIP_LOG_TRACE(fmt, ...) (void)0 +#define MIP_LOG_TRACE(...) (void)0 #endif ///@} From 261fa2131c6f50bd0057b829bb55a51253e627a9 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 2 May 2023 12:10:08 -0400 Subject: [PATCH 052/252] Generate MIP definitions from branches/cv7_ins@54888. --- src/mip/definitions/commands_filter.h | 5 +++-- src/mip/definitions/commands_filter.hpp | 7 +++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 65e774d58..c99510354 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -1486,8 +1486,9 @@ typedef uint8_t mip_filter_initialization_configuration_command_alignment_select static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_NONE = 0x00; static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_DUAL_ANTENNA = 0x01; ///< Dual-antenna GNSS alignment static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_KINEMATIC = 0x02; ///< GNSS kinematic alignment (GNSS velocity determines initial heading) -static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_MAGNETOMETER = 0x04; ///< Magnetometer heading alignment -static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_ALL = 0x07; +static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_MAGNETOMETER = 0x04; ///< Magnetometer heading alignment (Internal magnetometer determines initial heading) +static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_EXTERNAL = 0x08; ///< External heading alignment (External heading input determines heading) +static const mip_filter_initialization_configuration_command_alignment_selector MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_ALIGNMENT_SELECTOR_ALL = 0x0F; typedef uint8_t mip_filter_initialization_configuration_command_initial_condition_source; static const mip_filter_initialization_configuration_command_initial_condition_source MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_INITIAL_CONDITION_SOURCE_AUTO_POS_VEL_ATT = 0; ///< Automatic position, velocity and attitude diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 0abc5773a..e49a50209 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -1846,8 +1846,9 @@ struct InitializationConfiguration NONE = 0x00, DUAL_ANTENNA = 0x01, ///< Dual-antenna GNSS alignment KINEMATIC = 0x02, ///< GNSS kinematic alignment (GNSS velocity determines initial heading) - MAGNETOMETER = 0x04, ///< Magnetometer heading alignment - ALL = 0x07, + MAGNETOMETER = 0x04, ///< Magnetometer heading alignment (Internal magnetometer determines initial heading) + EXTERNAL = 0x08, ///< External heading alignment (External heading input determines heading) + ALL = 0x0F, }; uint8_t value = NONE; @@ -1865,6 +1866,8 @@ struct InitializationConfiguration void kinematic(bool val) { if(val) value |= KINEMATIC; else value &= ~KINEMATIC; } bool magnetometer() const { return (value & MAGNETOMETER) > 0; } void magnetometer(bool val) { if(val) value |= MAGNETOMETER; else value &= ~MAGNETOMETER; } + bool external() const { return (value & EXTERNAL) > 0; } + void external(bool val) { if(val) value |= EXTERNAL; else value &= ~EXTERNAL; } bool allSet() const { return value == ALL; } void setAll() { value |= ALL; } From 70ca4dbe6831c19ec9f6d631ee4512642a3784bd Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 3 May 2023 12:07:32 -0400 Subject: [PATCH 053/252] Generate MIP definitions from branches/cv7_ins@54891. --- src/mip/definitions/commands_3dm.c | 14 +++++++------- src/mip/definitions/commands_3dm.cpp | 14 +++++++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index 2e39a0fd1..bb4551d74 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -2419,7 +2419,7 @@ void extract_mip_3dm_get_event_support_response(mip_serializer* serializer, mip_ extract_u8(serializer, &self->max_instances); assert(self->num_entries); - extract_count(serializer, &self->num_entries, self->num_entries); + extract_count(serializer, &self->num_entries, sizeof(self->entries)/sizeof(self->entries[0])); for(unsigned int i=0; i < self->num_entries; i++) extract_mip_3dm_get_event_support_command_info(serializer, &self->entries[i]); @@ -2638,7 +2638,7 @@ void insert_mip_3dm_get_event_trigger_status_command(mip_serializer* serializer, void extract_mip_3dm_get_event_trigger_status_command(mip_serializer* serializer, mip_3dm_get_event_trigger_status_command* self) { assert(self->requested_count); - extract_count(serializer, &self->requested_count, self->requested_count); + extract_count(serializer, &self->requested_count, sizeof(self->requested_instances)/sizeof(self->requested_instances[0])); for(unsigned int i=0; i < self->requested_count; i++) extract_u8(serializer, &self->requested_instances[i]); @@ -2657,7 +2657,7 @@ void insert_mip_3dm_get_event_trigger_status_response(mip_serializer* serializer void extract_mip_3dm_get_event_trigger_status_response(mip_serializer* serializer, mip_3dm_get_event_trigger_status_response* self) { assert(self->count); - extract_count(serializer, &self->count, self->count); + extract_count(serializer, &self->count, sizeof(self->triggers)/sizeof(self->triggers[0])); for(unsigned int i=0; i < self->count; i++) extract_mip_3dm_get_event_trigger_status_command_entry(serializer, &self->triggers[i]); @@ -2736,7 +2736,7 @@ void insert_mip_3dm_get_event_action_status_command(mip_serializer* serializer, void extract_mip_3dm_get_event_action_status_command(mip_serializer* serializer, mip_3dm_get_event_action_status_command* self) { assert(self->requested_count); - extract_count(serializer, &self->requested_count, self->requested_count); + extract_count(serializer, &self->requested_count, sizeof(self->requested_instances)/sizeof(self->requested_instances[0])); for(unsigned int i=0; i < self->requested_count; i++) extract_u8(serializer, &self->requested_instances[i]); @@ -2755,7 +2755,7 @@ void insert_mip_3dm_get_event_action_status_response(mip_serializer* serializer, void extract_mip_3dm_get_event_action_status_response(mip_serializer* serializer, mip_3dm_get_event_action_status_response* self) { assert(self->count); - extract_count(serializer, &self->count, self->count); + extract_count(serializer, &self->count, sizeof(self->actions)/sizeof(self->actions[0])); for(unsigned int i=0; i < self->count; i++) extract_mip_3dm_get_event_action_status_command_entry(serializer, &self->actions[i]); @@ -3291,7 +3291,7 @@ void extract_mip_3dm_event_action_command_message_params(mip_serializer* seriali extract_u16(serializer, &self->decimation); assert(self->num_fields); - extract_count(serializer, &self->num_fields, self->num_fields); + extract_count(serializer, &self->num_fields, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_fields; i++) extract_u8(serializer, &self->descriptors[i]); @@ -4707,7 +4707,7 @@ void extract_mip_3dm_calibrated_sensor_ranges_response(mip_serializer* serialize extract_mip_sensor_range_type(serializer, &self->sensor); assert(self->num_ranges); - extract_count(serializer, &self->num_ranges, self->num_ranges); + extract_count(serializer, &self->num_ranges, sizeof(self->ranges)/sizeof(self->ranges[0])); for(unsigned int i=0; i < self->num_ranges; i++) extract_mip_3dm_calibrated_sensor_ranges_command_entry(serializer, &self->ranges[i]); diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index c727face8..5af25a297 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -2153,7 +2153,7 @@ void extract(Serializer& serializer, GetEventSupport::Response& self) extract(serializer, self.max_instances); - C::extract_count(&serializer, &self.num_entries, self.num_entries); + C::extract_count(&serializer, &self.num_entries, sizeof(self.entries)/sizeof(self.entries[0])); for(unsigned int i=0; i < self.num_entries; i++) extract(serializer, self.entries[i]); @@ -2332,7 +2332,7 @@ void insert(Serializer& serializer, const GetEventTriggerStatus& self) } void extract(Serializer& serializer, GetEventTriggerStatus& self) { - C::extract_count(&serializer, &self.requested_count, self.requested_count); + C::extract_count(&serializer, &self.requested_count, sizeof(self.requested_instances)/sizeof(self.requested_instances[0])); for(unsigned int i=0; i < self.requested_count; i++) extract(serializer, self.requested_instances[i]); @@ -2348,7 +2348,7 @@ void insert(Serializer& serializer, const GetEventTriggerStatus::Response& self) } void extract(Serializer& serializer, GetEventTriggerStatus::Response& self) { - C::extract_count(&serializer, &self.count, self.count); + C::extract_count(&serializer, &self.count, sizeof(self.triggers)/sizeof(self.triggers[0])); for(unsigned int i=0; i < self.count; i++) extract(serializer, self.triggers[i]); @@ -2409,7 +2409,7 @@ void insert(Serializer& serializer, const GetEventActionStatus& self) } void extract(Serializer& serializer, GetEventActionStatus& self) { - C::extract_count(&serializer, &self.requested_count, self.requested_count); + C::extract_count(&serializer, &self.requested_count, sizeof(self.requested_instances)/sizeof(self.requested_instances[0])); for(unsigned int i=0; i < self.requested_count; i++) extract(serializer, self.requested_instances[i]); @@ -2425,7 +2425,7 @@ void insert(Serializer& serializer, const GetEventActionStatus::Response& self) } void extract(Serializer& serializer, GetEventActionStatus::Response& self) { - C::extract_count(&serializer, &self.count, self.count); + C::extract_count(&serializer, &self.count, sizeof(self.actions)/sizeof(self.actions[0])); for(unsigned int i=0; i < self.count; i++) extract(serializer, self.actions[i]); @@ -2899,7 +2899,7 @@ void extract(Serializer& serializer, EventAction::MessageParams& self) extract(serializer, self.decimation); - C::extract_count(&serializer, &self.num_fields, self.num_fields); + C::extract_count(&serializer, &self.num_fields, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_fields; i++) extract(serializer, self.descriptors[i]); @@ -4178,7 +4178,7 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Response& self) { extract(serializer, self.sensor); - C::extract_count(&serializer, &self.num_ranges, self.num_ranges); + C::extract_count(&serializer, &self.num_ranges, sizeof(self.ranges)/sizeof(self.ranges[0])); for(unsigned int i=0; i < self.num_ranges; i++) extract(serializer, self.ranges[i]); From dd7ac1ce43be15a0cc8ef7a50d6eaa292c9595b8 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Wed, 31 May 2023 12:02:53 -0400 Subject: [PATCH 054/252] Added connection type identifiers and connection info access functions --- src/mip/extras/recording_connection.cpp | 1 + src/mip/extras/recording_connection.hpp | 4 +++- src/mip/mip_device.hpp | 12 ++++++++++++ src/mip/platform/serial_connection.cpp | 4 ++++ src/mip/platform/serial_connection.hpp | 12 ++++++++++++ src/mip/platform/tcp_connection.cpp | 4 ++++ src/mip/platform/tcp_connection.hpp | 10 ++++++++++ 7 files changed, 46 insertions(+), 1 deletion(-) diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp index 725bdfbf6..2c0976b56 100644 --- a/src/mip/extras/recording_connection.cpp +++ b/src/mip/extras/recording_connection.cpp @@ -13,6 +13,7 @@ namespace extras RecordingConnection::RecordingConnection(Connection* connection, std::ostream* recvStream, std::ostream* sendStream) : mConnection(connection), mRecvFile(recvStream), mSendFile(sendStream) { + mType = TYPE; } ///@copydoc mip::Connection::sendToDevice diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index 7446d8324..221a818a8 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -21,7 +21,9 @@ namespace extras class RecordingConnection : public Connection { public: - RecordingConnection(Connection* connection, std::ostream* recvStream=nullptr, std::ostream* sendStream=nullptr); + static constexpr auto TYPE = "Recording"; + + RecordingConnection(Connection *connection, std::ostream *recvStream = nullptr, std::ostream *sendStream = nullptr); bool sendToDevice(const uint8_t* data, size_t length) final; bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) final; diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index e8e21ca91..7628d7cb8 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -5,6 +5,7 @@ #include "definitions/descriptors.h" +#include namespace mip { @@ -143,12 +144,23 @@ template bool startCommand(C::mip_interface& device, C::mip_pending_c class Connection { public: + + static constexpr auto TYPE = "None"; + + Connection() { mType = TYPE; }; virtual ~Connection() {} virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) = 0; void connect_interface(C::mip_interface* device); + + std::string type() { return mType; }; + +protected: + + std::string mType; + }; diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index 3bc10794e..8f6e0e0b3 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -17,6 +17,10 @@ SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrat { if (!serial_port_open(&mPort, portName.c_str(), baudrate)) throw std::runtime_error("Unable to open serial port"); + + mPortName = portName; + mBaudrate = baudrate; + mType = TYPE; } ///@brief Closes the underlying serial port diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index 85adfc925..656ace8fb 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -20,6 +20,9 @@ namespace platform class SerialConnection : public mip::Connection { public: + + static constexpr auto TYPE = "Serial"; + SerialConnection() = default; SerialConnection(const std::string& portName, uint32_t baudrate); ~SerialConnection(); @@ -27,10 +30,19 @@ class SerialConnection : public mip::Connection bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; + void connectionInfo(std::string &name, uint32_t &baudrate) + { + name = mPortName; + baudrate = mBaudrate; + }; + private: serial_port mPort; + std::string mPortName; + uint32_t mBaudrate; }; + ///@} //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/platform/tcp_connection.cpp b/src/mip/platform/tcp_connection.cpp index 12780e4c2..ab02a0630 100644 --- a/src/mip/platform/tcp_connection.cpp +++ b/src/mip/platform/tcp_connection.cpp @@ -18,6 +18,10 @@ TcpConnection::TcpConnection(const std::string& hostname, uint16_t port) { if (!tcp_socket_open(&mSocket, hostname.c_str(), port, 3000)) throw std::runtime_error("Unable to open TCP socket"); + + mHostname = hostname; + mPort = port; + mType = TYPE; } ///@brief Closes the underlying TCP socket diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index 85823bb01..e65ff0d6c 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -21,6 +21,8 @@ namespace platform class TcpConnection : public mip::Connection { public: + static constexpr auto TYPE = "TCP"; + TcpConnection() = default; TcpConnection(const std::string& hostname, uint16_t port); ~TcpConnection(); @@ -28,8 +30,16 @@ class TcpConnection : public mip::Connection bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; + void connectionInfo(std::string &host_name, uint32_t &port) + { + host_name = mHostname; + port = mPort; + }; + private: tcp_socket mSocket; + std::string mHostname; + uint16_t mPort = 0; }; ///@} From 9e0d71fd5a2dcdb0ae227cf4496c65326974ee7d Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Wed, 31 May 2023 12:15:43 -0400 Subject: [PATCH 055/252] Changed to const char * from std::string to save on memory allocation --- src/mip/mip_device.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 7628d7cb8..121700e11 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -155,11 +155,11 @@ class Connection void connect_interface(C::mip_interface* device); - std::string type() { return mType; }; + const char* type() { return mType; }; protected: - std::string mType; + const char *mType; }; From 90f0e0523f6235e051c6450fdb97ad3bc66c220a Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Wed, 31 May 2023 16:40:15 -0400 Subject: [PATCH 056/252] Added connection controls: open, close, isOpen. Connection is created closed by default. --- src/mip/extras/recording_connection.cpp | 4 +-- src/mip/extras/recording_connection.hpp | 14 ++++++++++ src/mip/mip_device.hpp | 4 +++ src/mip/platform/serial_connection.cpp | 36 ++++++++++++++++++++++--- src/mip/platform/serial_connection.hpp | 7 +++++ src/mip/platform/tcp_connection.cpp | 35 +++++++++++++++++++++--- src/mip/platform/tcp_connection.hpp | 5 ++++ 7 files changed, 95 insertions(+), 10 deletions(-) diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp index 2c0976b56..e336230df 100644 --- a/src/mip/extras/recording_connection.cpp +++ b/src/mip/extras/recording_connection.cpp @@ -20,7 +20,7 @@ RecordingConnection::RecordingConnection(Connection* connection, std::ostream* r bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) { const bool ok = mConnection->sendToDevice(data, length); - if( ok && mSendFile != nullptr ) + if( ok && mSendFile != nullptr && mConnected) mSendFile->write(reinterpret_cast(data), length); return ok; } @@ -29,7 +29,7 @@ bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* count_out, Timestamp* timestamp_out) { const bool ok = mConnection->recvFromDevice(buffer, max_length, wait_time, count_out, timestamp_out); - if( ok && mRecvFile != nullptr ) + if( ok && mRecvFile != nullptr && mConnected) mRecvFile->write(reinterpret_cast(buffer), *count_out); return ok; } diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index 221a818a8..507b0d04b 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -28,12 +28,26 @@ class RecordingConnection : public Connection bool sendToDevice(const uint8_t* data, size_t length) final; bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) final; + bool isConnected() {return mConnected;}; + bool connect() + { + mConnected = true; + return true; + }; + bool disconnect() + { + mConnected = false; + return true; + }; + protected: Connection* mConnection; // Files may be NULL to not record one direction or the other std::ostream* mRecvFile; std::ostream* mSendFile; + + bool mConnected = false; }; //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 121700e11..a313db34e 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -153,6 +153,10 @@ class Connection virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) = 0; + virtual bool isConnected() = 0; + virtual bool connect() = 0; + virtual bool disconnect() = 0; + void connect_interface(C::mip_interface* device); const char* type() { return mType; }; diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index 8f6e0e0b3..1fc853b0f 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -15,9 +15,6 @@ namespace platform ///@param baudrate Baud rate to open the device at. Note that the device needs to be configured to SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrate) { - if (!serial_port_open(&mPort, portName.c_str(), baudrate)) - throw std::runtime_error("Unable to open serial port"); - mPortName = portName; mBaudrate = baudrate; mType = TYPE; @@ -26,9 +23,40 @@ SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrat ///@brief Closes the underlying serial port SerialConnection::~SerialConnection() { - serial_port_close(&mPort); + if(mConnected) + disconnect(); +} + +///@brief Check if the port is connected +bool SerialConnection::isConnected() +{ + return mConnected; +} + +///@brief Connect to the port +bool SerialConnection::connect() +{ + if(mConnected) + return false; + + mConnected = serial_port_open(&mPort, mPortName.c_str(), mBaudrate); + + return mConnected; } +///@brief Disconnect from the port +bool SerialConnection::disconnect() +{ + if(!mConnected) + return false; + + mConnected = serial_port_close(&mPort); + + return mConnected; +} + + + ///@copydoc mip::Connection::recvFromDevice bool SerialConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) { diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index 656ace8fb..8d2ded459 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -30,6 +30,12 @@ class SerialConnection : public mip::Connection bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; + bool isConnected(); + bool connect(); + bool disconnect(); + + + void connectionInfo(std::string &name, uint32_t &baudrate) { name = mPortName; @@ -40,6 +46,7 @@ class SerialConnection : public mip::Connection serial_port mPort; std::string mPortName; uint32_t mBaudrate; + bool mConnected = false; }; diff --git a/src/mip/platform/tcp_connection.cpp b/src/mip/platform/tcp_connection.cpp index ab02a0630..d1ec0b6d7 100644 --- a/src/mip/platform/tcp_connection.cpp +++ b/src/mip/platform/tcp_connection.cpp @@ -16,9 +16,6 @@ namespace platform ///@param port Port on hostName to connect to TcpConnection::TcpConnection(const std::string& hostname, uint16_t port) { - if (!tcp_socket_open(&mSocket, hostname.c_str(), port, 3000)) - throw std::runtime_error("Unable to open TCP socket"); - mHostname = hostname; mPort = port; mType = TYPE; @@ -27,7 +24,37 @@ TcpConnection::TcpConnection(const std::string& hostname, uint16_t port) ///@brief Closes the underlying TCP socket TcpConnection::~TcpConnection() { - tcp_socket_close(&mSocket); + if(mConnected) + disconnect(); +} + +///@brief Check if the socket is connected +bool TcpConnection::isConnected() +{ + return mConnected; +} + +///@brief Connect to the socket +bool TcpConnection::connect() +{ + if(mConnected) + return false; + + mConnected = tcp_socket_open(&mSocket, mHostname.c_str(), mPort, 3000); + + return mConnected; +} + + +///@brief Disconnect from the socket +bool TcpConnection::disconnect() +{ + if(!mConnected) + return false; + + mConnected = tcp_socket_close(&mSocket); + + return mConnected; } ///@copydoc mip::Connection::sendToDevice diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index e65ff0d6c..5828c267c 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -29,6 +29,10 @@ class TcpConnection : public mip::Connection bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; + + bool isConnected(); + bool connect(); + bool disconnect(); void connectionInfo(std::string &host_name, uint32_t &port) { @@ -40,6 +44,7 @@ class TcpConnection : public mip::Connection tcp_socket mSocket; std::string mHostname; uint16_t mPort = 0; + bool mConnected = false; }; ///@} From df2e0aa83d29ebee722742825b98ce5201b40242 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Wed, 31 May 2023 16:59:44 -0400 Subject: [PATCH 057/252] Fixed serial and recording connections to use inderlaying class isOpen functions --- src/mip/extras/recording_connection.cpp | 4 ++-- src/mip/extras/recording_connection.hpp | 21 ++++++++++++++------- src/mip/platform/serial_connection.cpp | 16 ++++++---------- src/mip/platform/serial_connection.hpp | 1 - 4 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp index e336230df..62ef0e2af 100644 --- a/src/mip/extras/recording_connection.cpp +++ b/src/mip/extras/recording_connection.cpp @@ -20,7 +20,7 @@ RecordingConnection::RecordingConnection(Connection* connection, std::ostream* r bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) { const bool ok = mConnection->sendToDevice(data, length); - if( ok && mSendFile != nullptr && mConnected) + if( ok && mSendFile != nullptr && mConnection->isConnected()) mSendFile->write(reinterpret_cast(data), length); return ok; } @@ -29,7 +29,7 @@ bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* count_out, Timestamp* timestamp_out) { const bool ok = mConnection->recvFromDevice(buffer, max_length, wait_time, count_out, timestamp_out); - if( ok && mRecvFile != nullptr && mConnected) + if (ok && mRecvFile != nullptr && mConnection->isConnected()) mRecvFile->write(reinterpret_cast(buffer), *count_out); return ok; } diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index 507b0d04b..7c7a8e2c7 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -28,16 +28,25 @@ class RecordingConnection : public Connection bool sendToDevice(const uint8_t* data, size_t length) final; bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) final; - bool isConnected() {return mConnected;}; + bool isConnected() + { + if(mConnection) + return mConnection->isConnected(); + + return false; + }; + bool connect() { - mConnected = true; - return true; + if (mConnection) return mConnection->connect(); + + return false; }; bool disconnect() { - mConnected = false; - return true; + if (mConnection) return mConnection->disconnect(); + + return false; }; protected: @@ -46,8 +55,6 @@ class RecordingConnection : public Connection // Files may be NULL to not record one direction or the other std::ostream* mRecvFile; std::ostream* mSendFile; - - bool mConnected = false; }; //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index 1fc853b0f..6d9a21678 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -23,36 +23,32 @@ SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrat ///@brief Closes the underlying serial port SerialConnection::~SerialConnection() { - if(mConnected) + if (serial_port_is_open(&mPort)) disconnect(); } ///@brief Check if the port is connected bool SerialConnection::isConnected() { - return mConnected; + return serial_port_is_open(&mPort); } ///@brief Connect to the port bool SerialConnection::connect() { - if(mConnected) + if (serial_port_is_open(&mPort)) return false; - mConnected = serial_port_open(&mPort, mPortName.c_str(), mBaudrate); - - return mConnected; + return serial_port_open(&mPort, mPortName.c_str(), mBaudrate); } ///@brief Disconnect from the port bool SerialConnection::disconnect() { - if(!mConnected) + if (!serial_port_is_open(&mPort)) return false; - mConnected = serial_port_close(&mPort); - - return mConnected; + return serial_port_close(&mPort); } diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index 8d2ded459..f0283a285 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -46,7 +46,6 @@ class SerialConnection : public mip::Connection serial_port mPort; std::string mPortName; uint32_t mBaudrate; - bool mConnected = false; }; From af8ba81f3483e09cd5e2e17b62971bf5e640a5ca Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Wed, 31 May 2023 17:10:26 -0400 Subject: [PATCH 058/252] Bug fixes --- src/mip/platform/serial_connection.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index 6d9a21678..a9c039d18 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -18,13 +18,18 @@ SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrat mPortName = portName; mBaudrate = baudrate; mType = TYPE; + + #ifdef WIN32 + mPort.handle = INVALID_HANDLE_VALUE; +#else + mPort.handle = 0; +#endif } ///@brief Closes the underlying serial port SerialConnection::~SerialConnection() { - if (serial_port_is_open(&mPort)) - disconnect(); + disconnect(); } ///@brief Check if the port is connected @@ -46,7 +51,7 @@ bool SerialConnection::connect() bool SerialConnection::disconnect() { if (!serial_port_is_open(&mPort)) - return false; + return true; return serial_port_close(&mPort); } From bbac177ae7cbfdb4c6128b4f6cacfa15cc27a639 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Wed, 31 May 2023 17:22:38 -0400 Subject: [PATCH 059/252] Fixed bug for when a device is disconnected and then reconnected --- src/mip/utils/serial_port.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 2f714cec4..3b85b9e8f 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -196,9 +196,11 @@ bool serial_port_close(serial_port *port) //Close the serial port CloseHandle(port->handle); + port->handle = INVALID_HANDLE_VALUE; #else //Linux close(port->handle); + port->handle = 0; #endif return true; From 16a2d23f2c1729c655381e0412404f1955d51568 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 8 Jun 2023 11:12:11 -0400 Subject: [PATCH 060/252] Generate MIP definitions from branches/cv7_ins@55009. --- src/mip/definitions/commands_aiding.c | 683 ++++++++++++++++++++++++ src/mip/definitions/commands_aiding.cpp | 604 +++++++++++++++++++++ src/mip/definitions/commands_aiding.h | 305 +++++++++++ src/mip/definitions/commands_aiding.hpp | 395 ++++++++++++++ src/mip/definitions/commands_filter.c | 65 --- src/mip/definitions/commands_filter.cpp | 64 --- src/mip/definitions/commands_filter.h | 25 - src/mip/definitions/commands_filter.hpp | 29 - src/mip/definitions/data_filter.c | 50 -- src/mip/definitions/data_filter.cpp | 41 -- src/mip/definitions/data_filter.h | 25 - src/mip/definitions/data_filter.hpp | 33 -- 12 files changed, 1987 insertions(+), 332 deletions(-) create mode 100644 src/mip/definitions/commands_aiding.c create mode 100644 src/mip/definitions/commands_aiding.cpp create mode 100644 src/mip/definitions/commands_aiding.h create mode 100644 src/mip/definitions/commands_aiding.hpp diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c new file mode 100644 index 000000000..051d52480 --- /dev/null +++ b/src/mip/definitions/commands_aiding.c @@ -0,0 +1,683 @@ + +#include "commands_aiding.h" + +#include "../utils/serialization.h" +#include "../mip_interface.h" + +#include + + +#ifdef __cplusplus +namespace mip { +namespace C { +extern "C" { + +#endif // __cplusplus +struct mip_interface; +struct mip_serializer; +struct mip_field; + + +//////////////////////////////////////////////////////////////////////////////// +// Shared Type Definitions +//////////////////////////////////////////////////////////////////////////////// + +void insert_mip_time(mip_serializer* serializer, const mip_time* self) +{ + insert_mip_time_timebase(serializer, self->timebase); + + insert_u8(serializer, self->reserved); + + insert_u64(serializer, self->nanoseconds); + +} +void extract_mip_time(mip_serializer* serializer, mip_time* self) +{ + extract_mip_time_timebase(serializer, &self->timebase); + + extract_u8(serializer, &self->reserved); + + extract_u64(serializer, &self->nanoseconds); + +} + +void insert_mip_time_timebase(struct mip_serializer* serializer, const mip_time_timebase self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timebase* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Mip Fields +//////////////////////////////////////////////////////////////////////////////// + +void insert_mip_aiding_ecef_pos_command(mip_serializer* serializer, const mip_aiding_ecef_pos_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_double(serializer, self->position[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_ecef_pos_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_ecef_pos_command(mip_serializer* serializer, mip_aiding_ecef_pos_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_double(serializer, &self->position[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_ecef_pos_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_ecef_pos_response(mip_serializer* serializer, const mip_aiding_ecef_pos_response* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_double(serializer, self->position[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_ecef_pos_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_ecef_pos_response(mip_serializer* serializer, mip_aiding_ecef_pos_response* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_double(serializer, &self->position[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_ecef_pos_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* position_out, float* uncertainty_out, mip_aiding_ecef_pos_command_valid_flags* valid_flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + assert(position || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_double(&serializer, position[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_ecef_pos_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECEF_POS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ECEF_POS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(time_out); + extract_mip_time(&deserializer, time_out); + + assert(sensor_id_out); + extract_u8(&deserializer, sensor_id_out); + + assert(position_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_double(&deserializer, &position_out[i]); + + assert(uncertainty_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &uncertainty_out[i]); + + assert(valid_flags_out); + extract_mip_aiding_ecef_pos_command_valid_flags(&deserializer, valid_flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert_mip_aiding_llh_pos_command(mip_serializer* serializer, const mip_aiding_llh_pos_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_double(serializer, self->latitude); + + insert_double(serializer, self->longitude); + + insert_double(serializer, self->height); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_llh_pos_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_llh_pos_command(mip_serializer* serializer, mip_aiding_llh_pos_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_double(serializer, &self->latitude); + + extract_double(serializer, &self->longitude); + + extract_double(serializer, &self->height); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_llh_pos_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_llh_pos_response(mip_serializer* serializer, const mip_aiding_llh_pos_response* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_double(serializer, self->latitude); + + insert_double(serializer, self->longitude); + + insert_double(serializer, self->height); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_llh_pos_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_llh_pos_response(mip_serializer* serializer, mip_aiding_llh_pos_response* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_double(serializer, &self->latitude); + + extract_double(serializer, &self->longitude); + + extract_double(serializer, &self->height); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_llh_pos_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* latitude_out, double* longitude_out, double* height_out, float* uncertainty_out, mip_aiding_llh_pos_command_valid_flags* valid_flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + insert_double(&serializer, latitude); + + insert_double(&serializer, longitude); + + insert_double(&serializer, height); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_llh_pos_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_LLH_POS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_LLH_POS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(time_out); + extract_mip_time(&deserializer, time_out); + + assert(sensor_id_out); + extract_u8(&deserializer, sensor_id_out); + + assert(latitude_out); + extract_double(&deserializer, latitude_out); + + assert(longitude_out); + extract_double(&deserializer, longitude_out); + + assert(height_out); + extract_double(&deserializer, height_out); + + assert(uncertainty_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &uncertainty_out[i]); + + assert(valid_flags_out); + extract_mip_aiding_llh_pos_command_valid_flags(&deserializer, valid_flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_vehicle_fixed_frame_velocity_response(mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_response* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_vehicle_fixed_frame_velocity_response(mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_response* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* valid_flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, velocity[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ODOM_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ODOM_VEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(time_out); + extract_mip_time(&deserializer, time_out); + + assert(sensor_id_out); + extract_u8(&deserializer, sensor_id_out); + + assert(velocity_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &velocity_out[i]); + + assert(uncertainty_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &uncertainty_out[i]); + + assert(valid_flags_out); + extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(&deserializer, valid_flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert_mip_aiding_true_heading_command(mip_serializer* serializer, const mip_aiding_true_heading_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_float(serializer, self->heading); + + insert_float(serializer, self->uncertainty); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_aiding_true_heading_command(mip_serializer* serializer, mip_aiding_true_heading_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_float(serializer, &self->heading); + + extract_float(serializer, &self->uncertainty); + + extract_u16(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_true_heading_response(mip_serializer* serializer, const mip_aiding_true_heading_response* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_float(serializer, self->heading); + + insert_float(serializer, self->uncertainty); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_aiding_true_heading_response(mip_serializer* serializer, mip_aiding_true_heading_response* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_float(serializer, &self->heading); + + extract_float(serializer, &self->uncertainty); + + extract_u16(serializer, &self->valid_flags); + +} + +mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* heading_out, float* uncertainty_out, uint16_t* valid_flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + insert_float(&serializer, heading); + + insert_float(&serializer, uncertainty); + + insert_u16(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_HEADING_TRUE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(time_out); + extract_mip_time(&deserializer, time_out); + + assert(sensor_id_out); + extract_u8(&deserializer, sensor_id_out); + + assert(heading_out); + extract_float(&deserializer, heading_out); + + assert(uncertainty_out); + extract_float(&deserializer, uncertainty_out); + + assert(valid_flags_out); + extract_u16(&deserializer, valid_flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); + + } +} +void extract_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); + + } +} + +void insert_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self) +{ + insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); + +} +void extract_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self) +{ + extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); + +} + +void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_aiding_aiding_echo_control_command_mode(&serializer, mode); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ECHO_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(mode_out); + extract_mip_aiding_aiding_echo_control_command_mode(&deserializer, mode_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} + +#ifdef __cplusplus +} // namespace C +} // namespace mip +} // extern "C" +#endif // __cplusplus + diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp new file mode 100644 index 000000000..5a94e5a04 --- /dev/null +++ b/src/mip/definitions/commands_aiding.cpp @@ -0,0 +1,604 @@ + +#include "commands_aiding.hpp" + +#include "../utils/serialization.h" +#include "../mip_interface.h" + +#include + + +namespace mip { +class Serializer; + +namespace C { +struct mip_interface; +} // namespace C + +namespace commands_aiding { + +using ::mip::insert; +using ::mip::extract; +using namespace ::mip::C; + +//////////////////////////////////////////////////////////////////////////////// +// Shared Type Definitions +//////////////////////////////////////////////////////////////////////////////// + +void insert(Serializer& serializer, const Time& self) +{ + insert(serializer, self.timebase); + + insert(serializer, self.reserved); + + insert(serializer, self.nanoseconds); + +} +void extract(Serializer& serializer, Time& self) +{ + extract(serializer, self.timebase); + + extract(serializer, self.reserved); + + extract(serializer, self.nanoseconds); + +} + + +//////////////////////////////////////////////////////////////////////////////// +// Mip Fields +//////////////////////////////////////////////////////////////////////////////// + +void insert(Serializer& serializer, const EcefPos& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.position[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, EcefPos& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.position[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +void insert(Serializer& serializer, const EcefPos::Response& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.position[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, EcefPos::Response& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.position[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* positionOut, float* uncertaintyOut, EcefPos::ValidFlags* validFlagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + assert(position || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, position[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECEF_POS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECEF_POS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(timeOut); + extract(deserializer, *timeOut); + + assert(sensorIdOut); + extract(deserializer, *sensorIdOut); + + assert(positionOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, positionOut[i]); + + assert(uncertaintyOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, uncertaintyOut[i]); + + assert(validFlagsOut); + extract(deserializer, *validFlagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert(Serializer& serializer, const LlhPos& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.latitude); + + insert(serializer, self.longitude); + + insert(serializer, self.height); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, LlhPos& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.latitude); + + extract(serializer, self.longitude); + + extract(serializer, self.height); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +void insert(Serializer& serializer, const LlhPos::Response& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.latitude); + + insert(serializer, self.longitude); + + insert(serializer, self.height); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, LlhPos::Response& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.latitude); + + extract(serializer, self.longitude); + + extract(serializer, self.height); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* latitudeOut, double* longitudeOut, double* heightOut, float* uncertaintyOut, LlhPos::ValidFlags* validFlagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + insert(serializer, latitude); + + insert(serializer, longitude); + + insert(serializer, height); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LLH_POS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LLH_POS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(timeOut); + extract(deserializer, *timeOut); + + assert(sensorIdOut); + extract(deserializer, *sensorIdOut); + + assert(latitudeOut); + extract(deserializer, *latitudeOut); + + assert(longitudeOut); + extract(deserializer, *longitudeOut); + + assert(heightOut); + extract(deserializer, *heightOut); + + assert(uncertaintyOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, uncertaintyOut[i]); + + assert(validFlagsOut); + extract(deserializer, *validFlagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +void insert(Serializer& serializer, const VehicleFixedFrameVelocity::Response& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, VehicleFixedFrameVelocity::Response& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, VehicleFixedFrameVelocity::ValidFlags* validFlagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, velocity[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOM_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOM_VEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(timeOut); + extract(deserializer, *timeOut); + + assert(sensorIdOut); + extract(deserializer, *sensorIdOut); + + assert(velocityOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, velocityOut[i]); + + assert(uncertaintyOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, uncertaintyOut[i]); + + assert(validFlagsOut); + extract(deserializer, *validFlagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert(Serializer& serializer, const TrueHeading& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.heading); + + insert(serializer, self.uncertainty); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, TrueHeading& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.heading); + + extract(serializer, self.uncertainty); + + extract(serializer, self.valid_flags); + +} + +void insert(Serializer& serializer, const TrueHeading::Response& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.heading); + + insert(serializer, self.uncertainty); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, TrueHeading::Response& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.heading); + + extract(serializer, self.uncertainty); + + extract(serializer, self.valid_flags); + +} + +CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags, Time* timeOut, uint8_t* sensorIdOut, float* headingOut, float* uncertaintyOut, uint16_t* validFlagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + insert(serializer, heading); + + insert(serializer, uncertainty); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_TRUE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(timeOut); + extract(deserializer, *timeOut); + + assert(sensorIdOut); + extract(deserializer, *sensorIdOut); + + assert(headingOut); + extract(deserializer, *headingOut); + + assert(uncertaintyOut); + extract(deserializer, *uncertaintyOut); + + assert(validFlagsOut); + extract(deserializer, *validFlagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert(Serializer& serializer, const AidingEchoControl& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.mode); + + } +} +void extract(Serializer& serializer, AidingEchoControl& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.mode); + + } +} + +void insert(Serializer& serializer, const AidingEchoControl::Response& self) +{ + insert(serializer, self.mode); + +} +void extract(Serializer& serializer, AidingEchoControl::Response& self) +{ + extract(serializer, self.mode); + +} + +CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, mode); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(modeOut); + extract(deserializer, *modeOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAidingEchoControl(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAidingEchoControl(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAidingEchoControl(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} + +} // namespace commands_aiding +} // namespace mip + diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h new file mode 100644 index 000000000..5973c182b --- /dev/null +++ b/src/mip/definitions/commands_aiding.h @@ -0,0 +1,305 @@ +#pragma once + +#include "descriptors.h" +#include "../mip_result.h" + +#include +#include +#include + +#ifdef __cplusplus +namespace mip { +namespace C { +extern "C" { + +#endif // __cplusplus +struct mip_interface; +struct mip_serializer; +struct mip_field; + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup MipCommands_c MIP Commands [C] +///@{ +///@defgroup aiding_commands_c Aiding Commands [C] +/// +///@{ + +//////////////////////////////////////////////////////////////////////////////// +// Descriptors +//////////////////////////////////////////////////////////////////////////////// + +enum +{ + MIP_AIDING_CMD_DESC_SET = 0x13, + + MIP_CMD_DESC_AIDING_ECEF_POS = 0x01, + MIP_CMD_DESC_AIDING_LLH_POS = 0x02, + MIP_CMD_DESC_AIDING_LOCAL_POS = 0x03, + MIP_CMD_DESC_AIDING_HEIGHT_ABS = 0x04, + MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x05, + MIP_CMD_DESC_AIDING_ECEF_VEL = 0x08, + MIP_CMD_DESC_AIDING_NED_VEL = 0x09, + MIP_CMD_DESC_AIDING_ODOM_VEL = 0x0A, + MIP_CMD_DESC_AIDING_WHEELSPEED = 0x0B, + MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x11, + MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x18, + MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x19, + MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x1A, + MIP_CMD_DESC_AIDING_ECHO_CONTROL = 0x6F, + + MIP_REPLY_DESC_AIDING_ECEF_POS = 0x81, + MIP_REPLY_DESC_AIDING_LLH_POS = 0x82, + MIP_REPLY_DESC_AIDING_ODOM_VEL = 0x8A, + MIP_REPLY_DESC_AIDING_HEADING_TRUE = 0x91, + MIP_REPLY_DESC_AIDING_ECHO_CONTROL = 0xEF, +}; + +//////////////////////////////////////////////////////////////////////////////// +// Shared Type Definitions +//////////////////////////////////////////////////////////////////////////////// + +typedef uint8_t mip_time_timebase; +static const mip_time_timebase MIP_TIME_TIMEBASE_INTERNAL_REFERENCE = 1; ///< Internal reference time from the device. +static const mip_time_timebase MIP_TIME_TIMEBASE_EXTERNAL_TIME = 2; ///< External reference time from PPS. + +struct mip_time +{ + mip_time_timebase timebase; ///< Timebase reference, e.g. internal, external, GPS, UTC, etc. + uint8_t reserved; ///< Reserved, set to 0x01. + uint64_t nanoseconds; ///< Nanoseconds since the timebase epoch. + +}; +typedef struct mip_time mip_time; +void insert_mip_time(struct mip_serializer* serializer, const mip_time* self); +void extract_mip_time(struct mip_serializer* serializer, mip_time* self); + +void insert_mip_time_timebase(struct mip_serializer* serializer, const mip_time_timebase self); +void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timebase* self); + + +//////////////////////////////////////////////////////////////////////////////// +// Mip Fields +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_ecef_pos (0x13,0x01) Ecef Pos [C] +/// +///@{ + +typedef uint16_t mip_aiding_ecef_pos_command_valid_flags; +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_X = 0x0002; ///< +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Y = 0x0004; ///< +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Z = 0x0008; ///< +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_ALL = 0x000E; + +struct mip_aiding_ecef_pos_command +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + double position[3]; ///< ECEF position. + float uncertainty[3]; ///< ECEF position uncertainty. + mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_ecef_pos_command mip_aiding_ecef_pos_command; +void insert_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command* self); +void extract_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, mip_aiding_ecef_pos_command* self); + +void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self); +void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self); + +struct mip_aiding_ecef_pos_response +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + double position[3]; ///< ECEF position. + float uncertainty[3]; ///< ECEF position uncertainty. + mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_ecef_pos_response mip_aiding_ecef_pos_response; +void insert_mip_aiding_ecef_pos_response(struct mip_serializer* serializer, const mip_aiding_ecef_pos_response* self); +void extract_mip_aiding_ecef_pos_response(struct mip_serializer* serializer, mip_aiding_ecef_pos_response* self); + +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* position_out, float* uncertainty_out, mip_aiding_ecef_pos_command_valid_flags* valid_flags_out); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_llh_pos (0x13,0x02) Llh Pos [C] +/// +///@{ + +typedef uint16_t mip_aiding_llh_pos_command_valid_flags; +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LATITUDE = 0x0002; ///< +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LONGITUDE = 0x0004; ///< +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_HEIGHT = 0x0008; ///< +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_ALL = 0x000E; + +struct mip_aiding_llh_pos_command +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + double latitude; + double longitude; + double height; + float uncertainty[3]; ///< ECEF position uncertainty. + mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_llh_pos_command mip_aiding_llh_pos_command; +void insert_mip_aiding_llh_pos_command(struct mip_serializer* serializer, const mip_aiding_llh_pos_command* self); +void extract_mip_aiding_llh_pos_command(struct mip_serializer* serializer, mip_aiding_llh_pos_command* self); + +void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self); +void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self); + +struct mip_aiding_llh_pos_response +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + double latitude; + double longitude; + double height; + float uncertainty[3]; ///< ECEF position uncertainty. + mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_llh_pos_response mip_aiding_llh_pos_response; +void insert_mip_aiding_llh_pos_response(struct mip_serializer* serializer, const mip_aiding_llh_pos_response* self); +void extract_mip_aiding_llh_pos_response(struct mip_serializer* serializer, mip_aiding_llh_pos_response* self); + +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* latitude_out, double* longitude_out, double* height_out, float* uncertainty_out, mip_aiding_llh_pos_command_valid_flags* valid_flags_out); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_vehicle_fixed_frame_velocity (0x13,0x0A) Vehicle Fixed Frame Velocity [C] +/// Estimate of velocity of the vehicle in the frame associated +/// with the given sensor ID. +/// +///@{ + +typedef uint16_t mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags; +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_X = 0x0002; ///< +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Y = 0x0004; ///< +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Z = 0x0008; ///< +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_ALL = 0x000E; + +struct mip_aiding_vehicle_fixed_frame_velocity_command +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + float velocity[3]; ///< [meters/second] + float uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; + +}; +typedef struct mip_aiding_vehicle_fixed_frame_velocity_command mip_aiding_vehicle_fixed_frame_velocity_command; +void insert_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command* self); +void extract_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command* self); + +void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self); +void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self); + +struct mip_aiding_vehicle_fixed_frame_velocity_response +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + float velocity[3]; ///< [meters/second] + float uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; + +}; +typedef struct mip_aiding_vehicle_fixed_frame_velocity_response mip_aiding_vehicle_fixed_frame_velocity_response; +void insert_mip_aiding_vehicle_fixed_frame_velocity_response(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_response* self); +void extract_mip_aiding_vehicle_fixed_frame_velocity_response(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_response* self); + +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* valid_flags_out); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_true_heading (0x13,0x11) True Heading [C] +/// +///@{ + +struct mip_aiding_true_heading_command +{ + mip_time time; + uint8_t sensor_id; + float heading; ///< Heading in [radians] + float uncertainty; + uint16_t valid_flags; + +}; +typedef struct mip_aiding_true_heading_command mip_aiding_true_heading_command; +void insert_mip_aiding_true_heading_command(struct mip_serializer* serializer, const mip_aiding_true_heading_command* self); +void extract_mip_aiding_true_heading_command(struct mip_serializer* serializer, mip_aiding_true_heading_command* self); + +struct mip_aiding_true_heading_response +{ + mip_time time; + uint8_t sensor_id; + float heading; ///< Heading in [radians] + float uncertainty; + uint16_t valid_flags; + +}; +typedef struct mip_aiding_true_heading_response mip_aiding_true_heading_response; +void insert_mip_aiding_true_heading_response(struct mip_serializer* serializer, const mip_aiding_true_heading_response* self); +void extract_mip_aiding_true_heading_response(struct mip_serializer* serializer, mip_aiding_true_heading_response* self); + +mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* heading_out, float* uncertainty_out, uint16_t* valid_flags_out); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_aiding_echo_control (0x13,0x6F) Aiding Echo Control [C] +/// +///@{ + +typedef uint8_t mip_aiding_aiding_echo_control_command_mode; +static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_SUPPRESS_ACK = 0; ///< Suppresses the usual command ack field for aiding messages. +static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_STANDARD = 1; ///< Normal ack/nack behavior. +static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_RESPONSE = 2; ///< Echo the data back as a response. + +struct mip_aiding_aiding_echo_control_command +{ + mip_function_selector function; + mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. + +}; +typedef struct mip_aiding_aiding_echo_control_command mip_aiding_aiding_echo_control_command; +void insert_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self); +void extract_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self); + +void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self); +void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self); + +struct mip_aiding_aiding_echo_control_response +{ + mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. + +}; +typedef struct mip_aiding_aiding_echo_control_response mip_aiding_aiding_echo_control_response; +void insert_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self); +void extract_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self); + +mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode); +mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out); +mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device); +///@} +/// + +///@} +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +#ifdef __cplusplus +} // namespace C +} // namespace mip +} // extern "C" +#endif // __cplusplus + diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp new file mode 100644 index 000000000..f533ae550 --- /dev/null +++ b/src/mip/definitions/commands_aiding.hpp @@ -0,0 +1,395 @@ +#pragma once + +#include "descriptors.h" +#include "../mip_result.h" + +#include +#include +#include + +namespace mip { +class Serializer; + +namespace C { +struct mip_interface; +} // namespace C + +namespace commands_aiding { + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup MipCommands_cpp MIP Commands [CPP] +///@{ +///@defgroup aiding_commands_cpp Aiding Commands [CPP] +/// +///@{ + +//////////////////////////////////////////////////////////////////////////////// +// Descriptors +//////////////////////////////////////////////////////////////////////////////// + +enum +{ + DESCRIPTOR_SET = 0x13, + + CMD_ECEF_POS = 0x01, + CMD_LLH_POS = 0x02, + CMD_LOCAL_POS = 0x03, + CMD_HEIGHT_ABS = 0x04, + CMD_HEIGHT_REL = 0x05, + CMD_ECEF_VEL = 0x08, + CMD_NED_VEL = 0x09, + CMD_ODOM_VEL = 0x0A, + CMD_WHEELSPEED = 0x0B, + CMD_HEADING_TRUE = 0x11, + CMD_DELTA_POSITION = 0x18, + CMD_DELTA_ATTITUDE = 0x19, + CMD_LOCAL_ANGULAR_RATE = 0x1A, + CMD_ECHO_CONTROL = 0x6F, + + REPLY_ECEF_POS = 0x81, + REPLY_LLH_POS = 0x82, + REPLY_ODOM_VEL = 0x8A, + REPLY_HEADING_TRUE = 0x91, + REPLY_ECHO_CONTROL = 0xEF, +}; + +//////////////////////////////////////////////////////////////////////////////// +// Shared Type Definitions +//////////////////////////////////////////////////////////////////////////////// + +struct Time +{ + enum class Timebase : uint8_t + { + INTERNAL_REFERENCE = 1, ///< Internal reference time from the device. + EXTERNAL_TIME = 2, ///< External reference time from PPS. + }; + + Timebase timebase = static_cast(0); ///< Timebase reference, e.g. internal, external, GPS, UTC, etc. + uint8_t reserved = 0; ///< Reserved, set to 0x01. + uint64_t nanoseconds = 0; ///< Nanoseconds since the timebase epoch. + +}; +void insert(Serializer& serializer, const Time& self); +void extract(Serializer& serializer, Time& self); + + +//////////////////////////////////////////////////////////////////////////////// +// Mip Fields +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_ecef_pos (0x13,0x01) Ecef Pos [CPP] +/// +///@{ + +struct EcefPos +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECEF_POS; + + static const bool HAS_FUNCTION_SELECTOR = false; + + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + X = 0x0002, ///< + Y = 0x0004, ///< + Z = 0x0008, ///< + ALL = 0x000E, + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool x() const { return (value & X) > 0; } + void x(bool val) { if(val) value |= X; else value &= ~X; } + bool y() const { return (value & Y) > 0; } + void y(bool val) { if(val) value |= Y; else value &= ~Y; } + bool z() const { return (value & Z) > 0; } + void z(bool val) { if(val) value |= Z; else value &= ~Z; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + double position[3] = {0}; ///< ECEF position. + float uncertainty[3] = {0}; ///< ECEF position uncertainty. + ValidFlags valid_flags; ///< Valid flags. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECEF_POS; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + double position[3] = {0}; ///< ECEF position. + float uncertainty[3] = {0}; ///< ECEF position uncertainty. + ValidFlags valid_flags; ///< Valid flags. + + }; +}; +void insert(Serializer& serializer, const EcefPos& self); +void extract(Serializer& serializer, EcefPos& self); + +void insert(Serializer& serializer, const EcefPos::Response& self); +void extract(Serializer& serializer, EcefPos::Response& self); + +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* positionOut, float* uncertaintyOut, EcefPos::ValidFlags* validFlagsOut); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_llh_pos (0x13,0x02) Llh Pos [CPP] +/// +///@{ + +struct LlhPos +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_LLH_POS; + + static const bool HAS_FUNCTION_SELECTOR = false; + + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + LATITUDE = 0x0002, ///< + LONGITUDE = 0x0004, ///< + HEIGHT = 0x0008, ///< + ALL = 0x000E, + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool latitude() const { return (value & LATITUDE) > 0; } + void latitude(bool val) { if(val) value |= LATITUDE; else value &= ~LATITUDE; } + bool longitude() const { return (value & LONGITUDE) > 0; } + void longitude(bool val) { if(val) value |= LONGITUDE; else value &= ~LONGITUDE; } + bool height() const { return (value & HEIGHT) > 0; } + void height(bool val) { if(val) value |= HEIGHT; else value &= ~HEIGHT; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + double latitude = 0; + double longitude = 0; + double height = 0; + float uncertainty[3] = {0}; ///< ECEF position uncertainty. + ValidFlags valid_flags; ///< Valid flags. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_LLH_POS; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + double latitude = 0; + double longitude = 0; + double height = 0; + float uncertainty[3] = {0}; ///< ECEF position uncertainty. + ValidFlags valid_flags; ///< Valid flags. + + }; +}; +void insert(Serializer& serializer, const LlhPos& self); +void extract(Serializer& serializer, LlhPos& self); + +void insert(Serializer& serializer, const LlhPos::Response& self); +void extract(Serializer& serializer, LlhPos::Response& self); + +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* latitudeOut, double* longitudeOut, double* heightOut, float* uncertaintyOut, LlhPos::ValidFlags* validFlagsOut); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_vehicle_fixed_frame_velocity (0x13,0x0A) Vehicle Fixed Frame Velocity [CPP] +/// Estimate of velocity of the vehicle in the frame associated +/// with the given sensor ID. +/// +///@{ + +struct VehicleFixedFrameVelocity +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ODOM_VEL; + + static const bool HAS_FUNCTION_SELECTOR = false; + + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + X = 0x0002, ///< + Y = 0x0004, ///< + Z = 0x0008, ///< + ALL = 0x000E, + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool x() const { return (value & X) > 0; } + void x(bool val) { if(val) value |= X; else value &= ~X; } + bool y() const { return (value & Y) > 0; } + void y(bool val) { if(val) value |= Y; else value &= ~Y; } + bool z() const { return (value & Z) > 0; } + void z(bool val) { if(val) value |= Z; else value &= ~Z; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + float velocity[3] = {0}; ///< [meters/second] + float uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + ValidFlags valid_flags; + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ODOM_VEL; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + float velocity[3] = {0}; ///< [meters/second] + float uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + ValidFlags valid_flags; + + }; +}; +void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); +void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); + +void insert(Serializer& serializer, const VehicleFixedFrameVelocity::Response& self); +void extract(Serializer& serializer, VehicleFixedFrameVelocity::Response& self); + +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, VehicleFixedFrameVelocity::ValidFlags* validFlagsOut); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_true_heading (0x13,0x11) True Heading [CPP] +/// +///@{ + +struct TrueHeading +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; + + static const bool HAS_FUNCTION_SELECTOR = false; + + Time time; + uint8_t sensor_id = 0; + float heading = 0; ///< Heading in [radians] + float uncertainty = 0; + uint16_t valid_flags = 0; + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_HEADING_TRUE; + + Time time; + uint8_t sensor_id = 0; + float heading = 0; ///< Heading in [radians] + float uncertainty = 0; + uint16_t valid_flags = 0; + + }; +}; +void insert(Serializer& serializer, const TrueHeading& self); +void extract(Serializer& serializer, TrueHeading& self); + +void insert(Serializer& serializer, const TrueHeading::Response& self); +void extract(Serializer& serializer, TrueHeading::Response& self); + +CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags, Time* timeOut, uint8_t* sensorIdOut, float* headingOut, float* uncertaintyOut, uint16_t* validFlagsOut); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_aiding_echo_control (0x13,0x6F) Aiding Echo Control [CPP] +/// +///@{ + +struct AidingEchoControl +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class Mode : uint8_t + { + SUPPRESS_ACK = 0, ///< Suppresses the usual command ack field for aiding messages. + STANDARD = 1, ///< Normal ack/nack behavior. + RESPONSE = 2, ///< Echo the data back as a response. + }; + + FunctionSelector function = static_cast(0); + Mode mode = static_cast(0); ///< Controls data echoing. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; + + Mode mode = static_cast(0); ///< Controls data echoing. + + }; +}; +void insert(Serializer& serializer, const AidingEchoControl& self); +void extract(Serializer& serializer, AidingEchoControl& self); + +void insert(Serializer& serializer, const AidingEchoControl::Response& self); +void extract(Serializer& serializer, AidingEchoControl::Response& self); + +CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); +CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); +CmdResult saveAidingEchoControl(C::mip_interface& device); +CmdResult loadAidingEchoControl(C::mip_interface& device); +CmdResult defaultAidingEchoControl(C::mip_interface& device); +///@} +/// + +///@} +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +} // namespace commands_aiding +} // namespace mip + diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index df37cf95b..ccbca3b69 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -5058,71 +5058,6 @@ mip_cmd_result mip_filter_set_initial_heading(struct mip_interface* device, floa return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SET_INITIAL_HEADING, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self) -{ - insert_u8(serializer, self->source_id); - - insert_double(serializer, self->gps_time); - - insert_u16(serializer, self->gps_week); - - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->velocity[i]); - - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - insert_u8(serializer, self->reserved[i]); - -} -void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self) -{ - extract_u8(serializer, &self->source_id); - - extract_double(serializer, &self->gps_time); - - extract_u16(serializer, &self->gps_week); - - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity[i]); - - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - extract_u8(serializer, &self->reserved[i]); - -} - -mip_cmd_result mip_filter_vehicle_frame_vel_with_unc_and_timestamp(struct mip_interface* device, uint8_t source_id, double gps_time, uint16_t gps_week, const float* velocity, const float* velocity_uncertainty, const uint8_t* reserved) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_u8(&serializer, source_id); - - insert_double(&serializer, gps_time); - - insert_u16(&serializer, gps_week); - - assert(velocity || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, velocity[i]); - - assert(velocity_uncertainty || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, velocity_uncertainty[i]); - - assert(reserved || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - insert_u8(&serializer, reserved[i]); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index c26e36d99..7048f63d0 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -4581,70 +4581,6 @@ CmdResult setInitialHeading(C::mip_interface& device, float heading) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SET_INITIAL_HEADING, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self) -{ - insert(serializer, self.source_id); - - insert(serializer, self.gps_time); - - insert(serializer, self.gps_week); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.velocity[i]); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - insert(serializer, self.reserved[i]); - -} -void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self) -{ - extract(serializer, self.source_id); - - extract(serializer, self.gps_time); - - extract(serializer, self.gps_week); - - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.velocity[i]); - - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - extract(serializer, self.reserved[i]); - -} - -CmdResult vehicleFrameVelWithUncAndTimestamp(C::mip_interface& device, uint8_t sourceId, double gpsTime, uint16_t gpsWeek, const float* velocity, const float* velocityUncertainty, const uint8_t* reserved) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, sourceId); - - insert(serializer, gpsTime); - - insert(serializer, gpsWeek); - - assert(velocity || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert(serializer, velocity[i]); - - assert(velocityUncertainty || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert(serializer, velocityUncertainty[i]); - - assert(reserved || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - insert(serializer, reserved[i]); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} } // namespace commands_filter } // namespace mip diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index c99510354..b941c6b77 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -95,7 +95,6 @@ enum MIP_CMD_DESC_WHEELED_VEHICLE_CONSTRAINT_CONTROL = 0x63, MIP_CMD_DESC_GNSS_ANTENNA_CALIBRATION_CONTROL = 0x64, MIP_CMD_DESC_SENSOR_TO_VEHICLE_CALIBRATION_CONTROL = 0x65, - MIP_CMD_DESC_FILTER_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x66, MIP_REPLY_DESC_FILTER_VEHICLE_DYNAMICS_MODE = 0x80, MIP_REPLY_DESC_FILTER_SENSOR2VEHICLE_ROTATION_EULER = 0x81, @@ -1971,30 +1970,6 @@ void extract_mip_filter_set_initial_heading_command(struct mip_serializer* seria mip_cmd_result mip_filter_set_initial_heading(struct mip_interface* device, float heading); ///@} /// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_filter_vehicle_frame_vel_with_unc_and_timestamp (0x0D,0x66) Vehicle Frame Vel With Unc And Timestamp [C] -/// Send and estimate of velocity in the vehicle frame at specified time, with associated uncertainties, to be incorporated by the Navigation Filter in the state estimation process. -/// The source_id field is used to differentiate between different physical measurement sources; note that source_id == 0 is reserved, and will result an invalid command response. -/// -///@{ - -struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command -{ - uint8_t source_id; ///< Source ID for this estimate - double gps_time; ///< [seconds] - uint16_t gps_week; ///< [GPS week number, not modulus 1024] - float velocity[3]; ///< [meters/second] - float velocity_uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - uint8_t reserved[4]; - -}; -typedef struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command; -void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(struct mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self); -void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command(struct mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_command* self); - -mip_cmd_result mip_filter_vehicle_frame_vel_with_unc_and_timestamp(struct mip_interface* device, uint8_t source_id, double gps_time, uint16_t gps_week, const float* velocity, const float* velocity_uncertainty, const uint8_t* reserved); -///@} -/// ///@} ///@} diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index e49a50209..2f88df9ed 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -94,7 +94,6 @@ enum CMD_VEHICLE_CONSTRAINT_CONTROL = 0x63, CMD_ANTENNA_CALIBRATION_CONTROL = 0x64, CMD_TO_VEHICLE_CALIBRATION_CONTROL = 0x65, - CMD_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x66, REPLY_VEHICLE_DYNAMICS_MODE = 0x80, REPLY_SENSOR2VEHICLE_ROTATION_EULER = 0x81, @@ -2456,34 +2455,6 @@ void extract(Serializer& serializer, SetInitialHeading& self); CmdResult setInitialHeading(C::mip_interface& device, float heading); ///@} /// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_vehicle_frame_vel_with_unc_and_timestamp (0x0D,0x66) Vehicle Frame Vel With Unc And Timestamp [CPP] -/// Send and estimate of velocity in the vehicle frame at specified time, with associated uncertainties, to be incorporated by the Navigation Filter in the state estimation process. -/// The source_id field is used to differentiate between different physical measurement sources; note that source_id == 0 is reserved, and will result an invalid command response. -/// -///@{ - -struct VehicleFrameVelWithUncAndTimestamp -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP; - - static const bool HAS_FUNCTION_SELECTOR = false; - - uint8_t source_id = 0; ///< Source ID for this estimate - double gps_time = 0; ///< [seconds] - uint16_t gps_week = 0; ///< [GPS week number, not modulus 1024] - float velocity[3] = {0}; ///< [meters/second] - float velocity_uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - uint8_t reserved[4] = {0}; - -}; -void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self); -void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self); - -CmdResult vehicleFrameVelWithUncAndTimestamp(C::mip_interface& device, uint8_t sourceId, double gpsTime, uint16_t gpsWeek, const float* velocity, const float* velocityUncertainty, const uint8_t* reserved); -///@} -/// ///@} ///@} diff --git a/src/mip/definitions/data_filter.c b/src/mip/definitions/data_filter.c index 634bdae87..f719207eb 100644 --- a/src/mip/definitions/data_filter.c +++ b/src/mip/definitions/data_filter.c @@ -1687,56 +1687,6 @@ void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags( *self = tmp; } -void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self) -{ - insert_u8(serializer, self->source_id); - - insert_double(serializer, self->gps_time); - - insert_u16(serializer, self->gps_week); - - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->velocity[i]); - - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - insert_u8(serializer, self->reserved[i]); - - insert_u16(serializer, self->valid_flags); - -} -void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self) -{ - extract_u8(serializer, &self->source_id); - - extract_double(serializer, &self->gps_time); - - extract_u16(serializer, &self->gps_week); - - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity[i]); - - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - extract_u8(serializer, &self->reserved[i]); - - extract_u16(serializer, &self->valid_flags); - -} -bool extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data_from_field(const mip_field* field, void* ptr) -{ - assert(ptr); - mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self = ptr; - struct mip_serializer serializer; - mip_serializer_init_from_field(&serializer, field); - extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(&serializer, self); - return mip_serializer_is_complete(&serializer); -} - #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/data_filter.cpp b/src/mip/definitions/data_filter.cpp index 87eaca530..fdb85f32b 100644 --- a/src/mip/definitions/data_filter.cpp +++ b/src/mip/definitions/data_filter.cpp @@ -1084,47 +1084,6 @@ void extract(Serializer& serializer, GnssDualAntennaStatus& self) } -void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self) -{ - insert(serializer, self.source_id); - - insert(serializer, self.gps_time); - - insert(serializer, self.gps_week); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.velocity[i]); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - insert(serializer, self.reserved[i]); - - insert(serializer, self.valid_flags); - -} -void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self) -{ - extract(serializer, self.source_id); - - extract(serializer, self.gps_time); - - extract(serializer, self.gps_week); - - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.velocity[i]); - - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.velocity_uncertainty[i]); - - for(unsigned int i=0; i < 4; i++) - extract(serializer, self.reserved[i]); - - extract(serializer, self.valid_flags); - -} - } // namespace data_filter } // namespace mip diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index 633132e2c..9dfe6eedd 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -92,7 +92,6 @@ enum MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR = 0x47, MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, MIP_DATA_DESC_FILTER_GNSS_DUAL_ANTENNA_STATUS = 0x49, - MIP_DATA_DESC_FILTER_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x50, }; @@ -1370,30 +1369,6 @@ void extract_mip_filter_gnss_dual_antenna_status_data_fix_type(struct mip_serial void insert_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags self); void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags* self); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_filter_vehicle_frame_vel_with_unc_and_timestamp (0x82,0x50) Vehicle Frame Vel With Unc And Timestamp [C] -/// Estimate of velocity in the vehicle frame at specified time, with associated uncertainties. -/// -///@{ - -struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data -{ - uint8_t source_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - double gps_time; ///< [seconds] - uint16_t gps_week; ///< [GPS week number, not modulus 1024] - float velocity[3]; ///< [meters/second] - float velocity_uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - uint8_t reserved[4]; - uint16_t valid_flags; ///< 0 - invalid, 1 - valid - -}; -typedef struct mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data; -void insert_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(struct mip_serializer* serializer, const mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self); -void extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data(struct mip_serializer* serializer, mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data* self); -bool extract_mip_filter_vehicle_frame_vel_with_unc_and_timestamp_data_from_field(const struct mip_field* field, void* ptr); - ///@} /// diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 2782c16a7..78bf3fd2b 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -91,7 +91,6 @@ enum DATA_ODOMETER_SCALE_FACTOR_ERROR = 0x47, DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, DATA_GNSS_DUAL_ANTENNA_STATUS = 0x49, - DATA_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP = 0x50, }; @@ -1969,38 +1968,6 @@ struct GnssDualAntennaStatus void insert(Serializer& serializer, const GnssDualAntennaStatus& self); void extract(Serializer& serializer, GnssDualAntennaStatus& self); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_vehicle_frame_vel_with_unc_and_timestamp (0x82,0x50) Vehicle Frame Vel With Unc And Timestamp [CPP] -/// Estimate of velocity in the vehicle frame at specified time, with associated uncertainties. -/// -///@{ - -struct VehicleFrameVelWithUncAndTimestamp -{ - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEH_FRAME_VEL_WITH_UNC_AND_TIMESTAMP; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(source_id,gps_time,gps_week,velocity[0],velocity[1],velocity[2],velocity_uncertainty[0],velocity_uncertainty[1],velocity_uncertainty[2],reserved,valid_flags); - } - - uint8_t source_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - double gps_time = 0; ///< [seconds] - uint16_t gps_week = 0; ///< [GPS week number, not modulus 1024] - float velocity[3] = {0}; ///< [meters/second] - float velocity_uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - uint8_t reserved[4] = {0}; - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - -}; -void insert(Serializer& serializer, const VehicleFrameVelWithUncAndTimestamp& self); -void extract(Serializer& serializer, VehicleFrameVelWithUncAndTimestamp& self); - ///@} /// From af3ad1fb6e3530dc450177a6646a825dca45db7b Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Thu, 8 Jun 2023 17:58:54 -0400 Subject: [PATCH 061/252] Fixed bytes skipped, serial port reads on Windows --- src/mip/mip_interface.c | 2 +- src/mip/mip_parser.c | 17 +++++++++++++---- src/mip/mip_parser.h | 11 ++++++----- src/mip/utils/serial_port.c | 13 ++++++++++--- 4 files changed, 30 insertions(+), 13 deletions(-) diff --git a/src/mip/mip_interface.c b/src/mip/mip_interface.c index 8a5e14f9a..d0c65735f 100644 --- a/src/mip/mip_interface.c +++ b/src/mip/mip_interface.c @@ -9,7 +9,6 @@ #include - //////////////////////////////////////////////////////////////////////////////// ///@typedef mip_send_callback /// @@ -377,6 +376,7 @@ bool mip_interface_default_update(struct mip_interface* device, timeout_type wai assert(count <= max_count); mip_parser_process_written(parser, count, timestamp, 0); + mip_cmd_queue_update(mip_interface_cmd_queue(device), timestamp); return true; diff --git a/src/mip/mip_parser.c b/src/mip/mip_parser.c index 9e7848d37..6b2f52fef 100644 --- a/src/mip/mip_parser.c +++ b/src/mip/mip_parser.c @@ -58,6 +58,7 @@ void mip_parser_reset(mip_parser* parser) byte_ring_clear(&parser->_ring); MIP_DIAG_ZERO(parser->_diag_bytes_read); + MIP_DIAG_ZERO(parser->_diag_bytes_skipped); MIP_DIAG_ZERO(parser->_diag_packet_bytes); MIP_DIAG_ZERO(parser->_diag_valid_packets); MIP_DIAG_ZERO(parser->_diag_invalid_packets); @@ -117,7 +118,10 @@ remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer if( parser->_expected_length != MIPPARSER_RESET_LENGTH && (timestamp - parser->_start_time) > parser->_timeout ) { if( byte_ring_count(&parser->_ring) > 0 ) + { byte_ring_pop(&parser->_ring, 1); + MIP_DIAG_INC(parser->_diag_bytes_skipped, 1); + } parser->_expected_length = MIPPARSER_RESET_LENGTH; @@ -183,7 +187,11 @@ bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packe if( parser->_expected_length == MIPPARSER_RESET_LENGTH ) { if( byte_ring_at(&parser->_ring, MIP_INDEX_SYNC1) != MIP_SYNC1 ) + { byte_ring_pop(&parser->_ring, 1); + + MIP_DIAG_INC(parser->_diag_bytes_skipped, 1); + } else { // Synchronized - set the start time and expect more data. @@ -197,6 +205,7 @@ bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packe if( byte_ring_at(&parser->_ring, MIP_INDEX_SYNC2) != MIP_SYNC2 ) { byte_ring_pop(&parser->_ring, 1); + MIP_DIAG_INC(parser->_diag_bytes_skipped, 1); parser->_expected_length = MIPPARSER_RESET_LENGTH; } else @@ -218,7 +227,7 @@ bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packe { // Invalid packet, drop just the first sync byte and restart. byte_ring_pop(&parser->_ring, 1); - + MIP_DIAG_INC(parser->_diag_bytes_skipped, 1); MIP_DIAG_INC(parser->_diag_invalid_packets, 1); } else // Checksum is valid @@ -411,7 +420,7 @@ uint32_t mip_parser_diagnostic_packet_bytes(const mip_parser* parser) /// uint32_t mip_parser_diagnostic_bytes_skipped(const mip_parser* parser) { - return parser->_diag_bytes_read - parser->_diag_packet_bytes; + return parser->_diag_bytes_skipped; } @@ -429,7 +438,7 @@ uint32_t mip_parser_diagnostic_valid_packets(const mip_parser* parser) /// These invalid packets are not emitted by the parser and are not included in /// the "valid packets" or "packet bytes" counters. /// -uint16_t mip_parser_diagnostic_invalid_packets(const mip_parser* parser) +uint32_t mip_parser_diagnostic_invalid_packets(const mip_parser* parser) { return parser->_diag_invalid_packets; } @@ -443,7 +452,7 @@ uint16_t mip_parser_diagnostic_invalid_packets(const mip_parser* parser) ///@li The length byte is corrupted to make the packet look longer ///@li The connection bandwidth and/or latency is too low /// -uint16_t mip_parser_diagnostic_timeouts(const mip_parser* parser) +uint32_t mip_parser_diagnostic_timeouts(const mip_parser* parser) { return parser->_diag_timeouts; } diff --git a/src/mip/mip_parser.h b/src/mip/mip_parser.h index 74595ee4b..746c5baa5 100644 --- a/src/mip/mip_parser.h +++ b/src/mip/mip_parser.h @@ -64,10 +64,11 @@ typedef struct mip_parser #ifdef MIP_ENABLE_DIAGNOSTICS uint32_t _diag_bytes_read; ///<@private Counts bytes read from the user input buffer. + uint32_t _diag_bytes_skipped; ///<@private Counts bytes read from the user input buffer. uint32_t _diag_packet_bytes; ///<@private Counts bytes parsed into valid packets. uint32_t _diag_valid_packets; ///<@private Counts packets successfully parsed. - uint16_t _diag_invalid_packets; ///<@private Counts invalid packets encountered (bad checksums). - uint16_t _diag_timeouts; ///<@private Counts packet timeouts. + uint32_t _diag_invalid_packets; ///<@private Counts invalid packets encountered (bad checksums). + uint32_t _diag_timeouts; ///<@private Counts packet timeouts. #endif // MIP_ENABLE_DIAGNOSTICS } mip_parser; @@ -108,12 +109,12 @@ timestamp_type mip_parser_last_packet_timestamp(const mip_parser* parser); #ifdef MIP_ENABLE_DIAGNOSTICS uint32_t mip_parser_diagnostic_bytes_read(const mip_parser* parser); -uint32_t mip_parser_diagnostic_packet_bytes(const mip_parser* parser); uint32_t mip_parser_diagnostic_bytes_skipped(const mip_parser* parser); +uint32_t mip_parser_diagnostic_packet_bytes(const mip_parser* parser); uint32_t mip_parser_diagnostic_valid_packets(const mip_parser* parser); -uint16_t mip_parser_diagnostic_invalid_packets(const mip_parser* parser); -uint16_t mip_parser_diagnostic_timeouts(const mip_parser* parser); +uint32_t mip_parser_diagnostic_invalid_packets(const mip_parser* parser); +uint32_t mip_parser_diagnostic_timeouts(const mip_parser* parser); #endif // MIP_ENABLE_DIAGNOSTICS diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 3b85b9e8f..6199ea1b2 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -84,6 +84,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) } //Set the timeouts + COMMTIMEOUTS timeouts; GetCommTimeouts(port->handle, &timeouts); @@ -93,9 +94,9 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) timeouts.ReadTotalTimeoutConstant = 1; timeouts.WriteTotalTimeoutMultiplier = 1; timeouts.WriteTotalTimeoutConstant = 1; - + SetCommTimeouts(port->handle, &timeouts); - + //Setup the com port parameters ready = GetCommState(port->handle, &dcb); @@ -249,14 +250,20 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai if(!serial_port_is_open(port)) return false; + uint32_t bytes_available = serial_port_read_count(port); + #ifdef WIN32 //Windows if( wait_time <= 0 ) { - if( serial_port_read_count(port) == 0 ) + if(bytes_available == 0 ) return true; } + //Don't let windows block on the read + if(bytes_available < num_bytes) + num_bytes = bytes_available; + DWORD local_bytes_read; //Call the windows read function From 51060188b2b8d407c40eb4deda98ee9fddf0d860 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Thu, 8 Jun 2023 20:56:26 -0400 Subject: [PATCH 062/252] Adding mac support --- src/mip/platform/serial_connection.cpp | 2 +- src/mip/utils/serial_port.c | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index a9c039d18..561073fec 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -22,7 +22,7 @@ SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrat #ifdef WIN32 mPort.handle = INVALID_HANDLE_VALUE; #else - mPort.handle = 0; + mPort.handle = -1; #endif } diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 3b85b9e8f..b6386f72c 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -127,9 +127,14 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) } #else //Linux - + +#ifdef __APPLE__ + port->handle = open(port_str, O_RDWR | O_NOCTTY | O_NDELAY); +#else port->handle = open(port_str, O_RDWR | O_NOCTTY | O_SYNC); - +#endif + + if (port->handle < 0) { MIP_LOG_ERROR("Unable to open port (%d): %s\n", errno, strerror(errno)); @@ -200,7 +205,7 @@ bool serial_port_close(serial_port *port) #else //Linux close(port->handle); - port->handle = 0; + port->handle = -1; #endif return true; From 92ec513a9859284e6149f252566bda03ba9c183f Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Fri, 9 Jun 2023 12:03:35 -0400 Subject: [PATCH 063/252] Serial read will always read 1 byte --- src/mip/utils/serial_port.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 6199ea1b2..fc9c050e2 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -262,7 +262,7 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai //Don't let windows block on the read if(bytes_available < num_bytes) - num_bytes = bytes_available; + num_bytes = (bytes_available > 0) ? bytes_available : 1; DWORD local_bytes_read; From 1427a72a86c67018988153a32b2829b8a8e1e8c4 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 9 Jun 2023 17:43:32 -0400 Subject: [PATCH 064/252] Add optional offset parameter to mip::insert template function. --- src/mip/utils/serialization.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mip/utils/serialization.h b/src/mip/utils/serialization.h index 076a98c5a..ae460ddb6 100644 --- a/src/mip/utils/serialization.h +++ b/src/mip/utils/serialization.h @@ -203,9 +203,9 @@ typename std::enable_if< std::is_enum::value, void>::type ///@returns true if sufficient space was available, false otherwise. /// template -bool insert(const T& value, uint8_t* buffer, size_t bufferSize) +bool insert(const T& value, uint8_t* buffer, size_t bufferSize, size_t offset=0) { - Serializer serializer(buffer, bufferSize); + Serializer serializer(buffer, bufferSize, offset); insert(serializer, value); return !!serializer; } From 6b38ac925e8a2c1993f2744c0f6280a0caf3b47d Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 12 Jun 2023 13:20:00 -0400 Subject: [PATCH 065/252] Add MIP_STATUS_USER_START to mip_cmd_result. --- src/mip/mip_result.c | 12 +++++++++++- src/mip/mip_result.h | 18 ++++++++++++------ 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/mip/mip_result.c b/src/mip/mip_result.c index 12ca8c9df..e8c08e438 100644 --- a/src/mip/mip_result.c +++ b/src/mip/mip_result.c @@ -32,7 +32,9 @@ const char* mip_cmd_result_to_string(enum mip_cmd_result result) case MIP_NACK_COMMAND_FAILED: return "Command Failed"; case MIP_NACK_COMMAND_TIMEOUT: return "Device Error"; - default: return ""; + default: + if(mip_cmd_result_is_user(result)) return "User Status"; + else return "Unknown"; } } @@ -61,6 +63,14 @@ bool mip_cmd_result_is_status(enum mip_cmd_result result) return result < 0; } +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the result code was generated by user software. +/// +bool mip_cmd_result_is_user(enum mip_cmd_result result) +{ + return result <= MIP_STATUS_USER_START; +} + //////////////////////////////////////////////////////////////////////////////// ///@brief Determines if the result is an ack (successful response from the device) /// diff --git a/src/mip/mip_result.h b/src/mip/mip_result.h index a09f6be70..cd4a2fbd5 100644 --- a/src/mip/mip_result.h +++ b/src/mip/mip_result.h @@ -22,13 +22,17 @@ extern "C" { /// typedef enum mip_cmd_result { - MIP_STATUS_ERROR = -6, ///< Command could not be executed (error sending/receiving) - MIP_STATUS_CANCELLED = -5, ///< Command was canceled in software. - MIP_STATUS_TIMEDOUT = -4, ///< Reply was not received before timeout expired. - MIP_STATUS_WAITING = -3, ///< Waiting for command reply (timeout timer has started). - MIP_STATUS_PENDING = -2, ///< Command has been queued but the I/O update hasn't run yet. - MIP_STATUS_NONE = -1, ///< Command has been initialized but not queued yet. + MIP_STATUS_USER_START = -10, ///< Values defined by user code must be less than or equal to this value. + // Status codes < 0 + MIP_STATUS_ERROR = -6, ///< Command could not be executed (error sending/receiving) + MIP_STATUS_CANCELLED = -5, ///< Command was canceled in software. + MIP_STATUS_TIMEDOUT = -4, ///< Reply was not received before timeout expired. + MIP_STATUS_WAITING = -3, ///< Waiting for command reply (timeout timer has started). + MIP_STATUS_PENDING = -2, ///< Command has been queued but the I/O update hasn't run yet. + MIP_STATUS_NONE = -1, ///< Command has been initialized but not queued yet. + + // Device replies >= 0 MIP_ACK_OK = 0x00, ///< Command completed successfully. MIP_NACK_COMMAND_UNKNOWN = 0x01, ///< Command not supported. MIP_NACK_INVALID_CHECKSUM = 0x02, ///< Reserved. @@ -43,6 +47,7 @@ bool mip_cmd_result_is_finished(enum mip_cmd_result result); bool mip_cmd_result_is_reply(enum mip_cmd_result result); bool mip_cmd_result_is_status(enum mip_cmd_result result); +bool mip_cmd_result_is_user(enum mip_cmd_result result); bool mip_cmd_result_is_ack(enum mip_cmd_result result); @@ -65,6 +70,7 @@ bool mip_cmd_result_is_ack(enum mip_cmd_result result); /// struct CmdResult { + static constexpr C::mip_cmd_result STATUS_USER = C::MIP_STATUS_USER_START; ///<@copydoc mip::C::MIP_STATUS_USER_START static constexpr C::mip_cmd_result STATUS_ERROR = C::MIP_STATUS_ERROR; ///<@copydoc mip::C::MIP_STATUS_ERROR static constexpr C::mip_cmd_result STATUS_CANCELLED = C::MIP_STATUS_CANCELLED; ///<@copydoc mip::C::MIP_STATUS_CANCELLED static constexpr C::mip_cmd_result STATUS_TIMEDOUT = C::MIP_STATUS_TIMEDOUT; ///<@copydoc mip::C::MIP_STATUS_TIMEDOUT From 2d5bd73d30088c4c668e695bec0b406fb0f98746 Mon Sep 17 00:00:00 2001 From: Rob <87914992+robbiefish@users.noreply.github.com> Date: Tue, 13 Jun 2023 16:50:24 -0400 Subject: [PATCH 066/252] Initializes serial port to invalid value so it doesn't show as connected when initialized (#63) * Initializes serial port to invalid value so it doesn't show as connected * Updates based on PR review --- src/mip/platform/serial_connection.cpp | 10 +++------ src/mip/utils/serial_port.c | 31 +++++++++++++++++++++----- src/mip/utils/serial_port.h | 2 +- 3 files changed, 30 insertions(+), 13 deletions(-) diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index a9c039d18..2074a3918 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -19,11 +19,7 @@ SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrat mBaudrate = baudrate; mType = TYPE; - #ifdef WIN32 - mPort.handle = INVALID_HANDLE_VALUE; -#else - mPort.handle = 0; -#endif + serial_port_init(&mPort); } ///@brief Closes the underlying serial port @@ -42,7 +38,7 @@ bool SerialConnection::isConnected() bool SerialConnection::connect() { if (serial_port_is_open(&mPort)) - return false; + return true; return serial_port_open(&mPort, mPortName.c_str(), mBaudrate); } @@ -51,7 +47,7 @@ bool SerialConnection::connect() bool SerialConnection::disconnect() { if (!serial_port_is_open(&mPort)) - return true; + return true; return serial_port_close(&mPort); } diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index fc9c050e2..da146f025 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -6,6 +6,9 @@ #define COM_PORT_BUFFER_SIZE 0x200 #ifndef WIN32 //Unix only + +#define INVALID_HANDLE_VALUE -1 + speed_t baud_rate_to_speed(int baud_rate) { switch(baud_rate) @@ -54,6 +57,11 @@ speed_t baud_rate_to_speed(int baud_rate) } #endif +void serial_port_init(serial_port *port) +{ + port->handle = INVALID_HANDLE_VALUE; +} + bool serial_port_open(serial_port *port, const char *port_str, int baudrate) { if(port_str == NULL) @@ -194,15 +202,12 @@ bool serial_port_close(serial_port *port) return false; #ifdef WIN32 //Windows - //Close the serial port CloseHandle(port->handle); - port->handle = INVALID_HANDLE_VALUE; - #else //Linux close(port->handle); - port->handle = 0; #endif + port->handle = INVALID_HANDLE_VALUE; return true; } @@ -277,7 +282,23 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai int poll_status = poll(&poll_fd, 1, wait_time); // Keep reading and polling while there is still data available - if (poll_status > 0 && poll_fd.revents & POLLIN) + if (poll_status == -1) + { + MIP_LOG_ERROR("Failed to poll serial port (%d): %s\n", errno, strerror(errno)); + return false; + } + else if (poll_fd.revents & POLLHUP) + { + MIP_LOG_ERROR("Poll encountered HUP, closing device"); + serial_port_close(port); + return false; + } + else if (poll_fd.revents & POLLERR || poll_fd.revents & POLLNVAL) + { + MIP_LOG_ERROR("Poll encountered error\n"); + return false; + } + else if (poll_status > 0 && poll_fd.revents & POLLIN) { ssize_t local_bytes_read = read(port->handle, buffer, num_bytes); diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index d06359d26..fd4c39e28 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -42,7 +42,7 @@ typedef struct serial_port #endif } serial_port; - +void serial_port_init(serial_port *port); bool serial_port_open(serial_port *port, const char *port_str, int baudrate); bool serial_port_close(serial_port *port); bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, size_t *bytes_written); From 8f908153c75c32269ad7f1d3c653f01f70ae0010 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 13 Jun 2023 12:31:01 -0400 Subject: [PATCH 067/252] Add const to appropriate connection functions. --- src/mip/extras/recording_connection.hpp | 2 +- src/mip/mip_device.hpp | 4 ++-- src/mip/platform/serial_connection.cpp | 4 ++-- src/mip/platform/serial_connection.hpp | 4 ++-- src/mip/platform/tcp_connection.cpp | 6 +++--- src/mip/platform/tcp_connection.hpp | 6 +++--- src/mip/utils/serial_port.c | 8 ++++---- src/mip/utils/serial_port.h | 2 +- 8 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index 7c7a8e2c7..8552d2069 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -28,7 +28,7 @@ class RecordingConnection : public Connection bool sendToDevice(const uint8_t* data, size_t length) final; bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) final; - bool isConnected() + bool isConnected() const { if(mConnection) return mConnection->isConnected(); diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index a313db34e..3318c0698 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -153,13 +153,13 @@ class Connection virtual bool sendToDevice(const uint8_t* data, size_t length) = 0; virtual bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, Timestamp* timestamp_out) = 0; - virtual bool isConnected() = 0; + virtual bool isConnected() const = 0; virtual bool connect() = 0; virtual bool disconnect() = 0; void connect_interface(C::mip_interface* device); - const char* type() { return mType; }; + const char* type() const { return mType; }; protected: diff --git a/src/mip/platform/serial_connection.cpp b/src/mip/platform/serial_connection.cpp index 2074a3918..22603e7a6 100644 --- a/src/mip/platform/serial_connection.cpp +++ b/src/mip/platform/serial_connection.cpp @@ -25,11 +25,11 @@ SerialConnection::SerialConnection(const std::string& portName, uint32_t baudrat ///@brief Closes the underlying serial port SerialConnection::~SerialConnection() { - disconnect(); + SerialConnection::disconnect(); } ///@brief Check if the port is connected -bool SerialConnection::isConnected() +bool SerialConnection::isConnected() const { return serial_port_is_open(&mPort); } diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index f0283a285..da281b176 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -30,13 +30,13 @@ class SerialConnection : public mip::Connection bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; - bool isConnected(); + bool isConnected() const; bool connect(); bool disconnect(); - void connectionInfo(std::string &name, uint32_t &baudrate) + void connectionInfo(std::string &name, uint32_t &baudrate) const { name = mPortName; baudrate = mBaudrate; diff --git a/src/mip/platform/tcp_connection.cpp b/src/mip/platform/tcp_connection.cpp index d1ec0b6d7..d4d99dd7f 100644 --- a/src/mip/platform/tcp_connection.cpp +++ b/src/mip/platform/tcp_connection.cpp @@ -25,11 +25,11 @@ TcpConnection::TcpConnection(const std::string& hostname, uint16_t port) TcpConnection::~TcpConnection() { if(mConnected) - disconnect(); + TcpConnection::disconnect(); } ///@brief Check if the socket is connected -bool TcpConnection::isConnected() +bool TcpConnection::isConnected() const { return mConnected; } @@ -41,7 +41,7 @@ bool TcpConnection::connect() return false; mConnected = tcp_socket_open(&mSocket, mHostname.c_str(), mPort, 3000); - + return mConnected; } diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index 5828c267c..591dc8634 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -29,12 +29,12 @@ class TcpConnection : public mip::Connection bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; - - bool isConnected(); + + bool isConnected() const; bool connect(); bool disconnect(); - void connectionInfo(std::string &host_name, uint32_t &port) + void connectionInfo(std::string &host_name, uint32_t &port) const { host_name = mHostname; port = mPort; diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index da146f025..51e72d59a 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -92,7 +92,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) } //Set the timeouts - + COMMTIMEOUTS timeouts; GetCommTimeouts(port->handle, &timeouts); @@ -102,9 +102,9 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) timeouts.ReadTotalTimeoutConstant = 1; timeouts.WriteTotalTimeoutMultiplier = 1; timeouts.WriteTotalTimeoutConstant = 1; - + SetCommTimeouts(port->handle, &timeouts); - + //Setup the com port parameters ready = GetCommState(port->handle, &dcb); @@ -344,7 +344,7 @@ uint32_t serial_port_read_count(serial_port *port) return 0; } -bool serial_port_is_open(serial_port *port) +bool serial_port_is_open(const serial_port *port) { #ifdef WIN32 return port->handle != INVALID_HANDLE_VALUE; diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index fd4c39e28..8f074afdd 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -48,7 +48,7 @@ bool serial_port_close(serial_port *port); bool serial_port_write(serial_port *port, const void *buffer, size_t num_bytes, size_t *bytes_written); bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wait_time, size_t *bytes_read); uint32_t serial_port_read_count(serial_port *port); -bool serial_port_is_open(serial_port *port); +bool serial_port_is_open(const serial_port *port); ///@} ///@} From ba0c8fe260e5d5e61104c41c4ba51c09fe0cddb2 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 14 Jun 2023 08:53:14 -0400 Subject: [PATCH 068/252] Generate MIP definitions from branches/cv7_ins@55037. --- src/mip/definitions/commands_aiding.c | 252 ++++++++++++++++++++++++ src/mip/definitions/commands_aiding.cpp | 224 +++++++++++++++++++++ src/mip/definitions/commands_aiding.h | 127 ++++++++++-- src/mip/definitions/commands_aiding.hpp | 181 +++++++++++++++-- 4 files changed, 752 insertions(+), 32 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 051d52480..54556eeab 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -328,6 +328,258 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* } return result; } +void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_aiding_ecef_vel_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_ecef_vel_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_ecef_vel_command(mip_serializer* serializer, mip_aiding_ecef_vel_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_ecef_vel_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_ecef_vel_response(mip_serializer* serializer, const mip_aiding_ecef_vel_response* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_ecef_vel_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_ecef_vel_response(mip_serializer* serializer, mip_aiding_ecef_vel_response* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_ecef_vel_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ecef_vel_command_valid_flags* valid_flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, velocity[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_ecef_vel_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECEF_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ECEF_VEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(time_out); + extract_mip_time(&deserializer, time_out); + + assert(sensor_id_out); + extract_u8(&deserializer, sensor_id_out); + + assert(velocity_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &velocity_out[i]); + + assert(uncertainty_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &uncertainty_out[i]); + + assert(valid_flags_out); + extract_mip_aiding_ecef_vel_command_valid_flags(&deserializer, valid_flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert_mip_aiding_ned_vel_command(mip_serializer* serializer, const mip_aiding_ned_vel_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_ned_vel_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_ned_vel_command(mip_serializer* serializer, mip_aiding_ned_vel_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_ned_vel_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_ned_vel_response(mip_serializer* serializer, const mip_aiding_ned_vel_response* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_ned_vel_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_ned_vel_response(mip_serializer* serializer, mip_aiding_ned_vel_response* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_ned_vel_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ned_vel_command_valid_flags* valid_flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, velocity[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_ned_vel_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_NED_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_NED_VEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(time_out); + extract_mip_time(&deserializer, time_out); + + assert(sensor_id_out); + extract_u8(&deserializer, sensor_id_out); + + assert(velocity_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &velocity_out[i]); + + assert(uncertainty_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &uncertainty_out[i]); + + assert(valid_flags_out); + extract_mip_aiding_ned_vel_command_valid_flags(&deserializer, valid_flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} void insert_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command* self) { insert_mip_time(serializer, &self->time); diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 5a94e5a04..9a15cf45f 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -291,6 +291,230 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d } return result; } +void insert(Serializer& serializer, const EcefVel& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, EcefVel& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +void insert(Serializer& serializer, const EcefVel::Response& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, EcefVel::Response& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, EcefVel::ValidFlags* validFlagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, velocity[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECEF_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECEF_VEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(timeOut); + extract(deserializer, *timeOut); + + assert(sensorIdOut); + extract(deserializer, *sensorIdOut); + + assert(velocityOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, velocityOut[i]); + + assert(uncertaintyOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, uncertaintyOut[i]); + + assert(validFlagsOut); + extract(deserializer, *validFlagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +void insert(Serializer& serializer, const NedVel& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, NedVel& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +void insert(Serializer& serializer, const NedVel::Response& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, NedVel::Response& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, NedVel::ValidFlags* validFlagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, velocity[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NED_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NED_VEL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(timeOut); + extract(deserializer, *timeOut); + + assert(sensorIdOut); + extract(deserializer, *sensorIdOut); + + assert(velocityOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, velocityOut[i]); + + assert(uncertaintyOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, uncertaintyOut[i]); + + assert(validFlagsOut); + extract(deserializer, *validFlagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) { insert(serializer, self.time); diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index 5973c182b..ac38558b1 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -49,6 +49,8 @@ enum MIP_REPLY_DESC_AIDING_ECEF_POS = 0x81, MIP_REPLY_DESC_AIDING_LLH_POS = 0x82, + MIP_REPLY_DESC_AIDING_ECEF_VEL = 0x88, + MIP_REPLY_DESC_AIDING_NED_VEL = 0x89, MIP_REPLY_DESC_AIDING_ODOM_VEL = 0x8A, MIP_REPLY_DESC_AIDING_HEADING_TRUE = 0x91, MIP_REPLY_DESC_AIDING_ECHO_CONTROL = 0xEF, @@ -83,6 +85,7 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_ecef_pos (0x13,0x01) Ecef Pos [C] +/// Cartesian vector position aiding command. Coordinates are given in the WGS84 ECEF system. /// ///@{ @@ -97,8 +100,8 @@ struct mip_aiding_ecef_pos_command { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Sensor ID. - double position[3]; ///< ECEF position. - float uncertainty[3]; ///< ECEF position uncertainty. + double position[3]; ///< ECEF position [m]. + float uncertainty[3]; ///< ECEF position uncertainty [m]. mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. }; @@ -113,8 +116,8 @@ struct mip_aiding_ecef_pos_response { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Sensor ID. - double position[3]; ///< ECEF position. - float uncertainty[3]; ///< ECEF position uncertainty. + double position[3]; ///< ECEF position [m]. + float uncertainty[3]; ///< ECEF position uncertainty [m]. mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. }; @@ -127,6 +130,8 @@ mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_llh_pos (0x13,0x02) Llh Pos [C] +/// Geodetic position aiding command. Coordinates are given in WGS84 geodetic latitude, longitude, and height above the ellipsoid. +/// Uncertainty is given in NED coordinates, which are parallel to incremental changes in latitude, longitude, and height. /// ///@{ @@ -141,10 +146,10 @@ struct mip_aiding_llh_pos_command { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Sensor ID. - double latitude; - double longitude; - double height; - float uncertainty[3]; ///< ECEF position uncertainty. + double latitude; ///< [deg] + double longitude; ///< [deg] + double height; ///< [m] + float uncertainty[3]; ///< NED position uncertainty. mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. }; @@ -159,10 +164,10 @@ struct mip_aiding_llh_pos_response { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Sensor ID. - double latitude; - double longitude; - double height; - float uncertainty[3]; ///< ECEF position uncertainty. + double latitude; ///< [deg] + double longitude; ///< [deg] + double height; ///< [m] + float uncertainty[3]; ///< NED position uncertainty. mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. }; @@ -174,6 +179,96 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_ecef_vel (0x13,0x08) Ecef Vel [C] +/// ECEF velocity aiding command. Coordinates are given in the WGS84 ECEF frame. +/// +///@{ + +typedef uint16_t mip_aiding_ecef_vel_command_valid_flags; +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_X = 0x0002; ///< +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Y = 0x0004; ///< +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Z = 0x0008; ///< +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_ALL = 0x000E; + +struct mip_aiding_ecef_vel_command +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + float velocity[3]; ///< ECEF velocity [m/s]. + float uncertainty[3]; ///< ECEF velocity uncertainty [m/s]. + mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_ecef_vel_command mip_aiding_ecef_vel_command; +void insert_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command* self); +void extract_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, mip_aiding_ecef_vel_command* self); + +void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self); +void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self); + +struct mip_aiding_ecef_vel_response +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + float velocity[3]; ///< ECEF velocity [m/s]. + float uncertainty[3]; ///< ECEF velocity uncertainty [m/s]. + mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_ecef_vel_response mip_aiding_ecef_vel_response; +void insert_mip_aiding_ecef_vel_response(struct mip_serializer* serializer, const mip_aiding_ecef_vel_response* self); +void extract_mip_aiding_ecef_vel_response(struct mip_serializer* serializer, mip_aiding_ecef_vel_response* self); + +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ecef_vel_command_valid_flags* valid_flags_out); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_ned_vel (0x13,0x09) Ned Vel [C] +/// NED velocity aiding command. Coordinates are given in the local North-East-Down frame. +/// +///@{ + +typedef uint16_t mip_aiding_ned_vel_command_valid_flags; +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_X = 0x0002; ///< +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Y = 0x0004; ///< +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Z = 0x0008; ///< +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_ALL = 0x000E; + +struct mip_aiding_ned_vel_command +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + float velocity[3]; ///< NED velocity [m/s]. + float uncertainty[3]; ///< NED velocity uncertainty [m/s]. + mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_ned_vel_command mip_aiding_ned_vel_command; +void insert_mip_aiding_ned_vel_command(struct mip_serializer* serializer, const mip_aiding_ned_vel_command* self); +void extract_mip_aiding_ned_vel_command(struct mip_serializer* serializer, mip_aiding_ned_vel_command* self); + +void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self); +void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self); + +struct mip_aiding_ned_vel_response +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Sensor ID. + float velocity[3]; ///< NED velocity [m/s]. + float uncertainty[3]; ///< NED velocity uncertainty [m/s]. + mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. + +}; +typedef struct mip_aiding_ned_vel_response mip_aiding_ned_vel_response; +void insert_mip_aiding_ned_vel_response(struct mip_serializer* serializer, const mip_aiding_ned_vel_response* self); +void extract_mip_aiding_ned_vel_response(struct mip_serializer* serializer, mip_aiding_ned_vel_response* self); + +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ned_vel_command_valid_flags* valid_flags_out); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_vehicle_fixed_frame_velocity (0x13,0x0A) Vehicle Fixed Frame Velocity [C] /// Estimate of velocity of the vehicle in the frame associated /// with the given sensor ID. @@ -191,8 +286,8 @@ struct mip_aiding_vehicle_fixed_frame_velocity_command { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3]; ///< [meters/second] - float uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + float velocity[3]; ///< [m/s] + float uncertainty[3]; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; }; @@ -207,8 +302,8 @@ struct mip_aiding_vehicle_fixed_frame_velocity_response { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3]; ///< [meters/second] - float uncertainty[3]; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + float velocity[3]; ///< [m/s] + float uncertainty[3]; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; }; diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index f533ae550..7fe893830 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -48,6 +48,8 @@ enum REPLY_ECEF_POS = 0x81, REPLY_LLH_POS = 0x82, + REPLY_ECEF_VEL = 0x88, + REPLY_NED_VEL = 0x89, REPLY_ODOM_VEL = 0x8A, REPLY_HEADING_TRUE = 0x91, REPLY_ECHO_CONTROL = 0xEF, @@ -80,6 +82,7 @@ void extract(Serializer& serializer, Time& self); //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_ecef_pos (0x13,0x01) Ecef Pos [CPP] +/// Cartesian vector position aiding command. Coordinates are given in the WGS84 ECEF system. /// ///@{ @@ -123,8 +126,8 @@ struct EcefPos Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Sensor ID. - double position[3] = {0}; ///< ECEF position. - float uncertainty[3] = {0}; ///< ECEF position uncertainty. + double position[3] = {0}; ///< ECEF position [m]. + float uncertainty[3] = {0}; ///< ECEF position uncertainty [m]. ValidFlags valid_flags; ///< Valid flags. struct Response @@ -134,8 +137,8 @@ struct EcefPos Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Sensor ID. - double position[3] = {0}; ///< ECEF position. - float uncertainty[3] = {0}; ///< ECEF position uncertainty. + double position[3] = {0}; ///< ECEF position [m]. + float uncertainty[3] = {0}; ///< ECEF position uncertainty [m]. ValidFlags valid_flags; ///< Valid flags. }; @@ -151,6 +154,8 @@ CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_llh_pos (0x13,0x02) Llh Pos [CPP] +/// Geodetic position aiding command. Coordinates are given in WGS84 geodetic latitude, longitude, and height above the ellipsoid. +/// Uncertainty is given in NED coordinates, which are parallel to incremental changes in latitude, longitude, and height. /// ///@{ @@ -194,10 +199,10 @@ struct LlhPos Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Sensor ID. - double latitude = 0; - double longitude = 0; - double height = 0; - float uncertainty[3] = {0}; ///< ECEF position uncertainty. + double latitude = 0; ///< [deg] + double longitude = 0; ///< [deg] + double height = 0; ///< [m] + float uncertainty[3] = {0}; ///< NED position uncertainty. ValidFlags valid_flags; ///< Valid flags. struct Response @@ -207,10 +212,10 @@ struct LlhPos Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Sensor ID. - double latitude = 0; - double longitude = 0; - double height = 0; - float uncertainty[3] = {0}; ///< ECEF position uncertainty. + double latitude = 0; ///< [deg] + double longitude = 0; ///< [deg] + double height = 0; ///< [m] + float uncertainty[3] = {0}; ///< NED position uncertainty. ValidFlags valid_flags; ///< Valid flags. }; @@ -225,6 +230,150 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_ecef_vel (0x13,0x08) Ecef Vel [CPP] +/// ECEF velocity aiding command. Coordinates are given in the WGS84 ECEF frame. +/// +///@{ + +struct EcefVel +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECEF_VEL; + + static const bool HAS_FUNCTION_SELECTOR = false; + + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + X = 0x0002, ///< + Y = 0x0004, ///< + Z = 0x0008, ///< + ALL = 0x000E, + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool x() const { return (value & X) > 0; } + void x(bool val) { if(val) value |= X; else value &= ~X; } + bool y() const { return (value & Y) > 0; } + void y(bool val) { if(val) value |= Y; else value &= ~Y; } + bool z() const { return (value & Z) > 0; } + void z(bool val) { if(val) value |= Z; else value &= ~Z; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + float velocity[3] = {0}; ///< ECEF velocity [m/s]. + float uncertainty[3] = {0}; ///< ECEF velocity uncertainty [m/s]. + ValidFlags valid_flags; ///< Valid flags. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECEF_VEL; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + float velocity[3] = {0}; ///< ECEF velocity [m/s]. + float uncertainty[3] = {0}; ///< ECEF velocity uncertainty [m/s]. + ValidFlags valid_flags; ///< Valid flags. + + }; +}; +void insert(Serializer& serializer, const EcefVel& self); +void extract(Serializer& serializer, EcefVel& self); + +void insert(Serializer& serializer, const EcefVel::Response& self); +void extract(Serializer& serializer, EcefVel::Response& self); + +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, EcefVel::ValidFlags* validFlagsOut); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_ned_vel (0x13,0x09) Ned Vel [CPP] +/// NED velocity aiding command. Coordinates are given in the local North-East-Down frame. +/// +///@{ + +struct NedVel +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_NED_VEL; + + static const bool HAS_FUNCTION_SELECTOR = false; + + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + X = 0x0002, ///< + Y = 0x0004, ///< + Z = 0x0008, ///< + ALL = 0x000E, + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool x() const { return (value & X) > 0; } + void x(bool val) { if(val) value |= X; else value &= ~X; } + bool y() const { return (value & Y) > 0; } + void y(bool val) { if(val) value |= Y; else value &= ~Y; } + bool z() const { return (value & Z) > 0; } + void z(bool val) { if(val) value |= Z; else value &= ~Z; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + float velocity[3] = {0}; ///< NED velocity [m/s]. + float uncertainty[3] = {0}; ///< NED velocity uncertainty [m/s]. + ValidFlags valid_flags; ///< Valid flags. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_NED_VEL; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Sensor ID. + float velocity[3] = {0}; ///< NED velocity [m/s]. + float uncertainty[3] = {0}; ///< NED velocity uncertainty [m/s]. + ValidFlags valid_flags; ///< Valid flags. + + }; +}; +void insert(Serializer& serializer, const NedVel& self); +void extract(Serializer& serializer, NedVel& self); + +void insert(Serializer& serializer, const NedVel::Response& self); +void extract(Serializer& serializer, NedVel::Response& self); + +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, NedVel::ValidFlags* validFlagsOut); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_vehicle_fixed_frame_velocity (0x13,0x0A) Vehicle Fixed Frame Velocity [CPP] /// Estimate of velocity of the vehicle in the frame associated /// with the given sensor ID. @@ -271,8 +420,8 @@ struct VehicleFixedFrameVelocity Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3] = {0}; ///< [meters/second] - float uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + float velocity[3] = {0}; ///< [m/s] + float uncertainty[3] = {0}; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; struct Response @@ -282,8 +431,8 @@ struct VehicleFixedFrameVelocity Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3] = {0}; ///< [meters/second] - float uncertainty[3] = {0}; ///< [meters/second] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + float velocity[3] = {0}; ///< [m/s] + float uncertainty[3] = {0}; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; }; From cdef1b2de1afe5a5591fe04b3aba6e641831c1c3 Mon Sep 17 00:00:00 2001 From: Rob Fisher Date: Wed, 14 Jun 2023 21:04:01 +0000 Subject: [PATCH 069/252] Adds aiding files to CMakeLists.txt --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 45af8210e..8a500aa44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -148,6 +148,8 @@ set(MIPDEF_SOURCES "${MIP_DIR}/definitions/commands_gnss.h" "${MIP_DIR}/definitions/commands_rtk.c" "${MIP_DIR}/definitions/commands_rtk.h" + "${MIP_DIR}/definitions/commands_aiding.c" + "${MIP_DIR}/definitions/commands_aiding.h" "${MIP_DIR}/definitions/commands_system.c" "${MIP_DIR}/definitions/commands_system.h" "${MIP_DIR}/definitions/data_filter.c" From 08856cd0817dcedc4ea92af58d819edfdc525fee Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 15 Jun 2023 14:28:09 -0400 Subject: [PATCH 070/252] Generate MIP definitions from branches/dev@55052. --- src/mip/definitions/commands_3dm.c | 333 +++++ src/mip/definitions/commands_3dm.cpp | 270 ++++ src/mip/definitions/commands_3dm.h | 137 ++ src/mip/definitions/commands_3dm.hpp | 171 +++ src/mip/definitions/commands_filter.c | 1733 ++++++++++++++++------- src/mip/definitions/commands_filter.cpp | 1610 ++++++++++++++------- src/mip/definitions/commands_filter.h | 557 ++++++-- src/mip/definitions/commands_filter.hpp | 667 ++++++--- 8 files changed, 4163 insertions(+), 1315 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index 2e39a0fd1..f05a25b03 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -1411,6 +1411,207 @@ mip_cmd_result mip_3dm_default_datastream_control(struct mip_interface* device, return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_3dm_constellation_settings_command(mip_serializer* serializer, const mip_3dm_constellation_settings_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u16(serializer, self->max_channels); + + insert_u8(serializer, self->config_count); + + + for(unsigned int i=0; i < self->config_count; i++) + insert_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + + } +} +void extract_mip_3dm_constellation_settings_command(mip_serializer* serializer, mip_3dm_constellation_settings_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u16(serializer, &self->max_channels); + + assert(self->config_count); + extract_count(serializer, &self->config_count, self->config_count); + + for(unsigned int i=0; i < self->config_count; i++) + extract_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + + } +} + +void insert_mip_3dm_constellation_settings_response(mip_serializer* serializer, const mip_3dm_constellation_settings_response* self) +{ + insert_u16(serializer, self->max_channels_available); + + insert_u16(serializer, self->max_channels_use); + + insert_u8(serializer, self->config_count); + + + for(unsigned int i=0; i < self->config_count; i++) + insert_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + +} +void extract_mip_3dm_constellation_settings_response(mip_serializer* serializer, mip_3dm_constellation_settings_response* self) +{ + extract_u16(serializer, &self->max_channels_available); + + extract_u16(serializer, &self->max_channels_use); + + assert(self->config_count); + extract_count(serializer, &self->config_count, self->config_count); + + for(unsigned int i=0; i < self->config_count; i++) + extract_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + +} + +void insert_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_constellation_id self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_constellation_id* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +void insert_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_option_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_option_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +void insert_mip_3dm_constellation_settings_command_settings(mip_serializer* serializer, const mip_3dm_constellation_settings_command_settings* self) +{ + insert_mip_3dm_constellation_settings_command_constellation_id(serializer, self->constellation_id); + + insert_u8(serializer, self->enable); + + insert_u8(serializer, self->reserved_channels); + + insert_u8(serializer, self->max_channels); + + insert_mip_3dm_constellation_settings_command_option_flags(serializer, self->option_flags); + +} +void extract_mip_3dm_constellation_settings_command_settings(mip_serializer* serializer, mip_3dm_constellation_settings_command_settings* self) +{ + extract_mip_3dm_constellation_settings_command_constellation_id(serializer, &self->constellation_id); + + extract_u8(serializer, &self->enable); + + extract_u8(serializer, &self->reserved_channels); + + extract_u8(serializer, &self->max_channels); + + extract_mip_3dm_constellation_settings_command_option_flags(serializer, &self->option_flags); + +} + +mip_cmd_result mip_3dm_write_constellation_settings(struct mip_interface* device, uint16_t max_channels, uint8_t config_count, const mip_3dm_constellation_settings_command_settings* settings) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u16(&serializer, max_channels); + + insert_u8(&serializer, config_count); + + assert(settings || (config_count == 0)); + for(unsigned int i=0; i < config_count; i++) + insert_mip_3dm_constellation_settings_command_settings(&serializer, &settings[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_read_constellation_settings(struct mip_interface* device, uint16_t* max_channels_available_out, uint16_t* max_channels_use_out, uint8_t* config_count_out, uint8_t config_count_out_max, mip_3dm_constellation_settings_command_settings* settings_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(max_channels_available_out); + extract_u16(&deserializer, max_channels_available_out); + + assert(max_channels_use_out); + extract_u16(&deserializer, max_channels_use_out); + + assert(config_count_out); + extract_count(&deserializer, config_count_out, config_count_out_max); + + assert(settings_out || (config_count_out == 0)); + for(unsigned int i=0; i < *config_count_out; i++) + extract_mip_3dm_constellation_settings_command_settings(&deserializer, &settings_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_3dm_save_constellation_settings(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_load_constellation_settings(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_default_constellation_settings(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_3dm_gnss_sbas_settings_command(mip_serializer* serializer, const mip_3dm_gnss_sbas_settings_command* self) { insert_mip_function_selector(serializer, self->function); @@ -1580,6 +1781,138 @@ mip_cmd_result mip_3dm_default_gnss_sbas_settings(struct mip_interface* device) return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_3dm_gnss_assisted_fix_command(mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, self->option); + + insert_u8(serializer, self->flags); + + } +} +void extract_mip_3dm_gnss_assisted_fix_command(mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, &self->option); + + extract_u8(serializer, &self->flags); + + } +} + +void insert_mip_3dm_gnss_assisted_fix_response(mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_response* self) +{ + insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, self->option); + + insert_u8(serializer, self->flags); + +} +void extract_mip_3dm_gnss_assisted_fix_response(mip_serializer* serializer, mip_3dm_gnss_assisted_fix_response* self) +{ + extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, &self->option); + + extract_u8(serializer, &self->flags); + +} + +void insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command_assisted_fix_option self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_3dm_write_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option option, uint8_t flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(&serializer, option); + + insert_u8(&serializer, flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_read_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* option_out, uint8_t* flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(option_out); + extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(&deserializer, option_out); + + assert(flags_out); + extract_u8(&deserializer, flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_3dm_save_gnss_assisted_fix(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_load_gnss_assisted_fix(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_default_gnss_assisted_fix(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_3dm_gnss_time_assistance_command(mip_serializer* serializer, const mip_3dm_gnss_time_assistance_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index c727face8..6c066ad6b 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -1286,6 +1286,166 @@ CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const ConstellationSettings& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.max_channels); + + insert(serializer, self.config_count); + + for(unsigned int i=0; i < self.config_count; i++) + insert(serializer, self.settings[i]); + + } +} +void extract(Serializer& serializer, ConstellationSettings& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.max_channels); + + C::extract_count(&serializer, &self.config_count, self.config_count); + for(unsigned int i=0; i < self.config_count; i++) + extract(serializer, self.settings[i]); + + } +} + +void insert(Serializer& serializer, const ConstellationSettings::Response& self) +{ + insert(serializer, self.max_channels_available); + + insert(serializer, self.max_channels_use); + + insert(serializer, self.config_count); + + for(unsigned int i=0; i < self.config_count; i++) + insert(serializer, self.settings[i]); + +} +void extract(Serializer& serializer, ConstellationSettings::Response& self) +{ + extract(serializer, self.max_channels_available); + + extract(serializer, self.max_channels_use); + + C::extract_count(&serializer, &self.config_count, self.config_count); + for(unsigned int i=0; i < self.config_count; i++) + extract(serializer, self.settings[i]); + +} + +void insert(Serializer& serializer, const ConstellationSettings::Settings& self) +{ + insert(serializer, self.constellation_id); + + insert(serializer, self.enable); + + insert(serializer, self.reserved_channels); + + insert(serializer, self.max_channels); + + insert(serializer, self.option_flags); + +} +void extract(Serializer& serializer, ConstellationSettings::Settings& self) +{ + extract(serializer, self.constellation_id); + + extract(serializer, self.enable); + + extract(serializer, self.reserved_channels); + + extract(serializer, self.max_channels); + + extract(serializer, self.option_flags); + +} + +CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, maxChannels); + + insert(serializer, configCount); + + assert(settings || (configCount == 0)); + for(unsigned int i=0; i < configCount; i++) + insert(serializer, settings[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(maxChannelsAvailableOut); + extract(deserializer, *maxChannelsAvailableOut); + + assert(maxChannelsUseOut); + extract(deserializer, *maxChannelsUseOut); + + C::extract_count(&deserializer, configCountOut, configCountOutMax); + assert(settingsOut || (configCountOut == 0)); + for(unsigned int i=0; i < *configCountOut; i++) + extract(deserializer, settingsOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveConstellationSettings(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadConstellationSettings(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultConstellationSettings(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const GnssSbasSettings& self) { insert(serializer, self.function); @@ -1425,6 +1585,116 @@ CmdResult defaultGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const GnssAssistedFix& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.option); + + insert(serializer, self.flags); + + } +} +void extract(Serializer& serializer, GnssAssistedFix& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.option); + + extract(serializer, self.flags); + + } +} + +void insert(Serializer& serializer, const GnssAssistedFix::Response& self) +{ + insert(serializer, self.option); + + insert(serializer, self.flags); + +} +void extract(Serializer& serializer, GnssAssistedFix::Response& self) +{ + extract(serializer, self.option); + + extract(serializer, self.flags); + +} + +CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, option); + + insert(serializer, flags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(optionOut); + extract(deserializer, *optionOut); + + assert(flagsOut); + extract(deserializer, *flagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGnssAssistedFix(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGnssAssistedFix(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGnssAssistedFix(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const GnssTimeAssistance& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 45fe8ec8a..b0de22517 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -745,6 +745,92 @@ mip_cmd_result mip_3dm_default_datastream_control(struct mip_interface* device, ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_3dm_constellation_settings (0x0C,0x21) Constellation Settings [C] +/// This command configures which satellite constellations are enabled and how many channels are dedicated to tracking each constellation. +/// +/// Maximum number of tracking channels to use (total for all constellations): +/// 0 to max_channels_available (from reply message) +/// +/// For each constellation you wish to use, include a ConstellationSettings struct. Note the following: +/// +/// Total number of tracking channels (sum of "reserved_channels" for all constellations) must be <= 32: +/// 0 -> 32 Number of reserved channels +/// 0 -> 32 Max number of channels (>= reserved channels) +/// +/// The factory default setting is: GPS and GLONASS enabled. Min/Max for GPS = 8/16, GLONASS = 8/14, SBAS = 1/3, QZSS = 0/3. +/// +/// Warning: SBAS functionality shall not be used in "safety of life" applications! +/// Warning: Any setting that causes the total reserved channels to exceed 32 will result in a NACK. +/// Warning: You cannot enable GLONASS and BeiDou at the same time. +/// Note: Enabling SBAS and QZSS augments GPS accuracy. +/// Note: It is recommended to disable GLONASS and BeiDou if a GPS-only antenna or GPS-only SAW filter is used. +/// +///@{ + +typedef uint8_t mip_3dm_constellation_settings_command_constellation_id; +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_GPS = 0; ///< GPS (G1-G32) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_SBAS = 1; ///< SBAS (S120-S158) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_GALILEO = 2; ///< GALILEO (E1-E36) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_BEIDOU = 3; ///< BeiDou (B1-B37) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_QZSS = 5; ///< QZSS (Q1-Q5) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_GLONASS = 6; ///< GLONASS (R1-R32) + +typedef uint16_t mip_3dm_constellation_settings_command_option_flags; +static const mip_3dm_constellation_settings_command_option_flags MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_OPTION_FLAGS_NONE = 0x0000; +static const mip_3dm_constellation_settings_command_option_flags MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_OPTION_FLAGS_L1SAIF = 0x0001; ///< Available only for QZSS +static const mip_3dm_constellation_settings_command_option_flags MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_OPTION_FLAGS_ALL = 0x0001; + +struct mip_3dm_constellation_settings_command_settings +{ + mip_3dm_constellation_settings_command_constellation_id constellation_id; ///< Constellation ID + uint8_t enable; ///< Enable/Disable constellation + uint8_t reserved_channels; ///< Minimum number of channels reserved for this constellation + uint8_t max_channels; ///< Maximum number of channels to use for this constallation + mip_3dm_constellation_settings_command_option_flags option_flags; ///< Constellation option Flags + +}; +typedef struct mip_3dm_constellation_settings_command_settings mip_3dm_constellation_settings_command_settings; +struct mip_3dm_constellation_settings_command +{ + mip_function_selector function; + uint16_t max_channels; + uint8_t config_count; + mip_3dm_constellation_settings_command_settings* settings; + +}; +typedef struct mip_3dm_constellation_settings_command mip_3dm_constellation_settings_command; +void insert_mip_3dm_constellation_settings_command(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command* self); +void extract_mip_3dm_constellation_settings_command(struct mip_serializer* serializer, mip_3dm_constellation_settings_command* self); + +void insert_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_constellation_id self); +void extract_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_constellation_id* self); + +void insert_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_option_flags self); +void extract_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_option_flags* self); + +void insert_mip_3dm_constellation_settings_command_settings(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_settings* self); +void extract_mip_3dm_constellation_settings_command_settings(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_settings* self); + +struct mip_3dm_constellation_settings_response +{ + uint16_t max_channels_available; ///< Maximum channels available + uint16_t max_channels_use; ///< Maximum channels to use + uint8_t config_count; ///< Number of constellation configurations + mip_3dm_constellation_settings_command_settings* settings; ///< Constellation Settings + +}; +typedef struct mip_3dm_constellation_settings_response mip_3dm_constellation_settings_response; +void insert_mip_3dm_constellation_settings_response(struct mip_serializer* serializer, const mip_3dm_constellation_settings_response* self); +void extract_mip_3dm_constellation_settings_response(struct mip_serializer* serializer, mip_3dm_constellation_settings_response* self); + +mip_cmd_result mip_3dm_write_constellation_settings(struct mip_interface* device, uint16_t max_channels, uint8_t config_count, const mip_3dm_constellation_settings_command_settings* settings); +mip_cmd_result mip_3dm_read_constellation_settings(struct mip_interface* device, uint16_t* max_channels_available_out, uint16_t* max_channels_use_out, uint8_t* config_count_out, uint8_t config_count_out_max, mip_3dm_constellation_settings_command_settings* settings_out); +mip_cmd_result mip_3dm_save_constellation_settings(struct mip_interface* device); +mip_cmd_result mip_3dm_load_constellation_settings(struct mip_interface* device); +mip_cmd_result mip_3dm_default_constellation_settings(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_3dm_gnss_sbas_settings (0x0C,0x22) Gnss Sbas Settings [C] /// Configure the SBAS subsystem /// @@ -796,6 +882,57 @@ mip_cmd_result mip_3dm_default_gnss_sbas_settings(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_3dm_gnss_assisted_fix (0x0C,0x23) Gnss Assisted Fix [C] +/// Set the options for assisted GNSS fix. +/// +/// Devices that implement this command have a dedicated GNSS flash memory and a non-volatile FRAM. +/// These storage mechanisms are used to retain information about the last good GNSS fix. This can greatly reduces the TTFF (Time To First Fix) depending on the age of the stored information. +/// The TTFF can be as low as one second, or up to the equivalent of a cold start. There is a small increase in power used when enabling assisted fix. +/// +/// The fastest fix will be obtained by supplying the device with a GNSS Assist Time Update message containing the current GPS time immediately after subsequent power up. +/// This allows the device to determine if the last GNSS information saved is still fresh enough to improve the TTFF. +/// +/// NOTE: Non-volatile GNSS memory is cleared when going from an enabled state to a disabled state. +/// WARNING: The clearing operation results in an erase operation on the GNSS Flash. The flash has a limited durability of 100,000 write/erase cycles +/// +///@{ + +typedef uint8_t mip_3dm_gnss_assisted_fix_command_assisted_fix_option; +static const mip_3dm_gnss_assisted_fix_command_assisted_fix_option MIP_3DM_GNSS_ASSISTED_FIX_COMMAND_ASSISTED_FIX_OPTION_NONE = 0; ///< No assisted fix (default) +static const mip_3dm_gnss_assisted_fix_command_assisted_fix_option MIP_3DM_GNSS_ASSISTED_FIX_COMMAND_ASSISTED_FIX_OPTION_ENABLED = 1; ///< Enable assisted fix + +struct mip_3dm_gnss_assisted_fix_command +{ + mip_function_selector function; + mip_3dm_gnss_assisted_fix_command_assisted_fix_option option; ///< Assisted fix options + uint8_t flags; ///< Assisted fix flags (set to 0xFF) + +}; +typedef struct mip_3dm_gnss_assisted_fix_command mip_3dm_gnss_assisted_fix_command; +void insert_mip_3dm_gnss_assisted_fix_command(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command* self); +void extract_mip_3dm_gnss_assisted_fix_command(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command* self); + +void insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command_assisted_fix_option self); +void extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* self); + +struct mip_3dm_gnss_assisted_fix_response +{ + mip_3dm_gnss_assisted_fix_command_assisted_fix_option option; ///< Assisted fix options + uint8_t flags; ///< Assisted fix flags (set to 0xFF) + +}; +typedef struct mip_3dm_gnss_assisted_fix_response mip_3dm_gnss_assisted_fix_response; +void insert_mip_3dm_gnss_assisted_fix_response(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_response* self); +void extract_mip_3dm_gnss_assisted_fix_response(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_response* self); + +mip_cmd_result mip_3dm_write_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option option, uint8_t flags); +mip_cmd_result mip_3dm_read_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* option_out, uint8_t* flags_out); +mip_cmd_result mip_3dm_save_gnss_assisted_fix(struct mip_interface* device); +mip_cmd_result mip_3dm_load_gnss_assisted_fix(struct mip_interface* device); +mip_cmd_result mip_3dm_default_gnss_assisted_fix(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_3dm_gnss_time_assistance (0x0C,0x24) Gnss Time Assistance [C] /// Provide the GNSS subsystem with initial time information. /// diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 845637b38..93a678f02 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -889,6 +889,117 @@ CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_3dm_constellation_settings (0x0C,0x21) Constellation Settings [CPP] +/// This command configures which satellite constellations are enabled and how many channels are dedicated to tracking each constellation. +/// +/// Maximum number of tracking channels to use (total for all constellations): +/// 0 to max_channels_available (from reply message) +/// +/// For each constellation you wish to use, include a ConstellationSettings struct. Note the following: +/// +/// Total number of tracking channels (sum of "reserved_channels" for all constellations) must be <= 32: +/// 0 -> 32 Number of reserved channels +/// 0 -> 32 Max number of channels (>= reserved channels) +/// +/// The factory default setting is: GPS and GLONASS enabled. Min/Max for GPS = 8/16, GLONASS = 8/14, SBAS = 1/3, QZSS = 0/3. +/// +/// Warning: SBAS functionality shall not be used in "safety of life" applications! +/// Warning: Any setting that causes the total reserved channels to exceed 32 will result in a NACK. +/// Warning: You cannot enable GLONASS and BeiDou at the same time. +/// Note: Enabling SBAS and QZSS augments GPS accuracy. +/// Note: It is recommended to disable GLONASS and BeiDou if a GPS-only antenna or GPS-only SAW filter is used. +/// +///@{ + +struct ConstellationSettings +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class ConstellationId : uint8_t + { + GPS = 0, ///< GPS (G1-G32) + SBAS = 1, ///< SBAS (S120-S158) + GALILEO = 2, ///< GALILEO (E1-E36) + BEIDOU = 3, ///< BeiDou (B1-B37) + QZSS = 5, ///< QZSS (Q1-Q5) + GLONASS = 6, ///< GLONASS (R1-R32) + }; + + struct OptionFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + L1SAIF = 0x0001, ///< Available only for QZSS + ALL = 0x0001, + }; + uint16_t value = NONE; + + OptionFlags() : value(NONE) {} + OptionFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + OptionFlags& operator=(uint16_t val) { value = val; return *this; } + OptionFlags& operator=(int val) { value = val; return *this; } + OptionFlags& operator|=(uint16_t val) { return *this = value | val; } + OptionFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool l1saif() const { return (value & L1SAIF) > 0; } + void l1saif(bool val) { if(val) value |= L1SAIF; else value &= ~L1SAIF; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + struct Settings + { + ConstellationId constellation_id = static_cast(0); ///< Constellation ID + uint8_t enable = 0; ///< Enable/Disable constellation + uint8_t reserved_channels = 0; ///< Minimum number of channels reserved for this constellation + uint8_t max_channels = 0; ///< Maximum number of channels to use for this constallation + OptionFlags option_flags; ///< Constellation option Flags + + }; + FunctionSelector function = static_cast(0); + uint16_t max_channels = 0; + uint8_t config_count = 0; + Settings* settings = {nullptr}; + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; + + uint16_t max_channels_available = 0; ///< Maximum channels available + uint16_t max_channels_use = 0; ///< Maximum channels to use + uint8_t config_count = 0; ///< Number of constellation configurations + Settings* settings = {nullptr}; ///< Constellation Settings + + }; +}; +void insert(Serializer& serializer, const ConstellationSettings& self); +void extract(Serializer& serializer, ConstellationSettings& self); + +void insert(Serializer& serializer, const ConstellationSettings::Settings& self); +void extract(Serializer& serializer, ConstellationSettings::Settings& self); + +void insert(Serializer& serializer, const ConstellationSettings::Response& self); +void extract(Serializer& serializer, ConstellationSettings::Response& self); + +CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); +CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); +CmdResult saveConstellationSettings(C::mip_interface& device); +CmdResult loadConstellationSettings(C::mip_interface& device); +CmdResult defaultConstellationSettings(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_3dm_gnss_sbas_settings (0x0C,0x22) Gnss Sbas Settings [CPP] /// Configure the SBAS subsystem /// @@ -971,6 +1082,66 @@ CmdResult defaultGnssSbasSettings(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_3dm_gnss_assisted_fix (0x0C,0x23) Gnss Assisted Fix [CPP] +/// Set the options for assisted GNSS fix. +/// +/// Devices that implement this command have a dedicated GNSS flash memory and a non-volatile FRAM. +/// These storage mechanisms are used to retain information about the last good GNSS fix. This can greatly reduces the TTFF (Time To First Fix) depending on the age of the stored information. +/// The TTFF can be as low as one second, or up to the equivalent of a cold start. There is a small increase in power used when enabling assisted fix. +/// +/// The fastest fix will be obtained by supplying the device with a GNSS Assist Time Update message containing the current GPS time immediately after subsequent power up. +/// This allows the device to determine if the last GNSS information saved is still fresh enough to improve the TTFF. +/// +/// NOTE: Non-volatile GNSS memory is cleared when going from an enabled state to a disabled state. +/// WARNING: The clearing operation results in an erase operation on the GNSS Flash. The flash has a limited durability of 100,000 write/erase cycles +/// +///@{ + +struct GnssAssistedFix +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class AssistedFixOption : uint8_t + { + NONE = 0, ///< No assisted fix (default) + ENABLED = 1, ///< Enable assisted fix + }; + + FunctionSelector function = static_cast(0); + AssistedFixOption option = static_cast(0); ///< Assisted fix options + uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; + + AssistedFixOption option = static_cast(0); ///< Assisted fix options + uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) + + }; +}; +void insert(Serializer& serializer, const GnssAssistedFix& self); +void extract(Serializer& serializer, GnssAssistedFix& self); + +void insert(Serializer& serializer, const GnssAssistedFix::Response& self); +void extract(Serializer& serializer, GnssAssistedFix::Response& self); + +CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); +CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); +CmdResult saveGnssAssistedFix(C::mip_interface& device); +CmdResult loadGnssAssistedFix(C::mip_interface& device); +CmdResult defaultGnssAssistedFix(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_3dm_gnss_time_assistance (0x0C,0x24) Gnss Time Assistance [CPP] /// Provide the GNSS subsystem with initial time information. /// diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index ccbca3b69..d71700893 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -33,11 +33,11 @@ void extract_mip_filter_reference_frame(struct mip_serializer* serializer, mip_f *self = tmp; } -void insert_mip_filter_mag_declination_source(struct mip_serializer* serializer, const mip_filter_mag_declination_source self) +void insert_mip_filter_mag_param_source(struct mip_serializer* serializer, const mip_filter_mag_param_source self) { insert_u8(serializer, (uint8_t)(self)); } -void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer, mip_filter_mag_declination_source* self) +void extract_mip_filter_mag_param_source(struct mip_serializer* serializer, mip_filter_mag_param_source* self) { uint8_t tmp = 0; extract_u8(serializer, &tmp); @@ -1561,11 +1561,8 @@ void insert_mip_filter_accel_noise_command(mip_serializer* serializer, const mip if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -1575,35 +1572,26 @@ void extract_mip_filter_accel_noise_command(mip_serializer* serializer, mip_filt if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_accel_noise_response(mip_serializer* serializer, const mip_filter_accel_noise_response* self) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_accel_noise_response(mip_serializer* serializer, mip_filter_accel_noise_response* self) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z) +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1611,17 +1599,15 @@ mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x); - - insert_float(&serializer, y); - - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1639,14 +1625,9 @@ mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); - - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1695,11 +1676,8 @@ void insert_mip_filter_gyro_noise_command(mip_serializer* serializer, const mip_ if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -1709,35 +1687,26 @@ void extract_mip_filter_gyro_noise_command(mip_serializer* serializer, mip_filte if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_gyro_noise_response(mip_serializer* serializer, const mip_filter_gyro_noise_response* self) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_gyro_noise_response(mip_serializer* serializer, mip_filter_gyro_noise_response* self) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z) +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1745,17 +1714,15 @@ mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x); - - insert_float(&serializer, y); - - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1773,14 +1740,9 @@ mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); - - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1829,17 +1791,11 @@ void insert_mip_filter_accel_bias_model_command(mip_serializer* serializer, cons if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -1849,53 +1805,35 @@ void extract_mip_filter_accel_bias_model_command(mip_serializer* serializer, mip if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_accel_bias_model_response(mip_serializer* serializer, const mip_filter_accel_bias_model_response* self) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_accel_bias_model_response(mip_serializer* serializer, mip_filter_accel_bias_model_response* self) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1903,23 +1841,19 @@ mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, f insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x_beta); - - insert_float(&serializer, y_beta); - - insert_float(&serializer, z_beta); - - insert_float(&serializer, x); - - insert_float(&serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, beta[i]); - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1937,23 +1871,13 @@ mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, fl mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_beta_out); - extract_float(&deserializer, x_beta_out); - - assert(y_beta_out); - extract_float(&deserializer, y_beta_out); - - assert(z_beta_out); - extract_float(&deserializer, z_beta_out); - - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); + assert(beta_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &beta_out[i]); - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2002,17 +1926,11 @@ void insert_mip_filter_gyro_bias_model_command(mip_serializer* serializer, const if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -2022,53 +1940,35 @@ void extract_mip_filter_gyro_bias_model_command(mip_serializer* serializer, mip_ if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_gyro_bias_model_response(mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_gyro_bias_model_response(mip_serializer* serializer, mip_filter_gyro_bias_model_response* self) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2076,23 +1976,19 @@ mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, fl insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x_beta); - - insert_float(&serializer, y_beta); - - insert_float(&serializer, z_beta); - - insert_float(&serializer, x); - - insert_float(&serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, beta[i]); - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2110,23 +2006,13 @@ mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, flo mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_beta_out); - extract_float(&deserializer, x_beta_out); - - assert(y_beta_out); - extract_float(&deserializer, y_beta_out); - - assert(z_beta_out); - extract_float(&deserializer, z_beta_out); - - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); + assert(beta_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &beta_out[i]); - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2175,7 +2061,7 @@ void insert_mip_filter_altitude_aiding_command(mip_serializer* serializer, const if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->aiding_selector); + insert_mip_filter_altitude_aiding_command_aiding_selector(serializer, self->selector); } } @@ -2185,23 +2071,34 @@ void extract_mip_filter_altitude_aiding_command(mip_serializer* serializer, mip_ if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->aiding_selector); + extract_mip_filter_altitude_aiding_command_aiding_selector(serializer, &self->selector); } } void insert_mip_filter_altitude_aiding_response(mip_serializer* serializer, const mip_filter_altitude_aiding_response* self) { - insert_u8(serializer, self->aiding_selector); + insert_mip_filter_altitude_aiding_command_aiding_selector(serializer, self->selector); } void extract_mip_filter_altitude_aiding_response(mip_serializer* serializer, mip_filter_altitude_aiding_response* self) { - extract_u8(serializer, &self->aiding_selector); + extract_mip_filter_altitude_aiding_command_aiding_selector(serializer, &self->selector); } -mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector) +void insert_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, const mip_filter_altitude_aiding_command_aiding_selector self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, mip_filter_altitude_aiding_command_aiding_selector* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector selector) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2209,13 +2106,13 @@ mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, ui insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, aiding_selector); + insert_mip_filter_altitude_aiding_command_aiding_selector(&serializer, selector); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out) +mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector* selector_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2233,8 +2130,8 @@ mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uin mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(aiding_selector_out); - extract_u8(&deserializer, aiding_selector_out); + assert(selector_out); + extract_mip_filter_altitude_aiding_command_aiding_selector(&deserializer, selector_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2277,6 +2174,125 @@ mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_filter_pitch_roll_aiding_command(mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, self->source); + + } +} +void extract_mip_filter_pitch_roll_aiding_command(mip_serializer* serializer, mip_filter_pitch_roll_aiding_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, &self->source); + + } +} + +void insert_mip_filter_pitch_roll_aiding_response(mip_serializer* serializer, const mip_filter_pitch_roll_aiding_response* self) +{ + insert_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, self->source); + +} +void extract_mip_filter_pitch_roll_aiding_response(mip_serializer* serializer, mip_filter_pitch_roll_aiding_response* self) +{ + extract_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, &self->source); + +} + +void insert_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command_aiding_source self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_command_aiding_source* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_filter_write_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source source) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_pitch_roll_aiding_command_aiding_source(&serializer, source); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source* source_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_pitch_roll_aiding_command_aiding_source(&deserializer, source_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_pitch_roll_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_pitch_roll_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_pitch_roll_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_filter_auto_zupt_command(mip_serializer* serializer, const mip_filter_auto_zupt_command* self) { insert_mip_function_selector(serializer, self->function); @@ -2418,27 +2434,1001 @@ void extract_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, mi { extract_u8(serializer, &self->enable); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->threshold); + + } +} + +void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +{ + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + +} +void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +{ + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + +} + +mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, enable); + + insert_float(&serializer, threshold); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_u8(&deserializer, enable_out); + + assert(threshold_out); + extract_float(&deserializer, threshold_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); +} +mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert_mip_filter_mag_capture_auto_cal_command(mip_serializer* serializer, const mip_filter_mag_capture_auto_cal_command* self) +{ + insert_mip_function_selector(serializer, self->function); + +} +void extract_mip_filter_mag_capture_auto_cal_command(mip_serializer* serializer, mip_filter_mag_capture_auto_cal_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + +} + +mip_cmd_result mip_filter_write_mag_capture_auto_cal(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_save_mag_capture_auto_cal(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_gravity_noise_command(mip_serializer* serializer, const mip_filter_gravity_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_gravity_noise_command(mip_serializer* serializer, mip_filter_gravity_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_gravity_noise_response(mip_serializer* serializer, const mip_filter_gravity_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_gravity_noise_response(mip_serializer* serializer, mip_filter_gravity_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_GRAVITY_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_pressure_altitude_noise_command(mip_serializer* serializer, const mip_filter_pressure_altitude_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->noise); + + } +} +void extract_mip_filter_pressure_altitude_noise_command(mip_serializer* serializer, mip_filter_pressure_altitude_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->noise); + + } +} + +void insert_mip_filter_pressure_altitude_noise_response(mip_serializer* serializer, const mip_filter_pressure_altitude_noise_response* self) +{ + insert_float(serializer, self->noise); + +} +void extract_mip_filter_pressure_altitude_noise_response(mip_serializer* serializer, mip_filter_pressure_altitude_noise_response* self) +{ + extract_float(serializer, &self->noise); + +} + +mip_cmd_result mip_filter_write_pressure_altitude_noise(struct mip_interface* device, float noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, noise); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out); + extract_float(&deserializer, noise_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_soft_iron_matrix_noise_command(mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 9; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_soft_iron_matrix_noise_command(mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 9; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_soft_iron_matrix_noise_response(mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_response* self) +{ + for(unsigned int i=0; i < 9; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_soft_iron_matrix_noise_response(mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_response* self) +{ + for(unsigned int i=0; i < 9; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_mag_noise_command(mip_serializer* serializer, const mip_filter_mag_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_mag_noise_command(mip_serializer* serializer, mip_filter_mag_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_mag_noise_response(mip_serializer* serializer, const mip_filter_mag_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_mag_noise_response(mip_serializer* serializer, mip_filter_mag_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAG_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_inclination_source_command(mip_serializer* serializer, const mip_filter_inclination_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->inclination); + + } +} +void extract_mip_filter_inclination_source_command(mip_serializer* serializer, mip_filter_inclination_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->inclination); + + } +} + +void insert_mip_filter_inclination_source_response(mip_serializer* serializer, const mip_filter_inclination_source_response* self) +{ + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->inclination); + +} +void extract_mip_filter_inclination_source_response(mip_serializer* serializer, mip_filter_inclination_source_response* self) +{ + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->inclination); + +} + +mip_cmd_result mip_filter_write_inclination_source(struct mip_interface* device, mip_filter_mag_param_source source, float inclination) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_mag_param_source(&serializer, source); + + insert_float(&serializer, inclination); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_inclination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* inclination_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_INCLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); + + assert(inclination_out); + extract_float(&deserializer, inclination_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->declination); + + } +} +void extract_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->declination); + + } +} + +void insert_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self) +{ + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->declination); + +} +void extract_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self) +{ + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->declination); + +} + +mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source source, float declination) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_mag_param_source(&serializer, source); + + insert_float(&serializer, declination); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* declination_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_DECLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); + + assert(declination_out); + extract_float(&deserializer, declination_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_mag_field_magnitude_source_command(mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->magnitude); + + } +} +void extract_mip_filter_mag_field_magnitude_source_command(mip_serializer* serializer, mip_filter_mag_field_magnitude_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->magnitude); } } -void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +void insert_mip_filter_mag_field_magnitude_source_response(mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_response* self) { - insert_u8(serializer, self->enable); + insert_mip_filter_mag_param_source(serializer, self->source); - insert_float(serializer, self->threshold); + insert_float(serializer, self->magnitude); } -void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +void extract_mip_filter_mag_field_magnitude_source_response(mip_serializer* serializer, mip_filter_mag_field_magnitude_source_response* self) { - extract_u8(serializer, &self->enable); + extract_mip_filter_mag_param_source(serializer, &self->source); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->magnitude); } -mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +mip_cmd_result mip_filter_write_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source source, float magnitude) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2446,15 +3436,15 @@ mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, enable); + insert_mip_filter_mag_param_source(&serializer, source); - insert_float(&serializer, threshold); + insert_float(&serializer, magnitude); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +mip_cmd_result mip_filter_read_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* magnitude_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2465,25 +3455,25 @@ mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, u assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(enable_out); - extract_u8(&deserializer, enable_out); + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); - assert(threshold_out); - extract_float(&deserializer, threshold_out); + assert(magnitude_out); + extract_float(&deserializer, magnitude_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_save_mag_field_magnitude_source(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2493,9 +3483,9 @@ mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_load_mag_field_magnitude_source(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2505,9 +3495,9 @@ mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_default_mag_field_magnitude_source(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2517,15 +3507,7 @@ mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); -} -mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert_mip_filter_reference_position_command(mip_serializer* serializer, const mip_filter_reference_position_command* self) { @@ -4780,261 +5762,6 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_GNSS_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self) -{ - insert_mip_function_selector(serializer, self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); - - } -} -void extract_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self) -{ - extract_mip_function_selector(serializer, &self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); - - } -} - -void insert_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self) -{ - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); - -} -void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self) -{ - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); - -} - -mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - - insert_float(&serializer, x); - - insert_float(&serializer, y); - - insert_float(&serializer, z); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - - assert(mip_serializer_is_ok(&serializer)); - - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); - - assert(z_out); - extract_float(&deserializer, z_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self) -{ - insert_mip_function_selector(serializer, self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - insert_mip_filter_mag_declination_source(serializer, self->source); - - insert_float(serializer, self->declination); - - } -} -void extract_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self) -{ - extract_mip_function_selector(serializer, &self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - extract_mip_filter_mag_declination_source(serializer, &self->source); - - extract_float(serializer, &self->declination); - - } -} - -void insert_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self) -{ - insert_mip_filter_mag_declination_source(serializer, self->source); - - insert_float(serializer, self->declination); - -} -void extract_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self) -{ - extract_mip_filter_mag_declination_source(serializer, &self->source); - - extract_float(serializer, &self->declination); - -} - -mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source source, float declination) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - - insert_mip_filter_mag_declination_source(&serializer, source); - - insert_float(&serializer, declination); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source* source_out, float* declination_out) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - - assert(mip_serializer_is_ok(&serializer)); - - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_DECLINATION_SOURCE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(source_out); - extract_mip_filter_mag_declination_source(&deserializer, source_out); - - assert(declination_out); - extract_float(&deserializer, declination_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert_mip_filter_set_initial_heading_command(mip_serializer* serializer, const mip_filter_set_initial_heading_command* self) { insert_float(serializer, self->heading); diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 7048f63d0..ec4f673ad 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -1361,11 +1361,8 @@ void insert(Serializer& serializer, const AccelNoise& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1375,51 +1372,40 @@ void extract(Serializer& serializer, AccelNoise& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const AccelNoise::Response& self) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, AccelNoise::Response& self) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z) +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, x); - - insert(serializer, y); - - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1434,14 +1420,9 @@ CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, flo { Serializer deserializer(buffer, responseLength); - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); - - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1484,11 +1465,8 @@ void insert(Serializer& serializer, const GyroNoise& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1498,51 +1476,40 @@ void extract(Serializer& serializer, GyroNoise& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const GyroNoise::Response& self) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, GyroNoise::Response& self) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z) +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, x); - - insert(serializer, y); - - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1557,14 +1524,9 @@ CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, floa { Serializer deserializer(buffer, responseLength); - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); - - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1607,17 +1569,11 @@ void insert(Serializer& serializer, const AccelBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1627,75 +1583,53 @@ void extract(Serializer& serializer, AccelBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const AccelBiasModel::Response& self) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, AccelBiasModel::Response& self) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, xBeta); - - insert(serializer, yBeta); - - insert(serializer, zBeta); - - insert(serializer, x); - - insert(serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, beta[i]); - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1710,23 +1644,13 @@ CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* y { Serializer deserializer(buffer, responseLength); - assert(xBetaOut); - extract(deserializer, *xBetaOut); - - assert(yBetaOut); - extract(deserializer, *yBetaOut); - - assert(zBetaOut); - extract(deserializer, *zBetaOut); - - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); + assert(betaOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, betaOut[i]); - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1769,17 +1693,11 @@ void insert(Serializer& serializer, const GyroBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1789,75 +1707,53 @@ void extract(Serializer& serializer, GyroBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const GyroBiasModel::Response& self) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, GyroBiasModel::Response& self) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, xBeta); - - insert(serializer, yBeta); - - insert(serializer, zBeta); - - insert(serializer, x); - - insert(serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, beta[i]); - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1872,23 +1768,13 @@ CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yB { Serializer deserializer(buffer, responseLength); - assert(xBetaOut); - extract(deserializer, *xBetaOut); - - assert(yBetaOut); - extract(deserializer, *yBetaOut); - - assert(zBetaOut); - extract(deserializer, *zBetaOut); - - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); + assert(betaOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, betaOut[i]); - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1931,7 +1817,7 @@ void insert(Serializer& serializer, const AltitudeAiding& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.aiding_selector); + insert(serializer, self.selector); } } @@ -1941,35 +1827,35 @@ void extract(Serializer& serializer, AltitudeAiding& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.aiding_selector); + extract(serializer, self.selector); } } void insert(Serializer& serializer, const AltitudeAiding::Response& self) { - insert(serializer, self.aiding_selector); + insert(serializer, self.selector); } void extract(Serializer& serializer, AltitudeAiding::Response& self) { - extract(serializer, self.aiding_selector); + extract(serializer, self.selector); } -CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector) +CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, aidingSelector); + insert(serializer, selector); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut) +CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1984,8 +1870,8 @@ CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOu { Serializer deserializer(buffer, responseLength); - assert(aidingSelectorOut); - extract(deserializer, *aidingSelectorOut); + assert(selectorOut); + extract(deserializer, *selectorOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -2022,6 +1908,103 @@ CmdResult defaultAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const PitchRollAiding& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + } +} +void extract(Serializer& serializer, PitchRollAiding& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + } +} + +void insert(Serializer& serializer, const PitchRollAiding::Response& self) +{ + insert(serializer, self.source); + +} +void extract(Serializer& serializer, PitchRollAiding::Response& self) +{ + extract(serializer, self.source); + +} + +CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult savePitchRollAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadPitchRollAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultPitchRollAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const AutoZupt& self) { insert(serializer, self.function); @@ -2178,15 +2161,919 @@ CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float t Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, enable); + insert(serializer, enable); + + insert(serializer, threshold); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(thresholdOut); + extract(deserializer, *thresholdOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const CommandedZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedAngularZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const MagCaptureAutoCal& self) +{ + insert(serializer, self.function); + +} +void extract(Serializer& serializer, MagCaptureAutoCal& self) +{ + extract(serializer, self.function); + +} + +CmdResult writeMagCaptureAutoCal(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult saveMagCaptureAutoCal(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const GravityNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, GravityNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const GravityNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, GravityNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const PressureAltitudeNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.noise); + + } +} +void extract(Serializer& serializer, PressureAltitudeNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.noise); + + } +} + +void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self) +{ + insert(serializer, self.noise); + +} +void extract(Serializer& serializer, PressureAltitudeNoise::Response& self) +{ + extract(serializer, self.noise); + +} + +CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, noise); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), CMD_PRESSURE_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut); + extract(deserializer, *noiseOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult savePressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadPressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultPressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const HardIronOffsetNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, HardIronOffsetNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const SoftIronMatrixNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 9; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, SoftIronMatrixNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 9; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self) +{ + for(unsigned int i=0; i < 9; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self) +{ + for(unsigned int i=0; i < 9; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, MagNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const MagNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, MagNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeMagNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const InclinationSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.inclination); + + } +} +void extract(Serializer& serializer, InclinationSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.inclination); + + } +} + +void insert(Serializer& serializer, const InclinationSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.inclination); + +} +void extract(Serializer& serializer, InclinationSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.inclination); + +} + +CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + insert(serializer, inclination); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + assert(inclinationOut); + extract(deserializer, *inclinationOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagneticDeclinationSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.declination); + + } +} +void extract(Serializer& serializer, MagneticDeclinationSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.declination); + + } +} + +void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.declination); + +} +void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.declination); + +} + +CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + insert(serializer, declination); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + assert(declinationOut); + extract(deserializer, *declinationOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagFieldMagnitudeSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.magnitude); + + } +} +void extract(Serializer& serializer, MagFieldMagnitudeSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.magnitude); + + } +} + +void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.magnitude); + +} +void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.magnitude); + +} + +CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); - insert(serializer, threshold); + insert(serializer, magnitude); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2195,24 +3082,24 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(enableOut); - extract(deserializer, *enableOut); + assert(sourceOut); + extract(deserializer, *sourceOut); - assert(thresholdOut); - extract(deserializer, *thresholdOut); + assert(magnitudeOut); + extract(deserializer, *magnitudeOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAutoAngularZupt(C::mip_interface& device) +CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2220,9 +3107,9 @@ CmdResult saveAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoAngularZupt(C::mip_interface& device) +CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2230,9 +3117,9 @@ CmdResult loadAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoAngularZupt(C::mip_interface& device) +CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2240,37 +3127,7 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert(Serializer& serializer, const CommandedZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); -} -void insert(Serializer& serializer, const CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedAngularZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert(Serializer& serializer, const ReferencePosition& self) { @@ -4326,239 +5183,6 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const HardIronOffsetNoise& self) -{ - insert(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); - - } -} -void extract(Serializer& serializer, HardIronOffsetNoise& self) -{ - extract(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); - - } -} - -void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self) -{ - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); - -} -void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) -{ - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); - -} - -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::WRITE); - insert(serializer, x); - - insert(serializer, y); - - insert(serializer, z); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::READ); - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); - - assert(zOut); - extract(deserializer, *zOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -CmdResult saveHardIronOffsetNoise(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::SAVE); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult loadHardIronOffsetNoise(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::LOAD); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::RESET); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert(Serializer& serializer, const MagneticDeclinationSource& self) -{ - insert(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - insert(serializer, self.source); - - insert(serializer, self.declination); - - } -} -void extract(Serializer& serializer, MagneticDeclinationSource& self) -{ - extract(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - extract(serializer, self.source); - - extract(serializer, self.declination); - - } -} - -void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self) -{ - insert(serializer, self.source); - - insert(serializer, self.declination); - -} -void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) -{ - extract(serializer, self.source); - - extract(serializer, self.declination); - -} - -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource source, float declination) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::WRITE); - insert(serializer, source); - - insert(serializer, declination); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource* sourceOut, float* declinationOut) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::READ); - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(sourceOut); - extract(deserializer, *sourceOut); - - assert(declinationOut); - extract(deserializer, *declinationOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -CmdResult saveMagneticDeclinationSource(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::SAVE); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult loadMagneticDeclinationSource(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::LOAD); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::RESET); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert(Serializer& serializer, const SetInitialHeading& self) { insert(serializer, self.heading); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 32f1eb72f..f373ee4d6 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -162,13 +162,13 @@ static const mip_filter_reference_frame MIP_FILTER_REFERENCE_FRAME_LLH = 2; /// void insert_mip_filter_reference_frame(struct mip_serializer* serializer, const mip_filter_reference_frame self); void extract_mip_filter_reference_frame(struct mip_serializer* serializer, mip_filter_reference_frame* self); -typedef uint8_t mip_filter_mag_declination_source; -static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE_NONE = 1; ///< Magnetic field is assumed to have an declination angle equal to zero. -static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE_WMM = 2; ///< Magnetic field is assumed to conform to the World Magnetic Model, calculated using current location estimate as an input to the model. -static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE_MANUAL = 3; ///< Magnetic field is assumed to have the declination angle specified by the user. +typedef uint8_t mip_filter_mag_param_source; +static const mip_filter_mag_param_source MIP_FILTER_MAG_PARAM_SOURCE_NONE = 1; ///< No source. See command documentation for default behavior +static const mip_filter_mag_param_source MIP_FILTER_MAG_PARAM_SOURCE_WMM = 2; ///< Magnetic field is assumed to conform to the World Magnetic Model, calculated using current location estimate as an input to the model. +static const mip_filter_mag_param_source MIP_FILTER_MAG_PARAM_SOURCE_MANUAL = 3; ///< Magnetic field is assumed to have the parameter specified by the user. -void insert_mip_filter_mag_declination_source(struct mip_serializer* serializer, const mip_filter_mag_declination_source self); -void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer, mip_filter_mag_declination_source* self); +void insert_mip_filter_mag_param_source(struct mip_serializer* serializer, const mip_filter_mag_param_source self); +void extract_mip_filter_mag_param_source(struct mip_serializer* serializer, mip_filter_mag_param_source* self); typedef uint8_t mip_filter_adaptive_measurement; static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_DISABLED = 0; ///< No adaptive measurement @@ -869,19 +869,16 @@ mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device /// /// Each of the noise values must be greater than 0.0. /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ struct mip_filter_accel_noise_command { mip_function_selector function; - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_noise_command mip_filter_accel_noise_command; @@ -890,17 +887,15 @@ void extract_mip_filter_accel_noise_command(struct mip_serializer* serializer, m struct mip_filter_accel_noise_response { - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_noise_response mip_filter_accel_noise_response; void insert_mip_filter_accel_noise_response(struct mip_serializer* serializer, const mip_filter_accel_noise_response* self); void extract_mip_filter_accel_noise_response(struct mip_serializer* serializer, mip_filter_accel_noise_response* self); -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z); -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); @@ -912,19 +907,16 @@ mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); /// /// Each of the noise values must be greater than 0.0 /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ struct mip_filter_gyro_noise_command { mip_function_selector function; - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_noise_command mip_filter_gyro_noise_command; @@ -933,17 +925,15 @@ void extract_mip_filter_gyro_noise_command(struct mip_serializer* serializer, mi struct mip_filter_gyro_noise_response { - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_noise_response mip_filter_gyro_noise_response; void insert_mip_filter_gyro_noise_response(struct mip_serializer* serializer, const mip_filter_gyro_noise_response* self); void extract_mip_filter_gyro_noise_response(struct mip_serializer* serializer, mip_filter_gyro_noise_response* self); -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z); -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); @@ -953,7 +943,7 @@ mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); ///@defgroup c_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [C] /// Accelerometer Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -961,12 +951,8 @@ mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); struct mip_filter_accel_bias_model_command { mip_function_selector function; - float x_beta; ///< Accel Bias Beta [1/second] - float y_beta; ///< Accel Bias Beta [1/second] - float z_beta; ///< Accel Bias Beta [1/second] - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3]; ///< Accel Bias Beta [1/second] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_bias_model_command mip_filter_accel_bias_model_command; @@ -975,20 +961,16 @@ void extract_mip_filter_accel_bias_model_command(struct mip_serializer* serializ struct mip_filter_accel_bias_model_response { - float x_beta; ///< Accel Bias Beta [1/second] - float y_beta; ///< Accel Bias Beta [1/second] - float z_beta; ///< Accel Bias Beta [1/second] - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3]; ///< Accel Bias Beta [1/second] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_bias_model_response mip_filter_accel_bias_model_response; void insert_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, const mip_filter_accel_bias_model_response* self); void extract_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, mip_filter_accel_bias_model_response* self); -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise); +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device); @@ -998,7 +980,7 @@ mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device) ///@defgroup c_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [C] /// Gyroscope Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -1006,12 +988,8 @@ mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device) struct mip_filter_gyro_bias_model_command { mip_function_selector function; - float x_beta; ///< Gyro Bias Beta [1/second] - float y_beta; ///< Gyro Bias Beta [1/second] - float z_beta; ///< Gyro Bias Beta [1/second] - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3]; ///< Gyro Bias Beta [1/second] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_bias_model_command mip_filter_gyro_bias_model_command; @@ -1020,20 +998,16 @@ void extract_mip_filter_gyro_bias_model_command(struct mip_serializer* serialize struct mip_filter_gyro_bias_model_response { - float x_beta; ///< Gyro Bias Beta [1/second] - float y_beta; ///< Gyro Bias Beta [1/second] - float z_beta; ///< Gyro Bias Beta [1/second] - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3]; ///< Gyro Bias Beta [1/second] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_bias_model_response mip_filter_gyro_bias_model_response; void insert_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self); void extract_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, mip_filter_gyro_bias_model_response* self); -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise); +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); @@ -1041,50 +1015,89 @@ mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [C] -/// Altitude Aiding Control -/// /// Select altitude input for absolute altitude and/or vertical velocity. The primary altitude reading is always GNSS. -/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during GNSS outages. +/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during outages. /// -/// Possible altitude aiding selector values: -/// -/// 0x00 - No altitude aiding (disable) -/// 0x01 - Enable pressure sensor aiding(1) -/// -/// 1. Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. +/// Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. /// /// ///@{ +typedef uint8_t mip_filter_altitude_aiding_command_aiding_selector; +static const mip_filter_altitude_aiding_command_aiding_selector MIP_FILTER_ALTITUDE_AIDING_COMMAND_AIDING_SELECTOR_NONE = 0; ///< No altitude aiding +static const mip_filter_altitude_aiding_command_aiding_selector MIP_FILTER_ALTITUDE_AIDING_COMMAND_AIDING_SELECTOR_PRESURE = 1; ///< Enable pressure sensor aiding + struct mip_filter_altitude_aiding_command { mip_function_selector function; - uint8_t aiding_selector; ///< See above + mip_filter_altitude_aiding_command_aiding_selector selector; ///< See above }; typedef struct mip_filter_altitude_aiding_command mip_filter_altitude_aiding_command; void insert_mip_filter_altitude_aiding_command(struct mip_serializer* serializer, const mip_filter_altitude_aiding_command* self); void extract_mip_filter_altitude_aiding_command(struct mip_serializer* serializer, mip_filter_altitude_aiding_command* self); +void insert_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, const mip_filter_altitude_aiding_command_aiding_selector self); +void extract_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, mip_filter_altitude_aiding_command_aiding_selector* self); + struct mip_filter_altitude_aiding_response { - uint8_t aiding_selector; ///< See above + mip_filter_altitude_aiding_command_aiding_selector selector; ///< See above }; typedef struct mip_filter_altitude_aiding_response mip_filter_altitude_aiding_response; void insert_mip_filter_altitude_aiding_response(struct mip_serializer* serializer, const mip_filter_altitude_aiding_response* self); void extract_mip_filter_altitude_aiding_response(struct mip_serializer* serializer, mip_filter_altitude_aiding_response* self); -mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector); -mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out); +mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector selector); +mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector* selector_out); mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device); mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device); mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_pitch_roll_aiding (0x0D,0x4B) Pitch Roll Aiding [C] +/// Select pitch/roll aiding input. Pitch/roll reading is always derived from GNSS corrected inertial solution. +/// Aiding inputs are used to improve that solution during periods of low dynamics and GNSS outages. +/// +///@{ + +typedef uint8_t mip_filter_pitch_roll_aiding_command_aiding_source; +static const mip_filter_pitch_roll_aiding_command_aiding_source MIP_FILTER_PITCH_ROLL_AIDING_COMMAND_AIDING_SOURCE_NONE = 0; ///< No pitch/roll aiding +static const mip_filter_pitch_roll_aiding_command_aiding_source MIP_FILTER_PITCH_ROLL_AIDING_COMMAND_AIDING_SOURCE_GRAVITY_VEC = 1; ///< Enable gravity vector aiding + +struct mip_filter_pitch_roll_aiding_command +{ + mip_function_selector function; + mip_filter_pitch_roll_aiding_command_aiding_source source; ///< Controls the aiding source + +}; +typedef struct mip_filter_pitch_roll_aiding_command mip_filter_pitch_roll_aiding_command; +void insert_mip_filter_pitch_roll_aiding_command(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command* self); +void extract_mip_filter_pitch_roll_aiding_command(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_command* self); + +void insert_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command_aiding_source self); +void extract_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_command_aiding_source* self); + +struct mip_filter_pitch_roll_aiding_response +{ + mip_filter_pitch_roll_aiding_command_aiding_source source; ///< Controls the aiding source + +}; +typedef struct mip_filter_pitch_roll_aiding_response mip_filter_pitch_roll_aiding_response; +void insert_mip_filter_pitch_roll_aiding_response(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_response* self); +void extract_mip_filter_pitch_roll_aiding_response(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_response* self); + +mip_cmd_result mip_filter_write_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source source); +mip_cmd_result mip_filter_read_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source* source_out); +mip_cmd_result mip_filter_save_pitch_roll_aiding(struct mip_interface* device); +mip_cmd_result mip_filter_load_pitch_roll_aiding(struct mip_interface* device); +mip_cmd_result mip_filter_default_pitch_roll_aiding(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_auto_zupt (0x0D,0x1E) Auto Zupt [C] -/// Zero Velocity Update /// The ZUPT is triggered when the scalar magnitude of the GNSS reported velocity vector is equal-to or less than the threshold value. /// The device will NACK threshold values that are less than zero (i.e.negative.) /// @@ -1156,7 +1169,6 @@ mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_commanded_zupt (0x0D,0x22) Commanded Zupt [C] -/// Commanded Zero Velocity Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1166,7 +1178,6 @@ mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_commanded_angular_zupt (0x0D,0x23) Commanded Angular Zupt [C] -/// Commanded Zero Angular Rate Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1175,6 +1186,329 @@ mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_capture_auto_cal (0x0D,0x27) Mag Capture Auto Cal [C] +/// This command captures the current value of the auto-calibration, applies it to the current fixed hard and soft iron calibration coefficients, and replaces the current fixed hard and soft iron calibration coefficients with the new values. +/// This may be used in place of (or in addition to) a manual hard and soft iron calibration utility. This command also resets the auto-calibration coefficients. +/// Function selector SAVE is the same as issuing the 0x0C, 0x3A and 0x0C, 0x3B commands with the SAVE function selector. +/// +///@{ + +struct mip_filter_mag_capture_auto_cal_command +{ + mip_function_selector function; + +}; +typedef struct mip_filter_mag_capture_auto_cal_command mip_filter_mag_capture_auto_cal_command; +void insert_mip_filter_mag_capture_auto_cal_command(struct mip_serializer* serializer, const mip_filter_mag_capture_auto_cal_command* self); +void extract_mip_filter_mag_capture_auto_cal_command(struct mip_serializer* serializer, mip_filter_mag_capture_auto_cal_command* self); + +mip_cmd_result mip_filter_write_mag_capture_auto_cal(struct mip_interface* device); +mip_cmd_result mip_filter_save_mag_capture_auto_cal(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_gravity_noise (0x0D,0x28) Gravity Noise [C] +/// Set the expected gravity noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// Note: Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. Changing this value modifies how the filter responds to dynamic input and can be used to tune filter performance. +/// Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_gravity_noise_command +{ + mip_function_selector function; + float noise[3]; ///< Gravity Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_gravity_noise_command mip_filter_gravity_noise_command; +void insert_mip_filter_gravity_noise_command(struct mip_serializer* serializer, const mip_filter_gravity_noise_command* self); +void extract_mip_filter_gravity_noise_command(struct mip_serializer* serializer, mip_filter_gravity_noise_command* self); + +struct mip_filter_gravity_noise_response +{ + float noise[3]; ///< Gravity Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_gravity_noise_response mip_filter_gravity_noise_response; +void insert_mip_filter_gravity_noise_response(struct mip_serializer* serializer, const mip_filter_gravity_noise_response* self); +void extract_mip_filter_gravity_noise_response(struct mip_serializer* serializer, mip_filter_gravity_noise_response* self); + +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_gravity_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_gravity_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_gravity_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_pressure_altitude_noise (0x0D,0x29) Pressure Altitude Noise [C] +/// Set the expected pressure altitude noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// The noise value must be greater than 0.0 +/// +/// This noise value represents pressure altitude model noise in the Estimation Filter. +/// A lower value will increase responsiveness of the sensor to pressure changes, however height estimates will be more susceptible to error from air pressure fluctuations not due to changes in altitude. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_pressure_altitude_noise_command +{ + mip_function_selector function; + float noise; ///< Pressure Altitude Noise 1-sigma [m] + +}; +typedef struct mip_filter_pressure_altitude_noise_command mip_filter_pressure_altitude_noise_command; +void insert_mip_filter_pressure_altitude_noise_command(struct mip_serializer* serializer, const mip_filter_pressure_altitude_noise_command* self); +void extract_mip_filter_pressure_altitude_noise_command(struct mip_serializer* serializer, mip_filter_pressure_altitude_noise_command* self); + +struct mip_filter_pressure_altitude_noise_response +{ + float noise; ///< Pressure Altitude Noise 1-sigma [m] + +}; +typedef struct mip_filter_pressure_altitude_noise_response mip_filter_pressure_altitude_noise_response; +void insert_mip_filter_pressure_altitude_noise_response(struct mip_serializer* serializer, const mip_filter_pressure_altitude_noise_response* self); +void extract_mip_filter_pressure_altitude_noise_response(struct mip_serializer* serializer, mip_filter_pressure_altitude_noise_response* self); + +mip_cmd_result mip_filter_write_pressure_altitude_noise(struct mip_interface* device, float noise); +mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_pressure_altitude_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_pressure_altitude_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_pressure_altitude_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [C] +/// Set the expected hard iron offset noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise values represent process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_hard_iron_offset_noise_command +{ + mip_function_selector function; + float noise[3]; ///< Hard Iron Offset Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_command mip_filter_hard_iron_offset_noise_command; +void insert_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self); +void extract_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self); + +struct mip_filter_hard_iron_offset_noise_response +{ + float noise[3]; ///< Hard Iron Offset Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_offset_noise_response; +void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); +void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_soft_iron_matrix_noise (0x0D,0x2C) Soft Iron Matrix Noise [C] +/// Set the expected soft iron matrix noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_soft_iron_matrix_noise_command +{ + mip_function_selector function; + float noise[9]; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + +}; +typedef struct mip_filter_soft_iron_matrix_noise_command mip_filter_soft_iron_matrix_noise_command; +void insert_mip_filter_soft_iron_matrix_noise_command(struct mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_command* self); +void extract_mip_filter_soft_iron_matrix_noise_command(struct mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_command* self); + +struct mip_filter_soft_iron_matrix_noise_response +{ + float noise[9]; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + +}; +typedef struct mip_filter_soft_iron_matrix_noise_response mip_filter_soft_iron_matrix_noise_response; +void insert_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_response* self); +void extract_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_response* self); + +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_soft_iron_matrix_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_soft_iron_matrix_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_noise (0x0D,0x42) Mag Noise [C] +/// Set the expected magnetometer noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 (gauss) +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions +/// +///@{ + +struct mip_filter_mag_noise_command +{ + mip_function_selector function; + float noise[3]; ///< Mag Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_mag_noise_command mip_filter_mag_noise_command; +void insert_mip_filter_mag_noise_command(struct mip_serializer* serializer, const mip_filter_mag_noise_command* self); +void extract_mip_filter_mag_noise_command(struct mip_serializer* serializer, mip_filter_mag_noise_command* self); + +struct mip_filter_mag_noise_response +{ + float noise[3]; ///< Mag Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_mag_noise_response mip_filter_mag_noise_response; +void insert_mip_filter_mag_noise_response(struct mip_serializer* serializer, const mip_filter_mag_noise_response* self); +void extract_mip_filter_mag_noise_response(struct mip_serializer* serializer, mip_filter_mag_noise_response* self); + +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_mag_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_inclination_source (0x0D,0x4C) Inclination Source [C] +/// Set/Get the local magnetic field inclination angle source. +/// +/// This can be used to correct for the local value of inclination (dip angle) of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct mip_filter_inclination_source_command +{ + mip_function_selector function; + mip_filter_mag_param_source source; ///< Inclination Source + float inclination; ///< Inclination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_inclination_source_command mip_filter_inclination_source_command; +void insert_mip_filter_inclination_source_command(struct mip_serializer* serializer, const mip_filter_inclination_source_command* self); +void extract_mip_filter_inclination_source_command(struct mip_serializer* serializer, mip_filter_inclination_source_command* self); + +struct mip_filter_inclination_source_response +{ + mip_filter_mag_param_source source; ///< Inclination Source + float inclination; ///< Inclination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_inclination_source_response mip_filter_inclination_source_response; +void insert_mip_filter_inclination_source_response(struct mip_serializer* serializer, const mip_filter_inclination_source_response* self); +void extract_mip_filter_inclination_source_response(struct mip_serializer* serializer, mip_filter_inclination_source_response* self); + +mip_cmd_result mip_filter_write_inclination_source(struct mip_interface* device, mip_filter_mag_param_source source, float inclination); +mip_cmd_result mip_filter_read_inclination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* inclination_out); +mip_cmd_result mip_filter_save_inclination_source(struct mip_interface* device); +mip_cmd_result mip_filter_load_inclination_source(struct mip_interface* device); +mip_cmd_result mip_filter_default_inclination_source(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [C] +/// Set/Get the local magnetic field declination angle source. +/// +/// This can be used to correct for the local value of declination of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct mip_filter_magnetic_declination_source_command +{ + mip_function_selector function; + mip_filter_mag_param_source source; ///< Magnetic field declination angle source + float declination; ///< Declination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_magnetic_declination_source_command mip_filter_magnetic_declination_source_command; +void insert_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self); +void extract_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self); + +struct mip_filter_magnetic_declination_source_response +{ + mip_filter_mag_param_source source; ///< Magnetic field declination angle source + float declination; ///< Declination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_magnetic_declination_source_response mip_filter_magnetic_declination_source_response; +void insert_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self); +void extract_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self); + +mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source source, float declination); +mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* declination_out); +mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device); +mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device); +mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_field_magnitude_source (0x0D,0x4D) Mag Field Magnitude Source [C] +/// Set/Get the local magnetic field magnitude source. +/// +/// This is used to specify the local magnitude of the earth's magnetic field. +/// Having a correct value for magnitude is important for best performance of the auto-mag calibration feature and for the magnetometer adaptive magnitude. If you do not have an accurate value for the local magnetic field magnitude, it is recommended that you leave the auto-mag calibration feature off. +/// +///@{ + +struct mip_filter_mag_field_magnitude_source_command +{ + mip_function_selector function; + mip_filter_mag_param_source source; ///< Magnetic Field Magnitude Source + float magnitude; ///< Magnitude [gauss] (only required if source = MANUAL) + +}; +typedef struct mip_filter_mag_field_magnitude_source_command mip_filter_mag_field_magnitude_source_command; +void insert_mip_filter_mag_field_magnitude_source_command(struct mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_command* self); +void extract_mip_filter_mag_field_magnitude_source_command(struct mip_serializer* serializer, mip_filter_mag_field_magnitude_source_command* self); + +struct mip_filter_mag_field_magnitude_source_response +{ + mip_filter_mag_param_source source; ///< Magnetic Field Magnitude Source + float magnitude; ///< Magnitude [gauss] (only required if source = MANUAL) + +}; +typedef struct mip_filter_mag_field_magnitude_source_response mip_filter_mag_field_magnitude_source_response; +void insert_mip_filter_mag_field_magnitude_source_response(struct mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_response* self); +void extract_mip_filter_mag_field_magnitude_source_response(struct mip_serializer* serializer, mip_filter_mag_field_magnitude_source_response* self); + +mip_cmd_result mip_filter_write_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source source, float magnitude); +mip_cmd_result mip_filter_read_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* magnitude_out); +mip_cmd_result mip_filter_save_mag_field_magnitude_source(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_field_magnitude_source(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_field_magnitude_source(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_reference_position (0x0D,0x26) Reference Position [C] /// Set the Lat/Long/Alt reference position for the sensor. /// @@ -1870,85 +2204,6 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [C] -/// Set the expected hard iron offset noise 1-sigma values -/// -/// This function can be used to tune the filter performance in the target application. -/// -/// Each of the noise values must be greater than 0.0 -/// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. -/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. -/// Default values provide good performance for most laboratory conditions. -/// -/// -///@{ - -struct mip_filter_hard_iron_offset_noise_command -{ - mip_function_selector function; - float x; ///< HI Offset Noise 1-sima [gauss] - float y; ///< HI Offset Noise 1-sima [gauss] - float z; ///< HI Offset Noise 1-sima [gauss] - -}; -typedef struct mip_filter_hard_iron_offset_noise_command mip_filter_hard_iron_offset_noise_command; -void insert_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self); -void extract_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self); - -struct mip_filter_hard_iron_offset_noise_response -{ - float x; ///< HI Offset Noise 1-sima [gauss] - float y; ///< HI Offset Noise 1-sima [gauss] - float z; ///< HI Offset Noise 1-sima [gauss] - -}; -typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_offset_noise_response; -void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); -void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); - -mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z); -mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); -mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); -mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); -mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [C] -/// Source for magnetic declination angle, and user supplied value for manual selection. -/// -///@{ - -struct mip_filter_magnetic_declination_source_command -{ - mip_function_selector function; - mip_filter_mag_declination_source source; ///< Magnetic field declination angle source - float declination; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - -}; -typedef struct mip_filter_magnetic_declination_source_command mip_filter_magnetic_declination_source_command; -void insert_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self); -void extract_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self); - -struct mip_filter_magnetic_declination_source_response -{ - mip_filter_mag_declination_source source; ///< Magnetic field declination angle source - float declination; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - -}; -typedef struct mip_filter_magnetic_declination_source_response mip_filter_magnetic_declination_source_response; -void insert_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self); -void extract_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self); - -mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source source, float declination); -mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source* source_out, float* declination_out); -mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device); -mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device); -mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_set_initial_heading (0x0D,0x03) Set Initial Heading [C] /// Set the initial heading angle. /// diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 06bebc646..636788352 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -160,11 +160,11 @@ enum class FilterReferenceFrame : uint8_t LLH = 2, ///< WGS84 Latitude, longitude, and height above ellipsoid }; -enum class FilterMagDeclinationSource : uint8_t +enum class FilterMagParamSource : uint8_t { - NONE = 1, ///< Magnetic field is assumed to have an declination angle equal to zero. + NONE = 1, ///< No source. See command documentation for default behavior WMM = 2, ///< Magnetic field is assumed to conform to the World Magnetic Model, calculated using current location estimate as an input to the model. - MANUAL = 3, ///< Magnetic field is assumed to have the declination angle specified by the user. + MANUAL = 3, ///< Magnetic field is assumed to have the parameter specified by the user. }; enum class FilterAdaptiveMeasurement : uint8_t @@ -1050,10 +1050,9 @@ CmdResult defaultAutoInitControl(C::mip_interface& device); /// /// Each of the noise values must be greater than 0.0. /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ @@ -1069,18 +1068,14 @@ struct AccelNoise static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] }; }; @@ -1090,8 +1085,8 @@ void extract(Serializer& serializer, AccelNoise& self); void insert(Serializer& serializer, const AccelNoise::Response& self); void extract(Serializer& serializer, AccelNoise::Response& self); -CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise); +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut); CmdResult saveAccelNoise(C::mip_interface& device); CmdResult loadAccelNoise(C::mip_interface& device); CmdResult defaultAccelNoise(C::mip_interface& device); @@ -1103,10 +1098,9 @@ CmdResult defaultAccelNoise(C::mip_interface& device); /// /// Each of the noise values must be greater than 0.0 /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ @@ -1122,18 +1116,14 @@ struct GyroNoise static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] }; }; @@ -1143,8 +1133,8 @@ void extract(Serializer& serializer, GyroNoise& self); void insert(Serializer& serializer, const GyroNoise::Response& self); void extract(Serializer& serializer, GyroNoise::Response& self); -CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise); +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut); CmdResult saveGyroNoise(C::mip_interface& device); CmdResult loadGyroNoise(C::mip_interface& device); CmdResult defaultGyroNoise(C::mip_interface& device); @@ -1154,7 +1144,7 @@ CmdResult defaultGyroNoise(C::mip_interface& device); ///@defgroup cpp_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [CPP] /// Accelerometer Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -1171,24 +1161,16 @@ struct AccelBiasModel static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x_beta = 0; ///< Accel Bias Beta [1/second] - float y_beta = 0; ///< Accel Bias Beta [1/second] - float z_beta = 0; ///< Accel Bias Beta [1/second] - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Accel Bias Beta [1/second] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; - float x_beta = 0; ///< Accel Bias Beta [1/second] - float y_beta = 0; ///< Accel Bias Beta [1/second] - float z_beta = 0; ///< Accel Bias Beta [1/second] - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Accel Bias Beta [1/second] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] }; }; @@ -1198,8 +1180,8 @@ void extract(Serializer& serializer, AccelBiasModel& self); void insert(Serializer& serializer, const AccelBiasModel::Response& self); void extract(Serializer& serializer, AccelBiasModel::Response& self); -CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); -CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); CmdResult saveAccelBiasModel(C::mip_interface& device); CmdResult loadAccelBiasModel(C::mip_interface& device); CmdResult defaultAccelBiasModel(C::mip_interface& device); @@ -1209,7 +1191,7 @@ CmdResult defaultAccelBiasModel(C::mip_interface& device); ///@defgroup cpp_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [CPP] /// Gyroscope Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -1226,24 +1208,16 @@ struct GyroBiasModel static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x_beta = 0; ///< Gyro Bias Beta [1/second] - float y_beta = 0; ///< Gyro Bias Beta [1/second] - float z_beta = 0; ///< Gyro Bias Beta [1/second] - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Gyro Bias Beta [1/second] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; - float x_beta = 0; ///< Gyro Bias Beta [1/second] - float y_beta = 0; ///< Gyro Bias Beta [1/second] - float z_beta = 0; ///< Gyro Bias Beta [1/second] - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Gyro Bias Beta [1/second] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] }; }; @@ -1253,8 +1227,8 @@ void extract(Serializer& serializer, GyroBiasModel& self); void insert(Serializer& serializer, const GyroBiasModel::Response& self); void extract(Serializer& serializer, GyroBiasModel::Response& self); -CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); -CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); CmdResult saveGyroBiasModel(C::mip_interface& device); CmdResult loadGyroBiasModel(C::mip_interface& device); CmdResult defaultGyroBiasModel(C::mip_interface& device); @@ -1262,17 +1236,10 @@ CmdResult defaultGyroBiasModel(C::mip_interface& device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [CPP] -/// Altitude Aiding Control -/// /// Select altitude input for absolute altitude and/or vertical velocity. The primary altitude reading is always GNSS. -/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during GNSS outages. -/// -/// Possible altitude aiding selector values: -/// -/// 0x00 - No altitude aiding (disable) -/// 0x01 - Enable pressure sensor aiding(1) +/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during outages. /// -/// 1. Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. +/// Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. /// /// ///@{ @@ -1288,15 +1255,21 @@ struct AltitudeAiding static const bool HAS_LOAD_FUNCTION = true; static const bool HAS_RESET_FUNCTION = true; + enum class AidingSelector : uint8_t + { + NONE = 0, ///< No altitude aiding + PRESURE = 1, ///< Enable pressure sensor aiding + }; + FunctionSelector function = static_cast(0); - uint8_t aiding_selector = 0; ///< See above + AidingSelector selector = static_cast(0); ///< See above struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; - uint8_t aiding_selector = 0; ///< See above + AidingSelector selector = static_cast(0); ///< See above }; }; @@ -1306,16 +1279,64 @@ void extract(Serializer& serializer, AltitudeAiding& self); void insert(Serializer& serializer, const AltitudeAiding::Response& self); void extract(Serializer& serializer, AltitudeAiding::Response& self); -CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector); -CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut); +CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); +CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); CmdResult saveAltitudeAiding(C::mip_interface& device); CmdResult loadAltitudeAiding(C::mip_interface& device); CmdResult defaultAltitudeAiding(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_pitch_roll_aiding (0x0D,0x4B) Pitch Roll Aiding [CPP] +/// Select pitch/roll aiding input. Pitch/roll reading is always derived from GNSS corrected inertial solution. +/// Aiding inputs are used to improve that solution during periods of low dynamics and GNSS outages. +/// +///@{ + +struct PitchRollAiding +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class AidingSource : uint8_t + { + NONE = 0, ///< No pitch/roll aiding + GRAVITY_VEC = 1, ///< Enable gravity vector aiding + }; + + FunctionSelector function = static_cast(0); + AidingSource source = static_cast(0); ///< Controls the aiding source + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + + AidingSource source = static_cast(0); ///< Controls the aiding source + + }; +}; +void insert(Serializer& serializer, const PitchRollAiding& self); +void extract(Serializer& serializer, PitchRollAiding& self); + +void insert(Serializer& serializer, const PitchRollAiding::Response& self); +void extract(Serializer& serializer, PitchRollAiding::Response& self); + +CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); +CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); +CmdResult savePitchRollAiding(C::mip_interface& device); +CmdResult loadPitchRollAiding(C::mip_interface& device); +CmdResult defaultPitchRollAiding(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_auto_zupt (0x0D,0x1E) Auto Zupt [CPP] -/// Zero Velocity Update /// The ZUPT is triggered when the scalar magnitude of the GNSS reported velocity vector is equal-to or less than the threshold value. /// The device will NACK threshold values that are less than zero (i.e.negative.) /// @@ -1407,7 +1428,6 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_commanded_zupt (0x0D,0x22) Commanded Zupt [CPP] -/// Commanded Zero Velocity Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1429,7 +1449,6 @@ CmdResult commandedZupt(C::mip_interface& device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_commanded_angular_zupt (0x0D,0x23) Commanded Angular Zupt [CPP] -/// Commanded Zero Angular Rate Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1450,6 +1469,417 @@ CmdResult commandedAngularZupt(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_capture_auto_cal (0x0D,0x27) Mag Capture Auto Cal [CPP] +/// This command captures the current value of the auto-calibration, applies it to the current fixed hard and soft iron calibration coefficients, and replaces the current fixed hard and soft iron calibration coefficients with the new values. +/// This may be used in place of (or in addition to) a manual hard and soft iron calibration utility. This command also resets the auto-calibration coefficients. +/// Function selector SAVE is the same as issuing the 0x0C, 0x3A and 0x0C, 0x3B commands with the SAVE function selector. +/// +///@{ + +struct MagCaptureAutoCal +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = false; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = false; + static const bool HAS_RESET_FUNCTION = false; + + FunctionSelector function = static_cast(0); + +}; +void insert(Serializer& serializer, const MagCaptureAutoCal& self); +void extract(Serializer& serializer, MagCaptureAutoCal& self); + +CmdResult writeMagCaptureAutoCal(C::mip_interface& device); +CmdResult saveMagCaptureAutoCal(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_gravity_noise (0x0D,0x28) Gravity Noise [CPP] +/// Set the expected gravity noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// Note: Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. Changing this value modifies how the filter responds to dynamic input and can be used to tune filter performance. +/// Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct GravityNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[3] = {0}; ///< Gravity Noise 1-sigma [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; + + float noise[3] = {0}; ///< Gravity Noise 1-sigma [gauss] + + }; +}; +void insert(Serializer& serializer, const GravityNoise& self); +void extract(Serializer& serializer, GravityNoise& self); + +void insert(Serializer& serializer, const GravityNoise::Response& self); +void extract(Serializer& serializer, GravityNoise::Response& self); + +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise); +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveGravityNoise(C::mip_interface& device); +CmdResult loadGravityNoise(C::mip_interface& device); +CmdResult defaultGravityNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_pressure_altitude_noise (0x0D,0x29) Pressure Altitude Noise [CPP] +/// Set the expected pressure altitude noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// The noise value must be greater than 0.0 +/// +/// This noise value represents pressure altitude model noise in the Estimation Filter. +/// A lower value will increase responsiveness of the sensor to pressure changes, however height estimates will be more susceptible to error from air pressure fluctuations not due to changes in altitude. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct PressureAltitudeNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + + float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] + + }; +}; +void insert(Serializer& serializer, const PressureAltitudeNoise& self); +void extract(Serializer& serializer, PressureAltitudeNoise& self); + +void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self); +void extract(Serializer& serializer, PressureAltitudeNoise::Response& self); + +CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise); +CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); +CmdResult savePressureAltitudeNoise(C::mip_interface& device); +CmdResult loadPressureAltitudeNoise(C::mip_interface& device); +CmdResult defaultPressureAltitudeNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [CPP] +/// Set the expected hard iron offset noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise values represent process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct HardIronOffsetNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[3] = {0}; ///< Hard Iron Offset Noise 1-sigma [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + + float noise[3] = {0}; ///< Hard Iron Offset Noise 1-sigma [gauss] + + }; +}; +void insert(Serializer& serializer, const HardIronOffsetNoise& self); +void extract(Serializer& serializer, HardIronOffsetNoise& self); + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveHardIronOffsetNoise(C::mip_interface& device); +CmdResult loadHardIronOffsetNoise(C::mip_interface& device); +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_soft_iron_matrix_noise (0x0D,0x2C) Soft Iron Matrix Noise [CPP] +/// Set the expected soft iron matrix noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct SoftIronMatrixNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[9] = {0}; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; + + float noise[9] = {0}; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + + }; +}; +void insert(Serializer& serializer, const SoftIronMatrixNoise& self); +void extract(Serializer& serializer, SoftIronMatrixNoise& self); + +void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self); +void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self); + +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveSoftIronMatrixNoise(C::mip_interface& device); +CmdResult loadSoftIronMatrixNoise(C::mip_interface& device); +CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_noise (0x0D,0x42) Mag Noise [CPP] +/// Set the expected magnetometer noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 (gauss) +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions +/// +///@{ + +struct MagNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[3] = {0}; ///< Mag Noise 1-sigma [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; + + float noise[3] = {0}; ///< Mag Noise 1-sigma [gauss] + + }; +}; +void insert(Serializer& serializer, const MagNoise& self); +void extract(Serializer& serializer, MagNoise& self); + +void insert(Serializer& serializer, const MagNoise::Response& self); +void extract(Serializer& serializer, MagNoise::Response& self); + +CmdResult writeMagNoise(C::mip_interface& device, const float* noise); +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveMagNoise(C::mip_interface& device); +CmdResult loadMagNoise(C::mip_interface& device); +CmdResult defaultMagNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_inclination_source (0x0D,0x4C) Inclination Source [CPP] +/// Set/Get the local magnetic field inclination angle source. +/// +/// This can be used to correct for the local value of inclination (dip angle) of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct InclinationSource +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Inclination Source + float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; + + FilterMagParamSource source = static_cast(0); ///< Inclination Source + float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) + + }; +}; +void insert(Serializer& serializer, const InclinationSource& self); +void extract(Serializer& serializer, InclinationSource& self); + +void insert(Serializer& serializer, const InclinationSource::Response& self); +void extract(Serializer& serializer, InclinationSource::Response& self); + +CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); +CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); +CmdResult saveInclinationSource(C::mip_interface& device); +CmdResult loadInclinationSource(C::mip_interface& device); +CmdResult defaultInclinationSource(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [CPP] +/// Set/Get the local magnetic field declination angle source. +/// +/// This can be used to correct for the local value of declination of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct MagneticDeclinationSource +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source + float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; + + FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source + float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) + + }; +}; +void insert(Serializer& serializer, const MagneticDeclinationSource& self); +void extract(Serializer& serializer, MagneticDeclinationSource& self); + +void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); +void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); + +CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); +CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); +CmdResult saveMagneticDeclinationSource(C::mip_interface& device); +CmdResult loadMagneticDeclinationSource(C::mip_interface& device); +CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_field_magnitude_source (0x0D,0x4D) Mag Field Magnitude Source [CPP] +/// Set/Get the local magnetic field magnitude source. +/// +/// This is used to specify the local magnitude of the earth's magnetic field. +/// Having a correct value for magnitude is important for best performance of the auto-mag calibration feature and for the magnetometer adaptive magnitude. If you do not have an accurate value for the local magnetic field magnitude, it is recommended that you leave the auto-mag calibration feature off. +/// +///@{ + +struct MagFieldMagnitudeSource +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source + float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; + + FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source + float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) + + }; +}; +void insert(Serializer& serializer, const MagFieldMagnitudeSource& self); +void extract(Serializer& serializer, MagFieldMagnitudeSource& self); + +void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self); +void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self); + +CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); +CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); +CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device); +CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device); +CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_reference_position (0x0D,0x26) Reference Position [CPP] /// Set the Lat/Long/Alt reference position for the sensor. /// @@ -2329,105 +2759,6 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [CPP] -/// Set the expected hard iron offset noise 1-sigma values -/// -/// This function can be used to tune the filter performance in the target application. -/// -/// Each of the noise values must be greater than 0.0 -/// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. -/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. -/// Default values provide good performance for most laboratory conditions. -/// -/// -///@{ - -struct HardIronOffsetNoise -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - FunctionSelector function = static_cast(0); - float x = 0; ///< HI Offset Noise 1-sima [gauss] - float y = 0; ///< HI Offset Noise 1-sima [gauss] - float z = 0; ///< HI Offset Noise 1-sima [gauss] - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; - - float x = 0; ///< HI Offset Noise 1-sima [gauss] - float y = 0; ///< HI Offset Noise 1-sima [gauss] - float z = 0; ///< HI Offset Noise 1-sima [gauss] - - }; -}; -void insert(Serializer& serializer, const HardIronOffsetNoise& self); -void extract(Serializer& serializer, HardIronOffsetNoise& self); - -void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); -void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); - -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); -CmdResult saveHardIronOffsetNoise(C::mip_interface& device); -CmdResult loadHardIronOffsetNoise(C::mip_interface& device); -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [CPP] -/// Source for magnetic declination angle, and user supplied value for manual selection. -/// -///@{ - -struct MagneticDeclinationSource -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - FunctionSelector function = static_cast(0); - FilterMagDeclinationSource source = static_cast(0); ///< Magnetic field declination angle source - float declination = 0; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; - - FilterMagDeclinationSource source = static_cast(0); ///< Magnetic field declination angle source - float declination = 0; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - - }; -}; -void insert(Serializer& serializer, const MagneticDeclinationSource& self); -void extract(Serializer& serializer, MagneticDeclinationSource& self); - -void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); -void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); - -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource source, float declination); -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource* sourceOut, float* declinationOut); -CmdResult saveMagneticDeclinationSource(C::mip_interface& device); -CmdResult loadMagneticDeclinationSource(C::mip_interface& device); -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_set_initial_heading (0x0D,0x03) Set Initial Heading [CPP] /// Set the initial heading angle. /// From 529f10f68751d9a83ed2ccc5ac51d93ff06b0a5c Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 16 Jun 2023 16:45:07 -0400 Subject: [PATCH 071/252] Generate MIP definitions from branches/cv7_ins@55062. --- src/mip/definitions/commands_3dm.c | 333 +++++ src/mip/definitions/commands_3dm.cpp | 270 ++++ src/mip/definitions/commands_3dm.h | 137 ++ src/mip/definitions/commands_3dm.hpp | 171 +++ src/mip/definitions/commands_aiding.c | 884 ++++++------ src/mip/definitions/commands_aiding.cpp | 753 +++++----- src/mip/definitions/commands_aiding.h | 300 ++-- src/mip/definitions/commands_aiding.hpp | 361 ++--- src/mip/definitions/commands_filter.c | 1733 ++++++++++++++++------- src/mip/definitions/commands_filter.cpp | 1610 ++++++++++++++------- src/mip/definitions/commands_filter.h | 557 ++++++-- src/mip/definitions/commands_filter.hpp | 667 ++++++--- 12 files changed, 5232 insertions(+), 2544 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index bb4551d74..3e4977659 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -1411,6 +1411,207 @@ mip_cmd_result mip_3dm_default_datastream_control(struct mip_interface* device, return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_3dm_constellation_settings_command(mip_serializer* serializer, const mip_3dm_constellation_settings_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u16(serializer, self->max_channels); + + insert_u8(serializer, self->config_count); + + + for(unsigned int i=0; i < self->config_count; i++) + insert_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + + } +} +void extract_mip_3dm_constellation_settings_command(mip_serializer* serializer, mip_3dm_constellation_settings_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u16(serializer, &self->max_channels); + + assert(self->config_count); + extract_count(serializer, &self->config_count, self->config_count); + + for(unsigned int i=0; i < self->config_count; i++) + extract_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + + } +} + +void insert_mip_3dm_constellation_settings_response(mip_serializer* serializer, const mip_3dm_constellation_settings_response* self) +{ + insert_u16(serializer, self->max_channels_available); + + insert_u16(serializer, self->max_channels_use); + + insert_u8(serializer, self->config_count); + + + for(unsigned int i=0; i < self->config_count; i++) + insert_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + +} +void extract_mip_3dm_constellation_settings_response(mip_serializer* serializer, mip_3dm_constellation_settings_response* self) +{ + extract_u16(serializer, &self->max_channels_available); + + extract_u16(serializer, &self->max_channels_use); + + assert(self->config_count); + extract_count(serializer, &self->config_count, self->config_count); + + for(unsigned int i=0; i < self->config_count; i++) + extract_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); + +} + +void insert_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_constellation_id self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_constellation_id* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +void insert_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_option_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_option_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +void insert_mip_3dm_constellation_settings_command_settings(mip_serializer* serializer, const mip_3dm_constellation_settings_command_settings* self) +{ + insert_mip_3dm_constellation_settings_command_constellation_id(serializer, self->constellation_id); + + insert_u8(serializer, self->enable); + + insert_u8(serializer, self->reserved_channels); + + insert_u8(serializer, self->max_channels); + + insert_mip_3dm_constellation_settings_command_option_flags(serializer, self->option_flags); + +} +void extract_mip_3dm_constellation_settings_command_settings(mip_serializer* serializer, mip_3dm_constellation_settings_command_settings* self) +{ + extract_mip_3dm_constellation_settings_command_constellation_id(serializer, &self->constellation_id); + + extract_u8(serializer, &self->enable); + + extract_u8(serializer, &self->reserved_channels); + + extract_u8(serializer, &self->max_channels); + + extract_mip_3dm_constellation_settings_command_option_flags(serializer, &self->option_flags); + +} + +mip_cmd_result mip_3dm_write_constellation_settings(struct mip_interface* device, uint16_t max_channels, uint8_t config_count, const mip_3dm_constellation_settings_command_settings* settings) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u16(&serializer, max_channels); + + insert_u8(&serializer, config_count); + + assert(settings || (config_count == 0)); + for(unsigned int i=0; i < config_count; i++) + insert_mip_3dm_constellation_settings_command_settings(&serializer, &settings[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_read_constellation_settings(struct mip_interface* device, uint16_t* max_channels_available_out, uint16_t* max_channels_use_out, uint8_t* config_count_out, uint8_t config_count_out_max, mip_3dm_constellation_settings_command_settings* settings_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(max_channels_available_out); + extract_u16(&deserializer, max_channels_available_out); + + assert(max_channels_use_out); + extract_u16(&deserializer, max_channels_use_out); + + assert(config_count_out); + extract_count(&deserializer, config_count_out, config_count_out_max); + + assert(settings_out || (config_count_out == 0)); + for(unsigned int i=0; i < *config_count_out; i++) + extract_mip_3dm_constellation_settings_command_settings(&deserializer, &settings_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_3dm_save_constellation_settings(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_load_constellation_settings(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_default_constellation_settings(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_3dm_gnss_sbas_settings_command(mip_serializer* serializer, const mip_3dm_gnss_sbas_settings_command* self) { insert_mip_function_selector(serializer, self->function); @@ -1580,6 +1781,138 @@ mip_cmd_result mip_3dm_default_gnss_sbas_settings(struct mip_interface* device) return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_3dm_gnss_assisted_fix_command(mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, self->option); + + insert_u8(serializer, self->flags); + + } +} +void extract_mip_3dm_gnss_assisted_fix_command(mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, &self->option); + + extract_u8(serializer, &self->flags); + + } +} + +void insert_mip_3dm_gnss_assisted_fix_response(mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_response* self) +{ + insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, self->option); + + insert_u8(serializer, self->flags); + +} +void extract_mip_3dm_gnss_assisted_fix_response(mip_serializer* serializer, mip_3dm_gnss_assisted_fix_response* self) +{ + extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(serializer, &self->option); + + extract_u8(serializer, &self->flags); + +} + +void insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command_assisted_fix_option self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_3dm_write_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option option, uint8_t flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(&serializer, option); + + insert_u8(&serializer, flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_read_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* option_out, uint8_t* flags_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(option_out); + extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(&deserializer, option_out); + + assert(flags_out); + extract_u8(&deserializer, flags_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_3dm_save_gnss_assisted_fix(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_load_gnss_assisted_fix(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_3dm_default_gnss_assisted_fix(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_3dm_gnss_time_assistance_command(mip_serializer* serializer, const mip_3dm_gnss_time_assistance_command* self) { insert_mip_function_selector(serializer, self->function); diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index 5af25a297..a5b1051ce 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -1286,6 +1286,166 @@ CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const ConstellationSettings& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.max_channels); + + insert(serializer, self.config_count); + + for(unsigned int i=0; i < self.config_count; i++) + insert(serializer, self.settings[i]); + + } +} +void extract(Serializer& serializer, ConstellationSettings& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.max_channels); + + C::extract_count(&serializer, &self.config_count, self.config_count); + for(unsigned int i=0; i < self.config_count; i++) + extract(serializer, self.settings[i]); + + } +} + +void insert(Serializer& serializer, const ConstellationSettings::Response& self) +{ + insert(serializer, self.max_channels_available); + + insert(serializer, self.max_channels_use); + + insert(serializer, self.config_count); + + for(unsigned int i=0; i < self.config_count; i++) + insert(serializer, self.settings[i]); + +} +void extract(Serializer& serializer, ConstellationSettings::Response& self) +{ + extract(serializer, self.max_channels_available); + + extract(serializer, self.max_channels_use); + + C::extract_count(&serializer, &self.config_count, self.config_count); + for(unsigned int i=0; i < self.config_count; i++) + extract(serializer, self.settings[i]); + +} + +void insert(Serializer& serializer, const ConstellationSettings::Settings& self) +{ + insert(serializer, self.constellation_id); + + insert(serializer, self.enable); + + insert(serializer, self.reserved_channels); + + insert(serializer, self.max_channels); + + insert(serializer, self.option_flags); + +} +void extract(Serializer& serializer, ConstellationSettings::Settings& self) +{ + extract(serializer, self.constellation_id); + + extract(serializer, self.enable); + + extract(serializer, self.reserved_channels); + + extract(serializer, self.max_channels); + + extract(serializer, self.option_flags); + +} + +CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, maxChannels); + + insert(serializer, configCount); + + assert(settings || (configCount == 0)); + for(unsigned int i=0; i < configCount; i++) + insert(serializer, settings[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(maxChannelsAvailableOut); + extract(deserializer, *maxChannelsAvailableOut); + + assert(maxChannelsUseOut); + extract(deserializer, *maxChannelsUseOut); + + C::extract_count(&deserializer, configCountOut, configCountOutMax); + assert(settingsOut || (configCountOut == 0)); + for(unsigned int i=0; i < *configCountOut; i++) + extract(deserializer, settingsOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveConstellationSettings(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadConstellationSettings(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultConstellationSettings(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const GnssSbasSettings& self) { insert(serializer, self.function); @@ -1425,6 +1585,116 @@ CmdResult defaultGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const GnssAssistedFix& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.option); + + insert(serializer, self.flags); + + } +} +void extract(Serializer& serializer, GnssAssistedFix& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.option); + + extract(serializer, self.flags); + + } +} + +void insert(Serializer& serializer, const GnssAssistedFix::Response& self) +{ + insert(serializer, self.option); + + insert(serializer, self.flags); + +} +void extract(Serializer& serializer, GnssAssistedFix::Response& self) +{ + extract(serializer, self.option); + + extract(serializer, self.flags); + +} + +CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, option); + + insert(serializer, flags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(optionOut); + extract(deserializer, *optionOut); + + assert(flagsOut); + extract(deserializer, *flagsOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGnssAssistedFix(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGnssAssistedFix(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGnssAssistedFix(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const GnssTimeAssistance& self) { insert(serializer, self.function); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 0bc2ad40d..44a09e0fd 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -745,6 +745,92 @@ mip_cmd_result mip_3dm_default_datastream_control(struct mip_interface* device, ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_3dm_constellation_settings (0x0C,0x21) Constellation Settings [C] +/// This command configures which satellite constellations are enabled and how many channels are dedicated to tracking each constellation. +/// +/// Maximum number of tracking channels to use (total for all constellations): +/// 0 to max_channels_available (from reply message) +/// +/// For each constellation you wish to use, include a ConstellationSettings struct. Note the following: +/// +/// Total number of tracking channels (sum of "reserved_channels" for all constellations) must be <= 32: +/// 0 -> 32 Number of reserved channels +/// 0 -> 32 Max number of channels (>= reserved channels) +/// +/// The factory default setting is: GPS and GLONASS enabled. Min/Max for GPS = 8/16, GLONASS = 8/14, SBAS = 1/3, QZSS = 0/3. +/// +/// Warning: SBAS functionality shall not be used in "safety of life" applications! +/// Warning: Any setting that causes the total reserved channels to exceed 32 will result in a NACK. +/// Warning: You cannot enable GLONASS and BeiDou at the same time. +/// Note: Enabling SBAS and QZSS augments GPS accuracy. +/// Note: It is recommended to disable GLONASS and BeiDou if a GPS-only antenna or GPS-only SAW filter is used. +/// +///@{ + +typedef uint8_t mip_3dm_constellation_settings_command_constellation_id; +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_GPS = 0; ///< GPS (G1-G32) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_SBAS = 1; ///< SBAS (S120-S158) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_GALILEO = 2; ///< GALILEO (E1-E36) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_BEIDOU = 3; ///< BeiDou (B1-B37) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_QZSS = 5; ///< QZSS (Q1-Q5) +static const mip_3dm_constellation_settings_command_constellation_id MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_CONSTELLATION_ID_GLONASS = 6; ///< GLONASS (R1-R32) + +typedef uint16_t mip_3dm_constellation_settings_command_option_flags; +static const mip_3dm_constellation_settings_command_option_flags MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_OPTION_FLAGS_NONE = 0x0000; +static const mip_3dm_constellation_settings_command_option_flags MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_OPTION_FLAGS_L1SAIF = 0x0001; ///< Available only for QZSS +static const mip_3dm_constellation_settings_command_option_flags MIP_3DM_CONSTELLATION_SETTINGS_COMMAND_OPTION_FLAGS_ALL = 0x0001; + +struct mip_3dm_constellation_settings_command_settings +{ + mip_3dm_constellation_settings_command_constellation_id constellation_id; ///< Constellation ID + uint8_t enable; ///< Enable/Disable constellation + uint8_t reserved_channels; ///< Minimum number of channels reserved for this constellation + uint8_t max_channels; ///< Maximum number of channels to use for this constallation + mip_3dm_constellation_settings_command_option_flags option_flags; ///< Constellation option Flags + +}; +typedef struct mip_3dm_constellation_settings_command_settings mip_3dm_constellation_settings_command_settings; +struct mip_3dm_constellation_settings_command +{ + mip_function_selector function; + uint16_t max_channels; + uint8_t config_count; + mip_3dm_constellation_settings_command_settings* settings; + +}; +typedef struct mip_3dm_constellation_settings_command mip_3dm_constellation_settings_command; +void insert_mip_3dm_constellation_settings_command(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command* self); +void extract_mip_3dm_constellation_settings_command(struct mip_serializer* serializer, mip_3dm_constellation_settings_command* self); + +void insert_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_constellation_id self); +void extract_mip_3dm_constellation_settings_command_constellation_id(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_constellation_id* self); + +void insert_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_option_flags self); +void extract_mip_3dm_constellation_settings_command_option_flags(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_option_flags* self); + +void insert_mip_3dm_constellation_settings_command_settings(struct mip_serializer* serializer, const mip_3dm_constellation_settings_command_settings* self); +void extract_mip_3dm_constellation_settings_command_settings(struct mip_serializer* serializer, mip_3dm_constellation_settings_command_settings* self); + +struct mip_3dm_constellation_settings_response +{ + uint16_t max_channels_available; ///< Maximum channels available + uint16_t max_channels_use; ///< Maximum channels to use + uint8_t config_count; ///< Number of constellation configurations + mip_3dm_constellation_settings_command_settings* settings; ///< Constellation Settings + +}; +typedef struct mip_3dm_constellation_settings_response mip_3dm_constellation_settings_response; +void insert_mip_3dm_constellation_settings_response(struct mip_serializer* serializer, const mip_3dm_constellation_settings_response* self); +void extract_mip_3dm_constellation_settings_response(struct mip_serializer* serializer, mip_3dm_constellation_settings_response* self); + +mip_cmd_result mip_3dm_write_constellation_settings(struct mip_interface* device, uint16_t max_channels, uint8_t config_count, const mip_3dm_constellation_settings_command_settings* settings); +mip_cmd_result mip_3dm_read_constellation_settings(struct mip_interface* device, uint16_t* max_channels_available_out, uint16_t* max_channels_use_out, uint8_t* config_count_out, uint8_t config_count_out_max, mip_3dm_constellation_settings_command_settings* settings_out); +mip_cmd_result mip_3dm_save_constellation_settings(struct mip_interface* device); +mip_cmd_result mip_3dm_load_constellation_settings(struct mip_interface* device); +mip_cmd_result mip_3dm_default_constellation_settings(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_3dm_gnss_sbas_settings (0x0C,0x22) Gnss Sbas Settings [C] /// Configure the SBAS subsystem /// @@ -796,6 +882,57 @@ mip_cmd_result mip_3dm_default_gnss_sbas_settings(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_3dm_gnss_assisted_fix (0x0C,0x23) Gnss Assisted Fix [C] +/// Set the options for assisted GNSS fix. +/// +/// Devices that implement this command have a dedicated GNSS flash memory and a non-volatile FRAM. +/// These storage mechanisms are used to retain information about the last good GNSS fix. This can greatly reduces the TTFF (Time To First Fix) depending on the age of the stored information. +/// The TTFF can be as low as one second, or up to the equivalent of a cold start. There is a small increase in power used when enabling assisted fix. +/// +/// The fastest fix will be obtained by supplying the device with a GNSS Assist Time Update message containing the current GPS time immediately after subsequent power up. +/// This allows the device to determine if the last GNSS information saved is still fresh enough to improve the TTFF. +/// +/// NOTE: Non-volatile GNSS memory is cleared when going from an enabled state to a disabled state. +/// WARNING: The clearing operation results in an erase operation on the GNSS Flash. The flash has a limited durability of 100,000 write/erase cycles +/// +///@{ + +typedef uint8_t mip_3dm_gnss_assisted_fix_command_assisted_fix_option; +static const mip_3dm_gnss_assisted_fix_command_assisted_fix_option MIP_3DM_GNSS_ASSISTED_FIX_COMMAND_ASSISTED_FIX_OPTION_NONE = 0; ///< No assisted fix (default) +static const mip_3dm_gnss_assisted_fix_command_assisted_fix_option MIP_3DM_GNSS_ASSISTED_FIX_COMMAND_ASSISTED_FIX_OPTION_ENABLED = 1; ///< Enable assisted fix + +struct mip_3dm_gnss_assisted_fix_command +{ + mip_function_selector function; + mip_3dm_gnss_assisted_fix_command_assisted_fix_option option; ///< Assisted fix options + uint8_t flags; ///< Assisted fix flags (set to 0xFF) + +}; +typedef struct mip_3dm_gnss_assisted_fix_command mip_3dm_gnss_assisted_fix_command; +void insert_mip_3dm_gnss_assisted_fix_command(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command* self); +void extract_mip_3dm_gnss_assisted_fix_command(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command* self); + +void insert_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_command_assisted_fix_option self); +void extract_mip_3dm_gnss_assisted_fix_command_assisted_fix_option(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* self); + +struct mip_3dm_gnss_assisted_fix_response +{ + mip_3dm_gnss_assisted_fix_command_assisted_fix_option option; ///< Assisted fix options + uint8_t flags; ///< Assisted fix flags (set to 0xFF) + +}; +typedef struct mip_3dm_gnss_assisted_fix_response mip_3dm_gnss_assisted_fix_response; +void insert_mip_3dm_gnss_assisted_fix_response(struct mip_serializer* serializer, const mip_3dm_gnss_assisted_fix_response* self); +void extract_mip_3dm_gnss_assisted_fix_response(struct mip_serializer* serializer, mip_3dm_gnss_assisted_fix_response* self); + +mip_cmd_result mip_3dm_write_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option option, uint8_t flags); +mip_cmd_result mip_3dm_read_gnss_assisted_fix(struct mip_interface* device, mip_3dm_gnss_assisted_fix_command_assisted_fix_option* option_out, uint8_t* flags_out); +mip_cmd_result mip_3dm_save_gnss_assisted_fix(struct mip_interface* device); +mip_cmd_result mip_3dm_load_gnss_assisted_fix(struct mip_interface* device); +mip_cmd_result mip_3dm_default_gnss_assisted_fix(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_3dm_gnss_time_assistance (0x0C,0x24) Gnss Time Assistance [C] /// Provide the GNSS subsystem with initial time information. /// diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index afe519d41..d354ccd76 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -889,6 +889,117 @@ CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_3dm_constellation_settings (0x0C,0x21) Constellation Settings [CPP] +/// This command configures which satellite constellations are enabled and how many channels are dedicated to tracking each constellation. +/// +/// Maximum number of tracking channels to use (total for all constellations): +/// 0 to max_channels_available (from reply message) +/// +/// For each constellation you wish to use, include a ConstellationSettings struct. Note the following: +/// +/// Total number of tracking channels (sum of "reserved_channels" for all constellations) must be <= 32: +/// 0 -> 32 Number of reserved channels +/// 0 -> 32 Max number of channels (>= reserved channels) +/// +/// The factory default setting is: GPS and GLONASS enabled. Min/Max for GPS = 8/16, GLONASS = 8/14, SBAS = 1/3, QZSS = 0/3. +/// +/// Warning: SBAS functionality shall not be used in "safety of life" applications! +/// Warning: Any setting that causes the total reserved channels to exceed 32 will result in a NACK. +/// Warning: You cannot enable GLONASS and BeiDou at the same time. +/// Note: Enabling SBAS and QZSS augments GPS accuracy. +/// Note: It is recommended to disable GLONASS and BeiDou if a GPS-only antenna or GPS-only SAW filter is used. +/// +///@{ + +struct ConstellationSettings +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class ConstellationId : uint8_t + { + GPS = 0, ///< GPS (G1-G32) + SBAS = 1, ///< SBAS (S120-S158) + GALILEO = 2, ///< GALILEO (E1-E36) + BEIDOU = 3, ///< BeiDou (B1-B37) + QZSS = 5, ///< QZSS (Q1-Q5) + GLONASS = 6, ///< GLONASS (R1-R32) + }; + + struct OptionFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + L1SAIF = 0x0001, ///< Available only for QZSS + ALL = 0x0001, + }; + uint16_t value = NONE; + + OptionFlags() : value(NONE) {} + OptionFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + OptionFlags& operator=(uint16_t val) { value = val; return *this; } + OptionFlags& operator=(int val) { value = val; return *this; } + OptionFlags& operator|=(uint16_t val) { return *this = value | val; } + OptionFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool l1saif() const { return (value & L1SAIF) > 0; } + void l1saif(bool val) { if(val) value |= L1SAIF; else value &= ~L1SAIF; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + struct Settings + { + ConstellationId constellation_id = static_cast(0); ///< Constellation ID + uint8_t enable = 0; ///< Enable/Disable constellation + uint8_t reserved_channels = 0; ///< Minimum number of channels reserved for this constellation + uint8_t max_channels = 0; ///< Maximum number of channels to use for this constallation + OptionFlags option_flags; ///< Constellation option Flags + + }; + FunctionSelector function = static_cast(0); + uint16_t max_channels = 0; + uint8_t config_count = 0; + Settings* settings = {nullptr}; + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; + + uint16_t max_channels_available = 0; ///< Maximum channels available + uint16_t max_channels_use = 0; ///< Maximum channels to use + uint8_t config_count = 0; ///< Number of constellation configurations + Settings* settings = {nullptr}; ///< Constellation Settings + + }; +}; +void insert(Serializer& serializer, const ConstellationSettings& self); +void extract(Serializer& serializer, ConstellationSettings& self); + +void insert(Serializer& serializer, const ConstellationSettings::Settings& self); +void extract(Serializer& serializer, ConstellationSettings::Settings& self); + +void insert(Serializer& serializer, const ConstellationSettings::Response& self); +void extract(Serializer& serializer, ConstellationSettings::Response& self); + +CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); +CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); +CmdResult saveConstellationSettings(C::mip_interface& device); +CmdResult loadConstellationSettings(C::mip_interface& device); +CmdResult defaultConstellationSettings(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_3dm_gnss_sbas_settings (0x0C,0x22) Gnss Sbas Settings [CPP] /// Configure the SBAS subsystem /// @@ -971,6 +1082,66 @@ CmdResult defaultGnssSbasSettings(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_3dm_gnss_assisted_fix (0x0C,0x23) Gnss Assisted Fix [CPP] +/// Set the options for assisted GNSS fix. +/// +/// Devices that implement this command have a dedicated GNSS flash memory and a non-volatile FRAM. +/// These storage mechanisms are used to retain information about the last good GNSS fix. This can greatly reduces the TTFF (Time To First Fix) depending on the age of the stored information. +/// The TTFF can be as low as one second, or up to the equivalent of a cold start. There is a small increase in power used when enabling assisted fix. +/// +/// The fastest fix will be obtained by supplying the device with a GNSS Assist Time Update message containing the current GPS time immediately after subsequent power up. +/// This allows the device to determine if the last GNSS information saved is still fresh enough to improve the TTFF. +/// +/// NOTE: Non-volatile GNSS memory is cleared when going from an enabled state to a disabled state. +/// WARNING: The clearing operation results in an erase operation on the GNSS Flash. The flash has a limited durability of 100,000 write/erase cycles +/// +///@{ + +struct GnssAssistedFix +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class AssistedFixOption : uint8_t + { + NONE = 0, ///< No assisted fix (default) + ENABLED = 1, ///< Enable assisted fix + }; + + FunctionSelector function = static_cast(0); + AssistedFixOption option = static_cast(0); ///< Assisted fix options + uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; + + AssistedFixOption option = static_cast(0); ///< Assisted fix options + uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) + + }; +}; +void insert(Serializer& serializer, const GnssAssistedFix& self); +void extract(Serializer& serializer, GnssAssistedFix& self); + +void insert(Serializer& serializer, const GnssAssistedFix::Response& self); +void extract(Serializer& serializer, GnssAssistedFix::Response& self); + +CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); +CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); +CmdResult saveGnssAssistedFix(C::mip_interface& device); +CmdResult loadGnssAssistedFix(C::mip_interface& device); +CmdResult defaultGnssAssistedFix(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_3dm_gnss_time_assistance (0x0C,0x24) Gnss Time Assistance [CPP] /// Provide the GNSS subsystem with initial time information. /// diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 54556eeab..0bcbab587 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -57,351 +57,534 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb // Mip Fields //////////////////////////////////////////////////////////////////////////////// -void insert_mip_aiding_ecef_pos_command(mip_serializer* serializer, const mip_aiding_ecef_pos_command* self) +void insert_mip_aiding_sensor_frame_mapping_command(mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_command* self) { - insert_mip_time(serializer, &self->time); + insert_mip_function_selector(serializer, self->function); + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->sensor_id); + + insert_u8(serializer, self->frame_id); + + } +} +void extract_mip_aiding_sensor_frame_mapping_command(mip_serializer* serializer, mip_aiding_sensor_frame_mapping_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->sensor_id); + + extract_u8(serializer, &self->frame_id); + + } +} + +void insert_mip_aiding_sensor_frame_mapping_response(mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_response* self) +{ insert_u8(serializer, self->sensor_id); - for(unsigned int i=0; i < 3; i++) - insert_double(serializer, self->position[i]); + insert_u8(serializer, self->frame_id); - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->uncertainty[i]); +} +void extract_mip_aiding_sensor_frame_mapping_response(mip_serializer* serializer, mip_aiding_sensor_frame_mapping_response* self) +{ + extract_u8(serializer, &self->sensor_id); - insert_mip_aiding_ecef_pos_command_valid_flags(serializer, self->valid_flags); + extract_u8(serializer, &self->frame_id); } -void extract_mip_aiding_ecef_pos_command(mip_serializer* serializer, mip_aiding_ecef_pos_command* self) + +mip_cmd_result mip_aiding_write_sensor_frame_mapping(struct mip_interface* device, uint8_t sensor_id, uint8_t frame_id) { - extract_mip_time(serializer, &self->time); + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - extract_u8(serializer, &self->sensor_id); + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - for(unsigned int i=0; i < 3; i++) - extract_double(serializer, &self->position[i]); + insert_u8(&serializer, sensor_id); - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->uncertainty[i]); + insert_u8(&serializer, frame_id); - extract_mip_aiding_ecef_pos_command_valid_flags(serializer, &self->valid_flags); + assert(mip_serializer_is_ok(&serializer)); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } - -void insert_mip_aiding_ecef_pos_response(mip_serializer* serializer, const mip_aiding_ecef_pos_response* self) +mip_cmd_result mip_aiding_read_sensor_frame_mapping(struct mip_interface* device, uint8_t* sensor_id_out, uint8_t* frame_id_out) { - insert_mip_time(serializer, &self->time); + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - insert_u8(serializer, self->sensor_id); + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - for(unsigned int i=0; i < 3; i++) - insert_double(serializer, self->position[i]); + assert(mip_serializer_is_ok(&serializer)); - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->uncertainty[i]); + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_SENSOR_FRAME_MAP, buffer, &responseLength); - insert_mip_aiding_ecef_pos_command_valid_flags(serializer, self->valid_flags); + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(sensor_id_out); + extract_u8(&deserializer, sensor_id_out); + + assert(frame_id_out); + extract_u8(&deserializer, frame_id_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_aiding_save_sensor_frame_mapping(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void extract_mip_aiding_ecef_pos_response(mip_serializer* serializer, mip_aiding_ecef_pos_response* self) +mip_cmd_result mip_aiding_load_sensor_frame_mapping(struct mip_interface* device) { - extract_mip_time(serializer, &self->time); + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - extract_u8(serializer, &self->sensor_id); + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_default_sensor_frame_mapping(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const mip_aiding_reference_frame_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_u8(serializer, self->frame_id); + + insert_mip_aiding_reference_frame_command_format(serializer, self->format); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->translation[i]); + + for(unsigned int i=0; i < 4; i++) + insert_float(serializer, self->rotation[i]); + + } +} +void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_aiding_reference_frame_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_u8(serializer, &self->frame_id); + + extract_mip_aiding_reference_frame_command_format(serializer, &self->format); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->translation[i]); + + for(unsigned int i=0; i < 4; i++) + extract_float(serializer, &self->rotation[i]); + + } +} + +void insert_mip_aiding_reference_frame_response(mip_serializer* serializer, const mip_aiding_reference_frame_response* self) +{ + insert_u8(serializer, self->frame_id); + + insert_mip_aiding_reference_frame_command_format(serializer, self->format); for(unsigned int i=0; i < 3; i++) - extract_double(serializer, &self->position[i]); + insert_float(serializer, self->translation[i]); + + for(unsigned int i=0; i < 4; i++) + insert_float(serializer, self->rotation[i]); + +} +void extract_mip_aiding_reference_frame_response(mip_serializer* serializer, mip_aiding_reference_frame_response* self) +{ + extract_u8(serializer, &self->frame_id); + + extract_mip_aiding_reference_frame_command_format(serializer, &self->format); for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->uncertainty[i]); + extract_float(serializer, &self->translation[i]); - extract_mip_aiding_ecef_pos_command_valid_flags(serializer, &self->valid_flags); + for(unsigned int i=0; i < 4; i++) + extract_float(serializer, &self->rotation[i]); } -void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self) +void insert_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, const mip_aiding_reference_frame_command_format self) { - insert_u16(serializer, (uint16_t)(self)); + insert_u8(serializer, (uint8_t)(self)); } -void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self) +void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, mip_aiding_reference_frame_command_format* self) { - uint16_t tmp = 0; - extract_u16(serializer, &tmp); + uint8_t tmp = 0; + extract_u8(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* position_out, float* uncertainty_out, mip_aiding_ecef_pos_command_valid_flags* valid_flags_out) +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - assert(time); - insert_mip_time(&serializer, time); + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); - assert(position || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert_double(&serializer, position[i]); + insert_mip_aiding_reference_frame_command_format(&serializer, format); - assert(uncertainty || (3 == 0)); + assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, uncertainty[i]); + insert_float(&serializer, translation[i]); - insert_mip_aiding_ecef_pos_command_valid_flags(&serializer, valid_flags); + assert(rotation || (4 == 0)); + for(unsigned int i=0; i < 4; i++) + insert_float(&serializer, rotation[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t* frame_id_out, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECEF_POS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ECEF_POS, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_FRAME_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(time_out); - extract_mip_time(&deserializer, time_out); + assert(frame_id_out); + extract_u8(&deserializer, frame_id_out); - assert(sensor_id_out); - extract_u8(&deserializer, sensor_id_out); + assert(format_out); + extract_mip_aiding_reference_frame_command_format(&deserializer, format_out); - assert(position_out || (3 == 0)); + assert(translation_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_double(&deserializer, &position_out[i]); + extract_float(&deserializer, &translation_out[i]); - assert(uncertainty_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &uncertainty_out[i]); - - assert(valid_flags_out); - extract_mip_aiding_ecef_pos_command_valid_flags(&deserializer, valid_flags_out); + assert(rotation_out || (4 == 0)); + for(unsigned int i=0; i < 4; i++) + extract_float(&deserializer, &rotation_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -void insert_mip_aiding_llh_pos_command(mip_serializer* serializer, const mip_aiding_llh_pos_command* self) +mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device) { - insert_mip_time(serializer, &self->time); - - insert_u8(serializer, self->sensor_id); + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - insert_double(serializer, self->latitude); + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - insert_double(serializer, self->longitude); + assert(mip_serializer_is_ok(&serializer)); - insert_double(serializer, self->height); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->uncertainty[i]); + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - insert_mip_aiding_llh_pos_command_valid_flags(serializer, self->valid_flags); + assert(mip_serializer_is_ok(&serializer)); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void extract_mip_aiding_llh_pos_command(mip_serializer* serializer, mip_aiding_llh_pos_command* self) +mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device) { - extract_mip_time(serializer, &self->time); - - extract_u8(serializer, &self->sensor_id); - - extract_double(serializer, &self->latitude); + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - extract_double(serializer, &self->longitude); + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - extract_double(serializer, &self->height); + assert(mip_serializer_is_ok(&serializer)); - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->uncertainty[i]); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self) +{ + insert_mip_function_selector(serializer, self->function); - extract_mip_aiding_llh_pos_command_valid_flags(serializer, &self->valid_flags); + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); + + } +} +void extract_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); + + } } -void insert_mip_aiding_llh_pos_response(mip_serializer* serializer, const mip_aiding_llh_pos_response* self) +void insert_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self) { - insert_mip_time(serializer, &self->time); - - insert_u8(serializer, self->sensor_id); - - insert_double(serializer, self->latitude); - - insert_double(serializer, self->longitude); - - insert_double(serializer, self->height); - - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->uncertainty[i]); - - insert_mip_aiding_llh_pos_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); } -void extract_mip_aiding_llh_pos_response(mip_serializer* serializer, mip_aiding_llh_pos_response* self) +void extract_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self) { - extract_mip_time(serializer, &self->time); - - extract_u8(serializer, &self->sensor_id); - - extract_double(serializer, &self->latitude); - - extract_double(serializer, &self->longitude); - - extract_double(serializer, &self->height); - - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->uncertainty[i]); - - extract_mip_aiding_llh_pos_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); } -void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self) +void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self) { - insert_u16(serializer, (uint16_t)(self)); + insert_u8(serializer, (uint8_t)(self)); } -void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self) +void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self) { - uint16_t tmp = 0; - extract_u16(serializer, &tmp); + uint8_t tmp = 0; + extract_u8(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* latitude_out, double* longitude_out, double* height_out, float* uncertainty_out, mip_aiding_llh_pos_command_valid_flags* valid_flags_out) +mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - assert(time); - insert_mip_time(&serializer, time); - - insert_u8(&serializer, sensor_id); - - insert_double(&serializer, latitude); + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_double(&serializer, longitude); + insert_mip_aiding_aiding_echo_control_command_mode(&serializer, mode); - insert_double(&serializer, height); + assert(mip_serializer_is_ok(&serializer)); - assert(uncertainty || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, uncertainty[i]); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - insert_mip_aiding_llh_pos_command_valid_flags(&serializer, valid_flags); + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_LLH_POS, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_LLH_POS, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ECHO_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(time_out); - extract_mip_time(&deserializer, time_out); - - assert(sensor_id_out); - extract_u8(&deserializer, sensor_id_out); - - assert(latitude_out); - extract_double(&deserializer, latitude_out); - - assert(longitude_out); - extract_double(&deserializer, longitude_out); - - assert(height_out); - extract_double(&deserializer, height_out); - - assert(uncertainty_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &uncertainty_out[i]); - - assert(valid_flags_out); - extract_mip_aiding_llh_pos_command_valid_flags(&deserializer, valid_flags_out); + assert(mode_out); + extract_mip_aiding_aiding_echo_control_command_mode(&deserializer, mode_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_aiding_ecef_vel_command* self) +mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_aiding_ecef_pos_command(mip_serializer* serializer, const mip_aiding_ecef_pos_command* self) { insert_mip_time(serializer, &self->time); insert_u8(serializer, self->sensor_id); for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->velocity[i]); + insert_double(serializer, self->position[i]); for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_ecef_vel_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_ecef_pos_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_ecef_vel_command(mip_serializer* serializer, mip_aiding_ecef_vel_command* self) +void extract_mip_aiding_ecef_pos_command(mip_serializer* serializer, mip_aiding_ecef_pos_command* self) { extract_mip_time(serializer, &self->time); extract_u8(serializer, &self->sensor_id); for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity[i]); + extract_double(serializer, &self->position[i]); for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->uncertainty[i]); - extract_mip_aiding_ecef_vel_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_ecef_pos_command_valid_flags(serializer, &self->valid_flags); } -void insert_mip_aiding_ecef_vel_response(mip_serializer* serializer, const mip_aiding_ecef_vel_response* self) +void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + assert(position || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_double(&serializer, position[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_ecef_pos_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_POS_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_aiding_llh_pos_command(mip_serializer* serializer, const mip_aiding_llh_pos_command* self) { insert_mip_time(serializer, &self->time); insert_u8(serializer, self->sensor_id); - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->velocity[i]); + insert_double(serializer, self->latitude); + + insert_double(serializer, self->longitude); + + insert_double(serializer, self->height); for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_ecef_vel_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_llh_pos_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_ecef_vel_response(mip_serializer* serializer, mip_aiding_ecef_vel_response* self) +void extract_mip_aiding_llh_pos_command(mip_serializer* serializer, mip_aiding_llh_pos_command* self) { extract_mip_time(serializer, &self->time); extract_u8(serializer, &self->sensor_id); - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity[i]); + extract_double(serializer, &self->latitude); + + extract_double(serializer, &self->longitude); + + extract_double(serializer, &self->height); for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->uncertainty[i]); - extract_mip_aiding_ecef_vel_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_llh_pos_command_valid_flags(serializer, &self->valid_flags); } -void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self) +void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self) { insert_u16(serializer, (uint16_t)(self)); } -void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self) +void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self) { uint16_t tmp = 0; extract_u16(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ecef_vel_command_valid_flags* valid_flags_out) +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -412,49 +595,23 @@ mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* insert_u8(&serializer, sensor_id); - assert(velocity || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, velocity[i]); + insert_double(&serializer, latitude); + + insert_double(&serializer, longitude); + + insert_double(&serializer, height); assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert_float(&serializer, uncertainty[i]); - insert_mip_aiding_ecef_vel_command_valid_flags(&serializer, valid_flags); + insert_mip_aiding_llh_pos_command_valid_flags(&serializer, valid_flags); assert(mip_serializer_is_ok(&serializer)); - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECEF_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ECEF_VEL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(time_out); - extract_mip_time(&deserializer, time_out); - - assert(sensor_id_out); - extract_u8(&deserializer, sensor_id_out); - - assert(velocity_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &velocity_out[i]); - - assert(uncertainty_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &uncertainty_out[i]); - - assert(valid_flags_out); - extract_mip_aiding_ecef_vel_command_valid_flags(&deserializer, valid_flags_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_ned_vel_command(mip_serializer* serializer, const mip_aiding_ned_vel_command* self) +void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_aiding_ecef_vel_command* self) { insert_mip_time(serializer, &self->time); @@ -466,26 +623,62 @@ void insert_mip_aiding_ned_vel_command(mip_serializer* serializer, const mip_aid for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_ned_vel_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_ecef_vel_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_ned_vel_command(mip_serializer* serializer, mip_aiding_ned_vel_command* self) +void extract_mip_aiding_ecef_vel_command(mip_serializer* serializer, mip_aiding_ecef_vel_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->velocity[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_ecef_vel_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) { - extract_mip_time(serializer, &self->time); + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - extract_u8(serializer, &self->sensor_id); + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity[i]); + insert_float(&serializer, velocity[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->uncertainty[i]); + insert_float(&serializer, uncertainty[i]); - extract_mip_aiding_ned_vel_command_valid_flags(serializer, &self->valid_flags); + insert_mip_aiding_ecef_vel_command_valid_flags(&serializer, valid_flags); + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); } - -void insert_mip_aiding_ned_vel_response(mip_serializer* serializer, const mip_aiding_ned_vel_response* self) +void insert_mip_aiding_ned_vel_command(mip_serializer* serializer, const mip_aiding_ned_vel_command* self) { insert_mip_time(serializer, &self->time); @@ -500,7 +693,7 @@ void insert_mip_aiding_ned_vel_response(mip_serializer* serializer, const mip_ai insert_mip_aiding_ned_vel_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_ned_vel_response(mip_serializer* serializer, mip_aiding_ned_vel_response* self) +void extract_mip_aiding_ned_vel_command(mip_serializer* serializer, mip_aiding_ned_vel_command* self) { extract_mip_time(serializer, &self->time); @@ -527,7 +720,7 @@ void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ned_vel_command_valid_flags* valid_flags_out) +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -550,35 +743,7 @@ mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* assert(mip_serializer_is_ok(&serializer)); - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_NED_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_NED_VEL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(time_out); - extract_mip_time(&deserializer, time_out); - - assert(sensor_id_out); - extract_u8(&deserializer, sensor_id_out); - - assert(velocity_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &velocity_out[i]); - - assert(uncertainty_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &uncertainty_out[i]); - - assert(valid_flags_out); - extract_mip_aiding_ned_vel_command_valid_flags(&deserializer, valid_flags_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_NED, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command* self) { @@ -611,37 +776,6 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* ser } -void insert_mip_aiding_vehicle_fixed_frame_velocity_response(mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_response* self) -{ - insert_mip_time(serializer, &self->time); - - insert_u8(serializer, self->sensor_id); - - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->velocity[i]); - - for(unsigned int i=0; i < 3; i++) - insert_float(serializer, self->uncertainty[i]); - - insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, self->valid_flags); - -} -void extract_mip_aiding_vehicle_fixed_frame_velocity_response(mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_response* self) -{ - extract_mip_time(serializer, &self->time); - - extract_u8(serializer, &self->sensor_id); - - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->velocity[i]); - - for(unsigned int i=0; i < 3; i++) - extract_float(serializer, &self->uncertainty[i]); - - extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, &self->valid_flags); - -} - void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self) { insert_u16(serializer, (uint16_t)(self)); @@ -653,7 +787,7 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct *self = tmp; } -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* valid_flags_out) +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -676,35 +810,7 @@ mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* dev assert(mip_serializer_is_ok(&serializer)); - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ODOM_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ODOM_VEL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(time_out); - extract_mip_time(&deserializer, time_out); - - assert(sensor_id_out); - extract_u8(&deserializer, sensor_id_out); - - assert(velocity_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &velocity_out[i]); - - assert(uncertainty_out || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &uncertainty_out[i]); - - assert(valid_flags_out); - extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(&deserializer, valid_flags_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_ODOM, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert_mip_aiding_true_heading_command(mip_serializer* serializer, const mip_aiding_true_heading_command* self) { @@ -733,34 +839,7 @@ void extract_mip_aiding_true_heading_command(mip_serializer* serializer, mip_aid } -void insert_mip_aiding_true_heading_response(mip_serializer* serializer, const mip_aiding_true_heading_response* self) -{ - insert_mip_time(serializer, &self->time); - - insert_u8(serializer, self->sensor_id); - - insert_float(serializer, self->heading); - - insert_float(serializer, self->uncertainty); - - insert_u16(serializer, self->valid_flags); - -} -void extract_mip_aiding_true_heading_response(mip_serializer* serializer, mip_aiding_true_heading_response* self) -{ - extract_mip_time(serializer, &self->time); - - extract_u8(serializer, &self->sensor_id); - - extract_float(serializer, &self->heading); - - extract_float(serializer, &self->uncertainty); - - extract_u16(serializer, &self->valid_flags); - -} - -mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* heading_out, float* uncertainty_out, uint16_t* valid_flags_out) +mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -779,152 +858,7 @@ mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_t assert(mip_serializer_is_ok(&serializer)); - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_HEADING_TRUE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(time_out); - extract_mip_time(&deserializer, time_out); - - assert(sensor_id_out); - extract_u8(&deserializer, sensor_id_out); - - assert(heading_out); - extract_float(&deserializer, heading_out); - - assert(uncertainty_out); - extract_float(&deserializer, uncertainty_out); - - assert(valid_flags_out); - extract_u16(&deserializer, valid_flags_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -void insert_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self) -{ - insert_mip_function_selector(serializer, self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); - - } -} -void extract_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self) -{ - extract_mip_function_selector(serializer, &self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); - - } -} - -void insert_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self) -{ - insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); - -} -void extract_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self) -{ - extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); - -} - -void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self) -{ - insert_u8(serializer, (uint8_t)(self)); -} -void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self) -{ - uint8_t tmp = 0; - extract_u8(serializer, &tmp); - *self = tmp; -} - -mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - - insert_mip_aiding_aiding_echo_control_command_mode(&serializer, mode); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - - assert(mip_serializer_is_ok(&serializer)); - - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_ECHO_CONTROL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(mode_out); - extract_mip_aiding_aiding_echo_control_command_mode(&deserializer, mode_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer)); } #ifdef __cplusplus diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 9a15cf45f..67c77b306 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -48,288 +48,371 @@ void extract(Serializer& serializer, Time& self) // Mip Fields //////////////////////////////////////////////////////////////////////////////// -void insert(Serializer& serializer, const EcefPos& self) +void insert(Serializer& serializer, const SensorFrameMapping& self) { - insert(serializer, self.time); - - insert(serializer, self.sensor_id); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.position[i]); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.uncertainty[i]); - - insert(serializer, self.valid_flags); + insert(serializer, self.function); + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.sensor_id); + + insert(serializer, self.frame_id); + + } } -void extract(Serializer& serializer, EcefPos& self) +void extract(Serializer& serializer, SensorFrameMapping& self) { - extract(serializer, self.time); - - extract(serializer, self.sensor_id); - - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.position[i]); - - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.uncertainty[i]); - - extract(serializer, self.valid_flags); + extract(serializer, self.function); + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.sensor_id); + + extract(serializer, self.frame_id); + + } } -void insert(Serializer& serializer, const EcefPos::Response& self) +void insert(Serializer& serializer, const SensorFrameMapping::Response& self) { - insert(serializer, self.time); - insert(serializer, self.sensor_id); - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.position[i]); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.uncertainty[i]); - - insert(serializer, self.valid_flags); + insert(serializer, self.frame_id); } -void extract(Serializer& serializer, EcefPos::Response& self) +void extract(Serializer& serializer, SensorFrameMapping::Response& self) { - extract(serializer, self.time); - extract(serializer, self.sensor_id); - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.position[i]); - - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.uncertainty[i]); - - extract(serializer, self.valid_flags); + extract(serializer, self.frame_id); } -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* positionOut, float* uncertaintyOut, EcefPos::ValidFlags* validFlagsOut) +CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); - insert(serializer, time); - + insert(serializer, FunctionSelector::WRITE); insert(serializer, sensorId); - assert(position || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert(serializer, position[i]); + insert(serializer, frameId); - assert(uncertainty || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert(serializer, uncertainty[i]); + assert(serializer.isOk()); - insert(serializer, validFlags); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + insert(serializer, FunctionSelector::READ); assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECEF_POS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECEF_POS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(timeOut); - extract(deserializer, *timeOut); - assert(sensorIdOut); extract(deserializer, *sensorIdOut); - assert(positionOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, positionOut[i]); - - assert(uncertaintyOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, uncertaintyOut[i]); - - assert(validFlagsOut); - extract(deserializer, *validFlagsOut); + assert(frameIdOut); + extract(deserializer, *frameIdOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -void insert(Serializer& serializer, const LlhPos& self) +CmdResult saveSensorFrameMapping(C::mip_interface& device) { - insert(serializer, self.time); + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - insert(serializer, self.sensor_id); + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); - insert(serializer, self.latitude); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadSensorFrameMapping(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - insert(serializer, self.longitude); + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); - insert(serializer, self.height); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultSensorFrameMapping(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.uncertainty[i]); + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); - insert(serializer, self.valid_flags); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const ReferenceFrame& self) +{ + insert(serializer, self.function); + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.frame_id); + + insert(serializer, self.format); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.translation[i]); + + for(unsigned int i=0; i < 4; i++) + insert(serializer, self.rotation[i]); + + } } -void extract(Serializer& serializer, LlhPos& self) +void extract(Serializer& serializer, ReferenceFrame& self) { - extract(serializer, self.time); + extract(serializer, self.function); - extract(serializer, self.sensor_id); + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.frame_id); + + extract(serializer, self.format); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.translation[i]); + + for(unsigned int i=0; i < 4; i++) + extract(serializer, self.rotation[i]); + + } +} + +void insert(Serializer& serializer, const ReferenceFrame::Response& self) +{ + insert(serializer, self.frame_id); - extract(serializer, self.latitude); + insert(serializer, self.format); - extract(serializer, self.longitude); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.translation[i]); - extract(serializer, self.height); + for(unsigned int i=0; i < 4; i++) + insert(serializer, self.rotation[i]); + +} +void extract(Serializer& serializer, ReferenceFrame::Response& self) +{ + extract(serializer, self.frame_id); + + extract(serializer, self.format); for(unsigned int i=0; i < 3; i++) - extract(serializer, self.uncertainty[i]); + extract(serializer, self.translation[i]); - extract(serializer, self.valid_flags); + for(unsigned int i=0; i < 4; i++) + extract(serializer, self.rotation[i]); } -void insert(Serializer& serializer, const LlhPos::Response& self) +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation) { - insert(serializer, self.time); - - insert(serializer, self.sensor_id); - - insert(serializer, self.latitude); + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - insert(serializer, self.longitude); + insert(serializer, FunctionSelector::WRITE); + insert(serializer, frameId); - insert(serializer, self.height); + insert(serializer, format); + assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert(serializer, self.uncertainty[i]); + insert(serializer, translation[i]); - insert(serializer, self.valid_flags); + assert(rotation || (4 == 0)); + for(unsigned int i=0; i < 4; i++) + insert(serializer, rotation[i]); + + assert(serializer.isOk()); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void extract(Serializer& serializer, LlhPos::Response& self) +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t* frameIdOut, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut) { - extract(serializer, self.time); + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - extract(serializer, self.sensor_id); + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); - extract(serializer, self.latitude); + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); - extract(serializer, self.longitude); + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(frameIdOut); + extract(deserializer, *frameIdOut); + + assert(formatOut); + extract(deserializer, *formatOut); + + assert(translationOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, translationOut[i]); + + assert(rotationOut || (4 == 0)); + for(unsigned int i=0; i < 4; i++) + extract(deserializer, rotationOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveReferenceFrame(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - extract(serializer, self.height); + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.uncertainty[i]); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadReferenceFrame(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - extract(serializer, self.valid_flags); + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } - -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* latitudeOut, double* longitudeOut, double* heightOut, float* uncertaintyOut, LlhPos::ValidFlags* validFlagsOut) +CmdResult defaultReferenceFrame(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); - insert(serializer, time); + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); - insert(serializer, sensorId); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const AidingEchoControl& self) +{ + insert(serializer, self.function); - insert(serializer, latitude); + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.mode); + + } +} +void extract(Serializer& serializer, AidingEchoControl& self) +{ + extract(serializer, self.function); - insert(serializer, longitude); + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.mode); + + } +} + +void insert(Serializer& serializer, const AidingEchoControl::Response& self) +{ + insert(serializer, self.mode); - insert(serializer, height); +} +void extract(Serializer& serializer, AidingEchoControl::Response& self) +{ + extract(serializer, self.mode); - assert(uncertainty || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert(serializer, uncertainty[i]); +} + +CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - insert(serializer, validFlags); + insert(serializer, FunctionSelector::WRITE); + insert(serializer, mode); assert(serializer.isOk()); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LLH_POS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LLH_POS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(timeOut); - extract(deserializer, *timeOut); - - assert(sensorIdOut); - extract(deserializer, *sensorIdOut); - - assert(latitudeOut); - extract(deserializer, *latitudeOut); - - assert(longitudeOut); - extract(deserializer, *longitudeOut); - - assert(heightOut); - extract(deserializer, *heightOut); - - assert(uncertaintyOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, uncertaintyOut[i]); - - assert(validFlagsOut); - extract(deserializer, *validFlagsOut); + assert(modeOut); + extract(deserializer, *modeOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -void insert(Serializer& serializer, const EcefVel& self) +CmdResult saveAidingEchoControl(C::mip_interface& device) { - insert(serializer, self.time); - - insert(serializer, self.sensor_id); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.velocity[i]); - - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.uncertainty[i]); + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - insert(serializer, self.valid_flags); + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void extract(Serializer& serializer, EcefVel& self) +CmdResult loadAidingEchoControl(C::mip_interface& device) { - extract(serializer, self.time); - - extract(serializer, self.sensor_id); + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.velocity[i]); + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.uncertainty[i]); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAidingEchoControl(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); - extract(serializer, self.valid_flags); + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } - -void insert(Serializer& serializer, const EcefVel::Response& self) +void insert(Serializer& serializer, const EcefPos& self) { insert(serializer, self.time); insert(serializer, self.sensor_id); for(unsigned int i=0; i < 3; i++) - insert(serializer, self.velocity[i]); + insert(serializer, self.position[i]); for(unsigned int i=0; i < 3; i++) insert(serializer, self.uncertainty[i]); @@ -337,14 +420,14 @@ void insert(Serializer& serializer, const EcefVel::Response& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, EcefVel::Response& self) +void extract(Serializer& serializer, EcefPos& self) { extract(serializer, self.time); extract(serializer, self.sensor_id); for(unsigned int i=0; i < 3; i++) - extract(serializer, self.velocity[i]); + extract(serializer, self.position[i]); for(unsigned int i=0; i < 3; i++) extract(serializer, self.uncertainty[i]); @@ -353,7 +436,7 @@ void extract(Serializer& serializer, EcefVel::Response& self) } -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, EcefVel::ValidFlags* validFlagsOut) +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -362,55 +445,31 @@ CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, insert(serializer, sensorId); - assert(velocity || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - insert(serializer, velocity[i]); - - assert(uncertainty || (3 == 0)); + assert(position || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert(serializer, uncertainty[i]); - - insert(serializer, validFlags); - - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECEF_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECEF_VEL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(timeOut); - extract(deserializer, *timeOut); - - assert(sensorIdOut); - extract(deserializer, *sensorIdOut); - - assert(velocityOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, velocityOut[i]); - - assert(uncertaintyOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, uncertaintyOut[i]); - - assert(validFlagsOut); - extract(deserializer, *validFlagsOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; + insert(serializer, position[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_POS_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const NedVel& self) +void insert(Serializer& serializer, const LlhPos& self) { insert(serializer, self.time); insert(serializer, self.sensor_id); - for(unsigned int i=0; i < 3; i++) - insert(serializer, self.velocity[i]); + insert(serializer, self.latitude); + + insert(serializer, self.longitude); + + insert(serializer, self.height); for(unsigned int i=0; i < 3; i++) insert(serializer, self.uncertainty[i]); @@ -418,14 +477,17 @@ void insert(Serializer& serializer, const NedVel& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, NedVel& self) +void extract(Serializer& serializer, LlhPos& self) { extract(serializer, self.time); extract(serializer, self.sensor_id); - for(unsigned int i=0; i < 3; i++) - extract(serializer, self.velocity[i]); + extract(serializer, self.latitude); + + extract(serializer, self.longitude); + + extract(serializer, self.height); for(unsigned int i=0; i < 3; i++) extract(serializer, self.uncertainty[i]); @@ -434,7 +496,32 @@ void extract(Serializer& serializer, NedVel& self) } -void insert(Serializer& serializer, const NedVel::Response& self) +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + insert(serializer, latitude); + + insert(serializer, longitude); + + insert(serializer, height); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const EcefVel& self) { insert(serializer, self.time); @@ -449,7 +536,7 @@ void insert(Serializer& serializer, const NedVel::Response& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, NedVel::Response& self) +void extract(Serializer& serializer, EcefVel& self) { extract(serializer, self.time); @@ -465,7 +552,7 @@ void extract(Serializer& serializer, NedVel::Response& self) } -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, NedVel::ValidFlags* validFlagsOut) +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -486,36 +573,9 @@ CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, c assert(serializer.isOk()); - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NED_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NED_VEL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(timeOut); - extract(deserializer, *timeOut); - - assert(sensorIdOut); - extract(deserializer, *sensorIdOut); - - assert(velocityOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, velocityOut[i]); - - assert(uncertaintyOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, uncertaintyOut[i]); - - assert(validFlagsOut); - extract(deserializer, *validFlagsOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEL_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) +void insert(Serializer& serializer, const NedVel& self) { insert(serializer, self.time); @@ -530,7 +590,7 @@ void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) +void extract(Serializer& serializer, NedVel& self) { extract(serializer, self.time); @@ -546,7 +606,30 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -void insert(Serializer& serializer, const VehicleFixedFrameVelocity::Response& self) +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + assert(velocity || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, velocity[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEL_NED, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) { insert(serializer, self.time); @@ -561,7 +644,7 @@ void insert(Serializer& serializer, const VehicleFixedFrameVelocity::Response& s insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, VehicleFixedFrameVelocity::Response& self) +void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) { extract(serializer, self.time); @@ -577,7 +660,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity::Response& self) } -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, VehicleFixedFrameVelocity::ValidFlags* validFlagsOut) +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -598,34 +681,7 @@ CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, assert(serializer.isOk()); - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOM_VEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOM_VEL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(timeOut); - extract(deserializer, *timeOut); - - assert(sensorIdOut); - extract(deserializer, *sensorIdOut); - - assert(velocityOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, velocityOut[i]); - - assert(uncertaintyOut || (3 == 0)); - for(unsigned int i=0; i < 3; i++) - extract(deserializer, uncertaintyOut[i]); - - assert(validFlagsOut); - extract(deserializer, *validFlagsOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEL_ODOM, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert(Serializer& serializer, const TrueHeading& self) { @@ -654,34 +710,7 @@ void extract(Serializer& serializer, TrueHeading& self) } -void insert(Serializer& serializer, const TrueHeading::Response& self) -{ - insert(serializer, self.time); - - insert(serializer, self.sensor_id); - - insert(serializer, self.heading); - - insert(serializer, self.uncertainty); - - insert(serializer, self.valid_flags); - -} -void extract(Serializer& serializer, TrueHeading::Response& self) -{ - extract(serializer, self.time); - - extract(serializer, self.sensor_id); - - extract(serializer, self.heading); - - extract(serializer, self.uncertainty); - - extract(serializer, self.valid_flags); - -} - -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags, Time* timeOut, uint8_t* sensorIdOut, float* headingOut, float* uncertaintyOut, uint16_t* validFlagsOut) +CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -698,129 +727,7 @@ CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensor assert(serializer.isOk()); - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_TRUE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(timeOut); - extract(deserializer, *timeOut); - - assert(sensorIdOut); - extract(deserializer, *sensorIdOut); - - assert(headingOut); - extract(deserializer, *headingOut); - - assert(uncertaintyOut); - extract(deserializer, *uncertaintyOut); - - assert(validFlagsOut); - extract(deserializer, *validFlagsOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -void insert(Serializer& serializer, const AidingEchoControl& self) -{ - insert(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - insert(serializer, self.mode); - - } -} -void extract(Serializer& serializer, AidingEchoControl& self) -{ - extract(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - extract(serializer, self.mode); - - } -} - -void insert(Serializer& serializer, const AidingEchoControl::Response& self) -{ - insert(serializer, self.mode); - -} -void extract(Serializer& serializer, AidingEchoControl::Response& self) -{ - extract(serializer, self.mode); - -} - -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::WRITE); - insert(serializer, mode); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::READ); - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(modeOut); - extract(deserializer, *modeOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -CmdResult saveAidingEchoControl(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::SAVE); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult loadAidingEchoControl(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::LOAD); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult defaultAidingEchoControl(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::RESET); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer)); } } // namespace commands_aiding diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index ac38558b1..7dd4cab61 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -32,28 +32,27 @@ enum { MIP_AIDING_CMD_DESC_SET = 0x13, - MIP_CMD_DESC_AIDING_ECEF_POS = 0x01, - MIP_CMD_DESC_AIDING_LLH_POS = 0x02, - MIP_CMD_DESC_AIDING_LOCAL_POS = 0x03, - MIP_CMD_DESC_AIDING_HEIGHT_ABS = 0x04, - MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x05, - MIP_CMD_DESC_AIDING_ECEF_VEL = 0x08, - MIP_CMD_DESC_AIDING_NED_VEL = 0x09, - MIP_CMD_DESC_AIDING_ODOM_VEL = 0x0A, - MIP_CMD_DESC_AIDING_WHEELSPEED = 0x0B, - MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x11, - MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x18, - MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x19, - MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x1A, - MIP_CMD_DESC_AIDING_ECHO_CONTROL = 0x6F, + MIP_CMD_DESC_AIDING_FRAME_CONFIG = 0x01, + MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP = 0x02, + MIP_CMD_DESC_AIDING_LOCAL_FRAME = 0x03, + MIP_CMD_DESC_AIDING_ECHO_CONTROL = 0x1F, + MIP_CMD_DESC_AIDING_POS_LOCAL = 0x20, + MIP_CMD_DESC_AIDING_POS_ECEF = 0x21, + MIP_CMD_DESC_AIDING_POS_LLH = 0x22, + MIP_CMD_DESC_AIDING_HEIGHT_ABS = 0x23, + MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x24, + MIP_CMD_DESC_AIDING_VEL_ECEF = 0x28, + MIP_CMD_DESC_AIDING_VEL_NED = 0x29, + MIP_CMD_DESC_AIDING_VEL_ODOM = 0x2A, + MIP_CMD_DESC_AIDING_WHEELSPEED = 0x2B, + MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x31, + MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x38, + MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x39, + MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x3A, - MIP_REPLY_DESC_AIDING_ECEF_POS = 0x81, - MIP_REPLY_DESC_AIDING_LLH_POS = 0x82, - MIP_REPLY_DESC_AIDING_ECEF_VEL = 0x88, - MIP_REPLY_DESC_AIDING_NED_VEL = 0x89, - MIP_REPLY_DESC_AIDING_ODOM_VEL = 0x8A, - MIP_REPLY_DESC_AIDING_HEADING_TRUE = 0x91, - MIP_REPLY_DESC_AIDING_ECHO_CONTROL = 0xEF, + MIP_REPLY_DESC_AIDING_SENSOR_FRAME_MAP = 0x82, + MIP_REPLY_DESC_AIDING_FRAME_CONFIG = 0x81, + MIP_REPLY_DESC_AIDING_ECHO_CONTROL = 0x9F, }; //////////////////////////////////////////////////////////////////////////////// @@ -84,7 +83,123 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_ecef_pos (0x13,0x01) Ecef Pos [C] +///@defgroup c_aiding_sensor_frame_mapping (0x13,0x02) Sensor Frame Mapping [C] +/// +///@{ + +struct mip_aiding_sensor_frame_mapping_command +{ + mip_function_selector function; + uint8_t sensor_id; ///< Sensor ID to configure. Cannot be 0. + uint8_t frame_id; ///< Frame ID to assign to the sensor. Defaults to 1. + +}; +typedef struct mip_aiding_sensor_frame_mapping_command mip_aiding_sensor_frame_mapping_command; +void insert_mip_aiding_sensor_frame_mapping_command(struct mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_command* self); +void extract_mip_aiding_sensor_frame_mapping_command(struct mip_serializer* serializer, mip_aiding_sensor_frame_mapping_command* self); + +struct mip_aiding_sensor_frame_mapping_response +{ + uint8_t sensor_id; ///< Sensor ID to configure. Cannot be 0. + uint8_t frame_id; ///< Frame ID to assign to the sensor. Defaults to 1. + +}; +typedef struct mip_aiding_sensor_frame_mapping_response mip_aiding_sensor_frame_mapping_response; +void insert_mip_aiding_sensor_frame_mapping_response(struct mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_response* self); +void extract_mip_aiding_sensor_frame_mapping_response(struct mip_serializer* serializer, mip_aiding_sensor_frame_mapping_response* self); + +mip_cmd_result mip_aiding_write_sensor_frame_mapping(struct mip_interface* device, uint8_t sensor_id, uint8_t frame_id); +mip_cmd_result mip_aiding_read_sensor_frame_mapping(struct mip_interface* device, uint8_t* sensor_id_out, uint8_t* frame_id_out); +mip_cmd_result mip_aiding_save_sensor_frame_mapping(struct mip_interface* device); +mip_cmd_result mip_aiding_load_sensor_frame_mapping(struct mip_interface* device); +mip_cmd_result mip_aiding_default_sensor_frame_mapping(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_reference_frame (0x13,0x01) Reference Frame [C] +/// +///@{ + +typedef uint8_t mip_aiding_reference_frame_command_format; +static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER = 1; ///< Translation vector followed by euler angles (roll, pitch, yaw). +static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION = 2; ///< Translation vector followed by quaternion (w, x, y, z). + +struct mip_aiding_reference_frame_command +{ + mip_function_selector function; + uint8_t frame_id; ///< Reference frame number. Cannot be 0. + mip_aiding_reference_frame_command_format format; ///< Format of the transformation. + float translation[3]; ///< Translation X, Y, and Z. + float rotation[4]; ///< Depends on the format parameter. Unused values are ignored. + +}; +typedef struct mip_aiding_reference_frame_command mip_aiding_reference_frame_command; +void insert_mip_aiding_reference_frame_command(struct mip_serializer* serializer, const mip_aiding_reference_frame_command* self); +void extract_mip_aiding_reference_frame_command(struct mip_serializer* serializer, mip_aiding_reference_frame_command* self); + +void insert_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, const mip_aiding_reference_frame_command_format self); +void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, mip_aiding_reference_frame_command_format* self); + +struct mip_aiding_reference_frame_response +{ + uint8_t frame_id; ///< Reference frame number. Cannot be 0. + mip_aiding_reference_frame_command_format format; ///< Format of the transformation. + float translation[3]; ///< Translation X, Y, and Z. + float rotation[4]; ///< Depends on the format parameter. Unused values are ignored. + +}; +typedef struct mip_aiding_reference_frame_response mip_aiding_reference_frame_response; +void insert_mip_aiding_reference_frame_response(struct mip_serializer* serializer, const mip_aiding_reference_frame_response* self); +void extract_mip_aiding_reference_frame_response(struct mip_serializer* serializer, mip_aiding_reference_frame_response* self); + +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation); +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t* frame_id_out, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out); +mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device); +mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device); +mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [C] +/// +///@{ + +typedef uint8_t mip_aiding_aiding_echo_control_command_mode; +static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_SUPPRESS_ACK = 0; ///< Suppresses the usual command ack field for aiding messages. +static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_STANDARD = 1; ///< Normal ack/nack behavior. +static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_RESPONSE = 2; ///< Echo the data back as a response. + +struct mip_aiding_aiding_echo_control_command +{ + mip_function_selector function; + mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. + +}; +typedef struct mip_aiding_aiding_echo_control_command mip_aiding_aiding_echo_control_command; +void insert_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self); +void extract_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self); + +void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self); +void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self); + +struct mip_aiding_aiding_echo_control_response +{ + mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. + +}; +typedef struct mip_aiding_aiding_echo_control_response mip_aiding_aiding_echo_control_response; +void insert_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self); +void extract_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self); + +mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode); +mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out); +mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_ecef_pos (0x13,0x21) Ecef Pos [C] /// Cartesian vector position aiding command. Coordinates are given in the WGS84 ECEF system. /// ///@{ @@ -112,24 +227,11 @@ void extract_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self); void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self); -struct mip_aiding_ecef_pos_response -{ - mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. - double position[3]; ///< ECEF position [m]. - float uncertainty[3]; ///< ECEF position uncertainty [m]. - mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. - -}; -typedef struct mip_aiding_ecef_pos_response mip_aiding_ecef_pos_response; -void insert_mip_aiding_ecef_pos_response(struct mip_serializer* serializer, const mip_aiding_ecef_pos_response* self); -void extract_mip_aiding_ecef_pos_response(struct mip_serializer* serializer, mip_aiding_ecef_pos_response* self); - -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* position_out, float* uncertainty_out, mip_aiding_ecef_pos_command_valid_flags* valid_flags_out); +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_llh_pos (0x13,0x02) Llh Pos [C] +///@defgroup c_aiding_llh_pos (0x13,0x22) Llh Pos [C] /// Geodetic position aiding command. Coordinates are given in WGS84 geodetic latitude, longitude, and height above the ellipsoid. /// Uncertainty is given in NED coordinates, which are parallel to incremental changes in latitude, longitude, and height. /// @@ -160,26 +262,11 @@ void extract_mip_aiding_llh_pos_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self); void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self); -struct mip_aiding_llh_pos_response -{ - mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. - double latitude; ///< [deg] - double longitude; ///< [deg] - double height; ///< [m] - float uncertainty[3]; ///< NED position uncertainty. - mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. - -}; -typedef struct mip_aiding_llh_pos_response mip_aiding_llh_pos_response; -void insert_mip_aiding_llh_pos_response(struct mip_serializer* serializer, const mip_aiding_llh_pos_response* self); -void extract_mip_aiding_llh_pos_response(struct mip_serializer* serializer, mip_aiding_llh_pos_response* self); - -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, double* latitude_out, double* longitude_out, double* height_out, float* uncertainty_out, mip_aiding_llh_pos_command_valid_flags* valid_flags_out); +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_ecef_vel (0x13,0x08) Ecef Vel [C] +///@defgroup c_aiding_ecef_vel (0x13,0x28) Ecef Vel [C] /// ECEF velocity aiding command. Coordinates are given in the WGS84 ECEF frame. /// ///@{ @@ -207,24 +294,11 @@ void extract_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self); void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self); -struct mip_aiding_ecef_vel_response -{ - mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. - float velocity[3]; ///< ECEF velocity [m/s]. - float uncertainty[3]; ///< ECEF velocity uncertainty [m/s]. - mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. - -}; -typedef struct mip_aiding_ecef_vel_response mip_aiding_ecef_vel_response; -void insert_mip_aiding_ecef_vel_response(struct mip_serializer* serializer, const mip_aiding_ecef_vel_response* self); -void extract_mip_aiding_ecef_vel_response(struct mip_serializer* serializer, mip_aiding_ecef_vel_response* self); - -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ecef_vel_command_valid_flags* valid_flags_out); +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_ned_vel (0x13,0x09) Ned Vel [C] +///@defgroup c_aiding_ned_vel (0x13,0x29) Ned Vel [C] /// NED velocity aiding command. Coordinates are given in the local North-East-Down frame. /// ///@{ @@ -252,24 +326,11 @@ void extract_mip_aiding_ned_vel_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self); void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self); -struct mip_aiding_ned_vel_response -{ - mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. - float velocity[3]; ///< NED velocity [m/s]. - float uncertainty[3]; ///< NED velocity uncertainty [m/s]. - mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. - -}; -typedef struct mip_aiding_ned_vel_response mip_aiding_ned_vel_response; -void insert_mip_aiding_ned_vel_response(struct mip_serializer* serializer, const mip_aiding_ned_vel_response* self); -void extract_mip_aiding_ned_vel_response(struct mip_serializer* serializer, mip_aiding_ned_vel_response* self); - -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_ned_vel_command_valid_flags* valid_flags_out); +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_vehicle_fixed_frame_velocity (0x13,0x0A) Vehicle Fixed Frame Velocity [C] +///@defgroup c_aiding_vehicle_fixed_frame_velocity (0x13,0x2A) Vehicle Fixed Frame Velocity [C] /// Estimate of velocity of the vehicle in the frame associated /// with the given sensor ID. /// @@ -298,24 +359,11 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializ void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self); void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self); -struct mip_aiding_vehicle_fixed_frame_velocity_response -{ - mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3]; ///< [m/s] - float uncertainty[3]; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; - -}; -typedef struct mip_aiding_vehicle_fixed_frame_velocity_response mip_aiding_vehicle_fixed_frame_velocity_response; -void insert_mip_aiding_vehicle_fixed_frame_velocity_response(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_response* self); -void extract_mip_aiding_vehicle_fixed_frame_velocity_response(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_response* self); - -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* velocity_out, float* uncertainty_out, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* valid_flags_out); +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_true_heading (0x13,0x11) True Heading [C] +///@defgroup c_aiding_true_heading (0x13,0x31) True Heading [C] /// ///@{ @@ -332,59 +380,7 @@ typedef struct mip_aiding_true_heading_command mip_aiding_true_heading_command; void insert_mip_aiding_true_heading_command(struct mip_serializer* serializer, const mip_aiding_true_heading_command* self); void extract_mip_aiding_true_heading_command(struct mip_serializer* serializer, mip_aiding_true_heading_command* self); -struct mip_aiding_true_heading_response -{ - mip_time time; - uint8_t sensor_id; - float heading; ///< Heading in [radians] - float uncertainty; - uint16_t valid_flags; - -}; -typedef struct mip_aiding_true_heading_response mip_aiding_true_heading_response; -void insert_mip_aiding_true_heading_response(struct mip_serializer* serializer, const mip_aiding_true_heading_response* self); -void extract_mip_aiding_true_heading_response(struct mip_serializer* serializer, mip_aiding_true_heading_response* self); - -mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags, mip_time* time_out, uint8_t* sensor_id_out, float* heading_out, float* uncertainty_out, uint16_t* valid_flags_out); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_aiding_echo_control (0x13,0x6F) Aiding Echo Control [C] -/// -///@{ - -typedef uint8_t mip_aiding_aiding_echo_control_command_mode; -static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_SUPPRESS_ACK = 0; ///< Suppresses the usual command ack field for aiding messages. -static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_STANDARD = 1; ///< Normal ack/nack behavior. -static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_RESPONSE = 2; ///< Echo the data back as a response. - -struct mip_aiding_aiding_echo_control_command -{ - mip_function_selector function; - mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. - -}; -typedef struct mip_aiding_aiding_echo_control_command mip_aiding_aiding_echo_control_command; -void insert_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self); -void extract_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self); - -void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self); -void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self); - -struct mip_aiding_aiding_echo_control_response -{ - mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. - -}; -typedef struct mip_aiding_aiding_echo_control_response mip_aiding_aiding_echo_control_response; -void insert_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self); -void extract_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self); - -mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode); -mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out); -mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device); -mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device); -mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags); ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 7fe893830..e93c84e7a 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -31,28 +31,27 @@ enum { DESCRIPTOR_SET = 0x13, - CMD_ECEF_POS = 0x01, - CMD_LLH_POS = 0x02, - CMD_LOCAL_POS = 0x03, - CMD_HEIGHT_ABS = 0x04, - CMD_HEIGHT_REL = 0x05, - CMD_ECEF_VEL = 0x08, - CMD_NED_VEL = 0x09, - CMD_ODOM_VEL = 0x0A, - CMD_WHEELSPEED = 0x0B, - CMD_HEADING_TRUE = 0x11, - CMD_DELTA_POSITION = 0x18, - CMD_DELTA_ATTITUDE = 0x19, - CMD_LOCAL_ANGULAR_RATE = 0x1A, - CMD_ECHO_CONTROL = 0x6F, - - REPLY_ECEF_POS = 0x81, - REPLY_LLH_POS = 0x82, - REPLY_ECEF_VEL = 0x88, - REPLY_NED_VEL = 0x89, - REPLY_ODOM_VEL = 0x8A, - REPLY_HEADING_TRUE = 0x91, - REPLY_ECHO_CONTROL = 0xEF, + CMD_FRAME_CONFIG = 0x01, + CMD_SENSOR_FRAME_MAP = 0x02, + CMD_LOCAL_FRAME = 0x03, + CMD_ECHO_CONTROL = 0x1F, + CMD_POS_LOCAL = 0x20, + CMD_POS_ECEF = 0x21, + CMD_POS_LLH = 0x22, + CMD_HEIGHT_ABS = 0x23, + CMD_HEIGHT_REL = 0x24, + CMD_VEL_ECEF = 0x28, + CMD_VEL_NED = 0x29, + CMD_VEL_ODOM = 0x2A, + CMD_WHEELSPEED = 0x2B, + CMD_HEADING_TRUE = 0x31, + CMD_DELTA_POSITION = 0x38, + CMD_DELTA_ATTITUDE = 0x39, + CMD_LOCAL_ANGULAR_RATE = 0x3A, + + REPLY_SENSOR_FRAME_MAP = 0x82, + REPLY_FRAME_CONFIG = 0x81, + REPLY_ECHO_CONTROL = 0x9F, }; //////////////////////////////////////////////////////////////////////////////// @@ -81,7 +80,151 @@ void extract(Serializer& serializer, Time& self); //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_ecef_pos (0x13,0x01) Ecef Pos [CPP] +///@defgroup cpp_aiding_sensor_frame_mapping (0x13,0x02) Sensor Frame Mapping [CPP] +/// +///@{ + +struct SensorFrameMapping +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. + uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; + + uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. + uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. + + }; +}; +void insert(Serializer& serializer, const SensorFrameMapping& self); +void extract(Serializer& serializer, SensorFrameMapping& self); + +void insert(Serializer& serializer, const SensorFrameMapping::Response& self); +void extract(Serializer& serializer, SensorFrameMapping::Response& self); + +CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); +CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); +CmdResult saveSensorFrameMapping(C::mip_interface& device); +CmdResult loadSensorFrameMapping(C::mip_interface& device); +CmdResult defaultSensorFrameMapping(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_reference_frame (0x13,0x01) Reference Frame [CPP] +/// +///@{ + +struct ReferenceFrame +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class Format : uint8_t + { + EULER = 1, ///< Translation vector followed by euler angles (roll, pitch, yaw). + QUATERNION = 2, ///< Translation vector followed by quaternion (w, x, y, z). + }; + + FunctionSelector function = static_cast(0); + uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. + Format format = static_cast(0); ///< Format of the transformation. + float translation[3] = {0}; ///< Translation X, Y, and Z. + float rotation[4] = {0}; ///< Depends on the format parameter. Unused values are ignored. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; + + uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. + Format format = static_cast(0); ///< Format of the transformation. + float translation[3] = {0}; ///< Translation X, Y, and Z. + float rotation[4] = {0}; ///< Depends on the format parameter. Unused values are ignored. + + }; +}; +void insert(Serializer& serializer, const ReferenceFrame& self); +void extract(Serializer& serializer, ReferenceFrame& self); + +void insert(Serializer& serializer, const ReferenceFrame::Response& self); +void extract(Serializer& serializer, ReferenceFrame::Response& self); + +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation); +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t* frameIdOut, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut); +CmdResult saveReferenceFrame(C::mip_interface& device); +CmdResult loadReferenceFrame(C::mip_interface& device); +CmdResult defaultReferenceFrame(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [CPP] +/// +///@{ + +struct AidingEchoControl +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class Mode : uint8_t + { + SUPPRESS_ACK = 0, ///< Suppresses the usual command ack field for aiding messages. + STANDARD = 1, ///< Normal ack/nack behavior. + RESPONSE = 2, ///< Echo the data back as a response. + }; + + FunctionSelector function = static_cast(0); + Mode mode = static_cast(0); ///< Controls data echoing. + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; + + Mode mode = static_cast(0); ///< Controls data echoing. + + }; +}; +void insert(Serializer& serializer, const AidingEchoControl& self); +void extract(Serializer& serializer, AidingEchoControl& self); + +void insert(Serializer& serializer, const AidingEchoControl::Response& self); +void extract(Serializer& serializer, AidingEchoControl::Response& self); + +CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); +CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); +CmdResult saveAidingEchoControl(C::mip_interface& device); +CmdResult loadAidingEchoControl(C::mip_interface& device); +CmdResult defaultAidingEchoControl(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_ecef_pos (0x13,0x21) Ecef Pos [CPP] /// Cartesian vector position aiding command. Coordinates are given in the WGS84 ECEF system. /// ///@{ @@ -89,7 +232,7 @@ void extract(Serializer& serializer, Time& self); struct EcefPos { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECEF_POS; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; static const bool HAS_FUNCTION_SELECTOR = false; @@ -130,30 +273,15 @@ struct EcefPos float uncertainty[3] = {0}; ///< ECEF position uncertainty [m]. ValidFlags valid_flags; ///< Valid flags. - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECEF_POS; - - Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. - double position[3] = {0}; ///< ECEF position [m]. - float uncertainty[3] = {0}; ///< ECEF position uncertainty [m]. - ValidFlags valid_flags; ///< Valid flags. - - }; }; void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -void insert(Serializer& serializer, const EcefPos::Response& self); -void extract(Serializer& serializer, EcefPos::Response& self); - -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* positionOut, float* uncertaintyOut, EcefPos::ValidFlags* validFlagsOut); +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_llh_pos (0x13,0x02) Llh Pos [CPP] +///@defgroup cpp_aiding_llh_pos (0x13,0x22) Llh Pos [CPP] /// Geodetic position aiding command. Coordinates are given in WGS84 geodetic latitude, longitude, and height above the ellipsoid. /// Uncertainty is given in NED coordinates, which are parallel to incremental changes in latitude, longitude, and height. /// @@ -162,7 +290,7 @@ CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, struct LlhPos { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_LLH_POS; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; static const bool HAS_FUNCTION_SELECTOR = false; @@ -205,32 +333,15 @@ struct LlhPos float uncertainty[3] = {0}; ///< NED position uncertainty. ValidFlags valid_flags; ///< Valid flags. - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_LLH_POS; - - Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. - double latitude = 0; ///< [deg] - double longitude = 0; ///< [deg] - double height = 0; ///< [m] - float uncertainty[3] = {0}; ///< NED position uncertainty. - ValidFlags valid_flags; ///< Valid flags. - - }; }; void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -void insert(Serializer& serializer, const LlhPos::Response& self); -void extract(Serializer& serializer, LlhPos::Response& self); - -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, double* latitudeOut, double* longitudeOut, double* heightOut, float* uncertaintyOut, LlhPos::ValidFlags* validFlagsOut); +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_ecef_vel (0x13,0x08) Ecef Vel [CPP] +///@defgroup cpp_aiding_ecef_vel (0x13,0x28) Ecef Vel [CPP] /// ECEF velocity aiding command. Coordinates are given in the WGS84 ECEF frame. /// ///@{ @@ -238,7 +349,7 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d struct EcefVel { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECEF_VEL; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; static const bool HAS_FUNCTION_SELECTOR = false; @@ -279,30 +390,15 @@ struct EcefVel float uncertainty[3] = {0}; ///< ECEF velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECEF_VEL; - - Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. - float velocity[3] = {0}; ///< ECEF velocity [m/s]. - float uncertainty[3] = {0}; ///< ECEF velocity uncertainty [m/s]. - ValidFlags valid_flags; ///< Valid flags. - - }; }; void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -void insert(Serializer& serializer, const EcefVel::Response& self); -void extract(Serializer& serializer, EcefVel::Response& self); - -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, EcefVel::ValidFlags* validFlagsOut); +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_ned_vel (0x13,0x09) Ned Vel [CPP] +///@defgroup cpp_aiding_ned_vel (0x13,0x29) Ned Vel [CPP] /// NED velocity aiding command. Coordinates are given in the local North-East-Down frame. /// ///@{ @@ -310,7 +406,7 @@ CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, struct NedVel { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_NED_VEL; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; static const bool HAS_FUNCTION_SELECTOR = false; @@ -351,30 +447,15 @@ struct NedVel float uncertainty[3] = {0}; ///< NED velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_NED_VEL; - - Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. - float velocity[3] = {0}; ///< NED velocity [m/s]. - float uncertainty[3] = {0}; ///< NED velocity uncertainty [m/s]. - ValidFlags valid_flags; ///< Valid flags. - - }; }; void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -void insert(Serializer& serializer, const NedVel::Response& self); -void extract(Serializer& serializer, NedVel::Response& self); - -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, NedVel::ValidFlags* validFlagsOut); +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_vehicle_fixed_frame_velocity (0x13,0x0A) Vehicle Fixed Frame Velocity [CPP] +///@defgroup cpp_aiding_vehicle_fixed_frame_velocity (0x13,0x2A) Vehicle Fixed Frame Velocity [CPP] /// Estimate of velocity of the vehicle in the frame associated /// with the given sensor ID. /// @@ -383,7 +464,7 @@ CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, c struct VehicleFixedFrameVelocity { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ODOM_VEL; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; static const bool HAS_FUNCTION_SELECTOR = false; @@ -424,30 +505,15 @@ struct VehicleFixedFrameVelocity float uncertainty[3] = {0}; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ODOM_VEL; - - Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3] = {0}; ///< [m/s] - float uncertainty[3] = {0}; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - ValidFlags valid_flags; - - }; }; void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -void insert(Serializer& serializer, const VehicleFixedFrameVelocity::Response& self); -void extract(Serializer& serializer, VehicleFixedFrameVelocity::Response& self); - -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags, Time* timeOut, uint8_t* sensorIdOut, float* velocityOut, float* uncertaintyOut, VehicleFixedFrameVelocity::ValidFlags* validFlagsOut); +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_true_heading (0x13,0x11) True Heading [CPP] +///@defgroup cpp_aiding_true_heading (0x13,0x31) True Heading [CPP] /// ///@{ @@ -464,74 +530,11 @@ struct TrueHeading float uncertainty = 0; uint16_t valid_flags = 0; - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_HEADING_TRUE; - - Time time; - uint8_t sensor_id = 0; - float heading = 0; ///< Heading in [radians] - float uncertainty = 0; - uint16_t valid_flags = 0; - - }; }; void insert(Serializer& serializer, const TrueHeading& self); void extract(Serializer& serializer, TrueHeading& self); -void insert(Serializer& serializer, const TrueHeading::Response& self); -void extract(Serializer& serializer, TrueHeading::Response& self); - -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags, Time* timeOut, uint8_t* sensorIdOut, float* headingOut, float* uncertaintyOut, uint16_t* validFlagsOut); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_aiding_echo_control (0x13,0x6F) Aiding Echo Control [CPP] -/// -///@{ - -struct AidingEchoControl -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - enum class Mode : uint8_t - { - SUPPRESS_ACK = 0, ///< Suppresses the usual command ack field for aiding messages. - STANDARD = 1, ///< Normal ack/nack behavior. - RESPONSE = 2, ///< Echo the data back as a response. - }; - - FunctionSelector function = static_cast(0); - Mode mode = static_cast(0); ///< Controls data echoing. - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; - - Mode mode = static_cast(0); ///< Controls data echoing. - - }; -}; -void insert(Serializer& serializer, const AidingEchoControl& self); -void extract(Serializer& serializer, AidingEchoControl& self); - -void insert(Serializer& serializer, const AidingEchoControl::Response& self); -void extract(Serializer& serializer, AidingEchoControl::Response& self); - -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); -CmdResult saveAidingEchoControl(C::mip_interface& device); -CmdResult loadAidingEchoControl(C::mip_interface& device); -CmdResult defaultAidingEchoControl(C::mip_interface& device); +CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); ///@} /// diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index ccbca3b69..d71700893 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -33,11 +33,11 @@ void extract_mip_filter_reference_frame(struct mip_serializer* serializer, mip_f *self = tmp; } -void insert_mip_filter_mag_declination_source(struct mip_serializer* serializer, const mip_filter_mag_declination_source self) +void insert_mip_filter_mag_param_source(struct mip_serializer* serializer, const mip_filter_mag_param_source self) { insert_u8(serializer, (uint8_t)(self)); } -void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer, mip_filter_mag_declination_source* self) +void extract_mip_filter_mag_param_source(struct mip_serializer* serializer, mip_filter_mag_param_source* self) { uint8_t tmp = 0; extract_u8(serializer, &tmp); @@ -1561,11 +1561,8 @@ void insert_mip_filter_accel_noise_command(mip_serializer* serializer, const mip if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -1575,35 +1572,26 @@ void extract_mip_filter_accel_noise_command(mip_serializer* serializer, mip_filt if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_accel_noise_response(mip_serializer* serializer, const mip_filter_accel_noise_response* self) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_accel_noise_response(mip_serializer* serializer, mip_filter_accel_noise_response* self) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z) +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1611,17 +1599,15 @@ mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x); - - insert_float(&serializer, y); - - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1639,14 +1625,9 @@ mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); - - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1695,11 +1676,8 @@ void insert_mip_filter_gyro_noise_command(mip_serializer* serializer, const mip_ if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -1709,35 +1687,26 @@ void extract_mip_filter_gyro_noise_command(mip_serializer* serializer, mip_filte if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_gyro_noise_response(mip_serializer* serializer, const mip_filter_gyro_noise_response* self) { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_gyro_noise_response(mip_serializer* serializer, mip_filter_gyro_noise_response* self) { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z) +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1745,17 +1714,15 @@ mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x); - - insert_float(&serializer, y); - - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1773,14 +1740,9 @@ mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); - - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1829,17 +1791,11 @@ void insert_mip_filter_accel_bias_model_command(mip_serializer* serializer, cons if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -1849,53 +1805,35 @@ void extract_mip_filter_accel_bias_model_command(mip_serializer* serializer, mip if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_accel_bias_model_response(mip_serializer* serializer, const mip_filter_accel_bias_model_response* self) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_accel_bias_model_response(mip_serializer* serializer, mip_filter_accel_bias_model_response* self) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1903,23 +1841,19 @@ mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, f insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x_beta); - - insert_float(&serializer, y_beta); - - insert_float(&serializer, z_beta); - - insert_float(&serializer, x); - - insert_float(&serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, beta[i]); - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1937,23 +1871,13 @@ mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, fl mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_beta_out); - extract_float(&deserializer, x_beta_out); - - assert(y_beta_out); - extract_float(&deserializer, y_beta_out); - - assert(z_beta_out); - extract_float(&deserializer, z_beta_out); - - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); + assert(beta_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &beta_out[i]); - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2002,17 +1926,11 @@ void insert_mip_filter_gyro_bias_model_command(mip_serializer* serializer, const if( self->function == MIP_FUNCTION_WRITE ) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } } @@ -2022,53 +1940,35 @@ void extract_mip_filter_gyro_bias_model_command(mip_serializer* serializer, mip_ if( self->function == MIP_FUNCTION_WRITE ) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } } void insert_mip_filter_gyro_bias_model_response(mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self) { - insert_float(serializer, self->x_beta); - - insert_float(serializer, self->y_beta); - - insert_float(serializer, self->z_beta); - - insert_float(serializer, self->x); - - insert_float(serializer, self->y); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->beta[i]); - insert_float(serializer, self->z); + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); } void extract_mip_filter_gyro_bias_model_response(mip_serializer* serializer, mip_filter_gyro_bias_model_response* self) { - extract_float(serializer, &self->x_beta); - - extract_float(serializer, &self->y_beta); - - extract_float(serializer, &self->z_beta); - - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->beta[i]); - extract_float(serializer, &self->z); + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); } -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z) +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2076,23 +1976,19 @@ mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, fl insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_float(&serializer, x_beta); - - insert_float(&serializer, y_beta); - - insert_float(&serializer, z_beta); - - insert_float(&serializer, x); - - insert_float(&serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, beta[i]); - insert_float(&serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out) +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2110,23 +2006,13 @@ mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, flo mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(x_beta_out); - extract_float(&deserializer, x_beta_out); - - assert(y_beta_out); - extract_float(&deserializer, y_beta_out); - - assert(z_beta_out); - extract_float(&deserializer, z_beta_out); - - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); + assert(beta_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &beta_out[i]); - assert(z_out); - extract_float(&deserializer, z_out); + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2175,7 +2061,7 @@ void insert_mip_filter_altitude_aiding_command(mip_serializer* serializer, const if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->aiding_selector); + insert_mip_filter_altitude_aiding_command_aiding_selector(serializer, self->selector); } } @@ -2185,23 +2071,34 @@ void extract_mip_filter_altitude_aiding_command(mip_serializer* serializer, mip_ if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->aiding_selector); + extract_mip_filter_altitude_aiding_command_aiding_selector(serializer, &self->selector); } } void insert_mip_filter_altitude_aiding_response(mip_serializer* serializer, const mip_filter_altitude_aiding_response* self) { - insert_u8(serializer, self->aiding_selector); + insert_mip_filter_altitude_aiding_command_aiding_selector(serializer, self->selector); } void extract_mip_filter_altitude_aiding_response(mip_serializer* serializer, mip_filter_altitude_aiding_response* self) { - extract_u8(serializer, &self->aiding_selector); + extract_mip_filter_altitude_aiding_command_aiding_selector(serializer, &self->selector); } -mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector) +void insert_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, const mip_filter_altitude_aiding_command_aiding_selector self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, mip_filter_altitude_aiding_command_aiding_selector* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector selector) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2209,13 +2106,13 @@ mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, ui insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, aiding_selector); + insert_mip_filter_altitude_aiding_command_aiding_selector(&serializer, selector); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out) +mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector* selector_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2233,8 +2130,8 @@ mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uin mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(aiding_selector_out); - extract_u8(&deserializer, aiding_selector_out); + assert(selector_out); + extract_mip_filter_altitude_aiding_command_aiding_selector(&deserializer, selector_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2277,6 +2174,125 @@ mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device) return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_filter_pitch_roll_aiding_command(mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, self->source); + + } +} +void extract_mip_filter_pitch_roll_aiding_command(mip_serializer* serializer, mip_filter_pitch_roll_aiding_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, &self->source); + + } +} + +void insert_mip_filter_pitch_roll_aiding_response(mip_serializer* serializer, const mip_filter_pitch_roll_aiding_response* self) +{ + insert_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, self->source); + +} +void extract_mip_filter_pitch_roll_aiding_response(mip_serializer* serializer, mip_filter_pitch_roll_aiding_response* self) +{ + extract_mip_filter_pitch_roll_aiding_command_aiding_source(serializer, &self->source); + +} + +void insert_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command_aiding_source self) +{ + insert_u8(serializer, (uint8_t)(self)); +} +void extract_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_command_aiding_source* self) +{ + uint8_t tmp = 0; + extract_u8(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_filter_write_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source source) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_pitch_roll_aiding_command_aiding_source(&serializer, source); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source* source_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_pitch_roll_aiding_command_aiding_source(&deserializer, source_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_pitch_roll_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_pitch_roll_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_pitch_roll_aiding(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_filter_auto_zupt_command(mip_serializer* serializer, const mip_filter_auto_zupt_command* self) { insert_mip_function_selector(serializer, self->function); @@ -2418,27 +2434,1001 @@ void extract_mip_filter_auto_angular_zupt_command(mip_serializer* serializer, mi { extract_u8(serializer, &self->enable); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->threshold); + + } +} + +void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +{ + insert_u8(serializer, self->enable); + + insert_float(serializer, self->threshold); + +} +void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +{ + extract_u8(serializer, &self->enable); + + extract_float(serializer, &self->threshold); + +} + +mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_u8(&serializer, enable); + + insert_float(&serializer, threshold); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(enable_out); + extract_u8(&deserializer, enable_out); + + assert(threshold_out); + extract_float(&deserializer, threshold_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); +} +mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) +{ + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert_mip_filter_mag_capture_auto_cal_command(mip_serializer* serializer, const mip_filter_mag_capture_auto_cal_command* self) +{ + insert_mip_function_selector(serializer, self->function); + +} +void extract_mip_filter_mag_capture_auto_cal_command(mip_serializer* serializer, mip_filter_mag_capture_auto_cal_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + +} + +mip_cmd_result mip_filter_write_mag_capture_auto_cal(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_save_mag_capture_auto_cal(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_gravity_noise_command(mip_serializer* serializer, const mip_filter_gravity_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_gravity_noise_command(mip_serializer* serializer, mip_filter_gravity_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_gravity_noise_response(mip_serializer* serializer, const mip_filter_gravity_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_gravity_noise_response(mip_serializer* serializer, mip_filter_gravity_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_GRAVITY_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_pressure_altitude_noise_command(mip_serializer* serializer, const mip_filter_pressure_altitude_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->noise); + + } +} +void extract_mip_filter_pressure_altitude_noise_command(mip_serializer* serializer, mip_filter_pressure_altitude_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->noise); + + } +} + +void insert_mip_filter_pressure_altitude_noise_response(mip_serializer* serializer, const mip_filter_pressure_altitude_noise_response* self) +{ + insert_float(serializer, self->noise); + +} +void extract_mip_filter_pressure_altitude_noise_response(mip_serializer* serializer, mip_filter_pressure_altitude_noise_response* self) +{ + extract_float(serializer, &self->noise); + +} + +mip_cmd_result mip_filter_write_pressure_altitude_noise(struct mip_interface* device, float noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, noise); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out); + extract_float(&deserializer, noise_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_soft_iron_matrix_noise_command(mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 9; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_soft_iron_matrix_noise_command(mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 9; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_soft_iron_matrix_noise_response(mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_response* self) +{ + for(unsigned int i=0; i < 9; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_soft_iron_matrix_noise_response(mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_response* self) +{ + for(unsigned int i=0; i < 9; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_mag_noise_command(mip_serializer* serializer, const mip_filter_mag_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_mag_noise_command(mip_serializer* serializer, mip_filter_mag_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_mag_noise_response(mip_serializer* serializer, const mip_filter_mag_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_mag_noise_response(mip_serializer* serializer, mip_filter_mag_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, const float* noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAG_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract_float(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_inclination_source_command(mip_serializer* serializer, const mip_filter_inclination_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->inclination); + + } +} +void extract_mip_filter_inclination_source_command(mip_serializer* serializer, mip_filter_inclination_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->inclination); + + } +} + +void insert_mip_filter_inclination_source_response(mip_serializer* serializer, const mip_filter_inclination_source_response* self) +{ + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->inclination); + +} +void extract_mip_filter_inclination_source_response(mip_serializer* serializer, mip_filter_inclination_source_response* self) +{ + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->inclination); + +} + +mip_cmd_result mip_filter_write_inclination_source(struct mip_interface* device, mip_filter_mag_param_source source, float inclination) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_mag_param_source(&serializer, source); + + insert_float(&serializer, inclination); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_inclination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* inclination_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_INCLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); + + assert(inclination_out); + extract_float(&deserializer, inclination_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->declination); + + } +} +void extract_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->declination); + + } +} + +void insert_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self) +{ + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->declination); + +} +void extract_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self) +{ + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->declination); + +} + +mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source source, float declination) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_mag_param_source(&serializer, source); + + insert_float(&serializer, declination); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* declination_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_DECLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); + + assert(declination_out); + extract_float(&deserializer, declination_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_mag_field_magnitude_source_command(mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->magnitude); + + } +} +void extract_mip_filter_mag_field_magnitude_source_command(mip_serializer* serializer, mip_filter_mag_field_magnitude_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->magnitude); } } -void insert_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, const mip_filter_auto_angular_zupt_response* self) +void insert_mip_filter_mag_field_magnitude_source_response(mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_response* self) { - insert_u8(serializer, self->enable); + insert_mip_filter_mag_param_source(serializer, self->source); - insert_float(serializer, self->threshold); + insert_float(serializer, self->magnitude); } -void extract_mip_filter_auto_angular_zupt_response(mip_serializer* serializer, mip_filter_auto_angular_zupt_response* self) +void extract_mip_filter_mag_field_magnitude_source_response(mip_serializer* serializer, mip_filter_mag_field_magnitude_source_response* self) { - extract_u8(serializer, &self->enable); + extract_mip_filter_mag_param_source(serializer, &self->source); - extract_float(serializer, &self->threshold); + extract_float(serializer, &self->magnitude); } -mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, uint8_t enable, float threshold) +mip_cmd_result mip_filter_write_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source source, float magnitude) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2446,15 +3436,15 @@ mip_cmd_result mip_filter_write_auto_angular_zupt(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_u8(&serializer, enable); + insert_mip_filter_mag_param_source(&serializer, source); - insert_float(&serializer, threshold); + insert_float(&serializer, magnitude); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, uint8_t* enable_out, float* threshold_out) +mip_cmd_result mip_filter_read_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* magnitude_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2465,25 +3455,25 @@ mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, u assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(enable_out); - extract_u8(&deserializer, enable_out); + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); - assert(threshold_out); - extract_float(&deserializer, threshold_out); + assert(magnitude_out); + extract_float(&deserializer, magnitude_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_save_mag_field_magnitude_source(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2493,9 +3483,9 @@ mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_load_mag_field_magnitude_source(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2505,9 +3495,9 @@ mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device) assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device) +mip_cmd_result mip_filter_default_mag_field_magnitude_source(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2517,15 +3507,7 @@ mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ZUPT, NULL, 0); -} -mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) -{ - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert_mip_filter_reference_position_command(mip_serializer* serializer, const mip_filter_reference_position_command* self) { @@ -4780,261 +5762,6 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_GNSS_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self) -{ - insert_mip_function_selector(serializer, self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); - - } -} -void extract_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self) -{ - extract_mip_function_selector(serializer, &self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); - - } -} - -void insert_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self) -{ - insert_float(serializer, self->x); - - insert_float(serializer, self->y); - - insert_float(serializer, self->z); - -} -void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self) -{ - extract_float(serializer, &self->x); - - extract_float(serializer, &self->y); - - extract_float(serializer, &self->z); - -} - -mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - - insert_float(&serializer, x); - - insert_float(&serializer, y); - - insert_float(&serializer, z); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - - assert(mip_serializer_is_ok(&serializer)); - - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(x_out); - extract_float(&deserializer, x_out); - - assert(y_out); - extract_float(&deserializer, y_out); - - assert(z_out); - extract_float(&deserializer, z_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self) -{ - insert_mip_function_selector(serializer, self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - insert_mip_filter_mag_declination_source(serializer, self->source); - - insert_float(serializer, self->declination); - - } -} -void extract_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self) -{ - extract_mip_function_selector(serializer, &self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - extract_mip_filter_mag_declination_source(serializer, &self->source); - - extract_float(serializer, &self->declination); - - } -} - -void insert_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self) -{ - insert_mip_filter_mag_declination_source(serializer, self->source); - - insert_float(serializer, self->declination); - -} -void extract_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self) -{ - extract_mip_filter_mag_declination_source(serializer, &self->source); - - extract_float(serializer, &self->declination); - -} - -mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source source, float declination) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - - insert_mip_filter_mag_declination_source(&serializer, source); - - insert_float(&serializer, declination); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source* source_out, float* declination_out) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - - assert(mip_serializer_is_ok(&serializer)); - - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_DECLINATION_SOURCE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(source_out); - extract_mip_filter_mag_declination_source(&deserializer, source_out); - - assert(declination_out); - extract_float(&deserializer, declination_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert_mip_filter_set_initial_heading_command(mip_serializer* serializer, const mip_filter_set_initial_heading_command* self) { insert_float(serializer, self->heading); diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 7048f63d0..ec4f673ad 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -1361,11 +1361,8 @@ void insert(Serializer& serializer, const AccelNoise& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1375,51 +1372,40 @@ void extract(Serializer& serializer, AccelNoise& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const AccelNoise::Response& self) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, AccelNoise::Response& self) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z) +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, x); - - insert(serializer, y); - - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1434,14 +1420,9 @@ CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, flo { Serializer deserializer(buffer, responseLength); - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); - - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1484,11 +1465,8 @@ void insert(Serializer& serializer, const GyroNoise& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1498,51 +1476,40 @@ void extract(Serializer& serializer, GyroNoise& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const GyroNoise::Response& self) { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, GyroNoise::Response& self) { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z) +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, x); - - insert(serializer, y); - - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1557,14 +1524,9 @@ CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, floa { Serializer deserializer(buffer, responseLength); - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); - - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1607,17 +1569,11 @@ void insert(Serializer& serializer, const AccelBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1627,75 +1583,53 @@ void extract(Serializer& serializer, AccelBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const AccelBiasModel::Response& self) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, AccelBiasModel::Response& self) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, xBeta); - - insert(serializer, yBeta); - - insert(serializer, zBeta); - - insert(serializer, x); - - insert(serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, beta[i]); - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1710,23 +1644,13 @@ CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* y { Serializer deserializer(buffer, responseLength); - assert(xBetaOut); - extract(deserializer, *xBetaOut); - - assert(yBetaOut); - extract(deserializer, *yBetaOut); - - assert(zBetaOut); - extract(deserializer, *zBetaOut); - - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); + assert(betaOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, betaOut[i]); - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1769,17 +1693,11 @@ void insert(Serializer& serializer, const GyroBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } } @@ -1789,75 +1707,53 @@ void extract(Serializer& serializer, GyroBiasModel& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } } void insert(Serializer& serializer, const GyroBiasModel::Response& self) { - insert(serializer, self.x_beta); - - insert(serializer, self.y_beta); - - insert(serializer, self.z_beta); - - insert(serializer, self.x); - - insert(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.beta[i]); - insert(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); } void extract(Serializer& serializer, GyroBiasModel::Response& self) { - extract(serializer, self.x_beta); - - extract(serializer, self.y_beta); - - extract(serializer, self.z_beta); - - extract(serializer, self.x); - - extract(serializer, self.y); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.beta[i]); - extract(serializer, self.z); + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); } -CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z) +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, xBeta); - - insert(serializer, yBeta); - - insert(serializer, zBeta); - - insert(serializer, x); - - insert(serializer, y); + assert(beta || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, beta[i]); - insert(serializer, z); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut) +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1872,23 +1768,13 @@ CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yB { Serializer deserializer(buffer, responseLength); - assert(xBetaOut); - extract(deserializer, *xBetaOut); - - assert(yBetaOut); - extract(deserializer, *yBetaOut); - - assert(zBetaOut); - extract(deserializer, *zBetaOut); - - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); + assert(betaOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, betaOut[i]); - assert(zOut); - extract(deserializer, *zOut); + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -1931,7 +1817,7 @@ void insert(Serializer& serializer, const AltitudeAiding& self) if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.aiding_selector); + insert(serializer, self.selector); } } @@ -1941,35 +1827,35 @@ void extract(Serializer& serializer, AltitudeAiding& self) if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.aiding_selector); + extract(serializer, self.selector); } } void insert(Serializer& serializer, const AltitudeAiding::Response& self) { - insert(serializer, self.aiding_selector); + insert(serializer, self.selector); } void extract(Serializer& serializer, AltitudeAiding::Response& self) { - extract(serializer, self.aiding_selector); + extract(serializer, self.selector); } -CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector) +CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, aidingSelector); + insert(serializer, selector); assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut) +CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1984,8 +1870,8 @@ CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOu { Serializer deserializer(buffer, responseLength); - assert(aidingSelectorOut); - extract(deserializer, *aidingSelectorOut); + assert(selectorOut); + extract(deserializer, *selectorOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; @@ -2022,6 +1908,103 @@ CmdResult defaultAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const PitchRollAiding& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + } +} +void extract(Serializer& serializer, PitchRollAiding& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + } +} + +void insert(Serializer& serializer, const PitchRollAiding::Response& self) +{ + insert(serializer, self.source); + +} +void extract(Serializer& serializer, PitchRollAiding::Response& self) +{ + extract(serializer, self.source); + +} + +CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult savePitchRollAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadPitchRollAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultPitchRollAiding(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const AutoZupt& self) { insert(serializer, self.function); @@ -2178,15 +2161,919 @@ CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float t Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - insert(serializer, enable); + insert(serializer, enable); + + insert(serializer, threshold); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(enableOut); + extract(deserializer, *enableOut); + + assert(thresholdOut); + extract(deserializer, *thresholdOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultAutoAngularZupt(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const CommandedZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} +void extract(Serializer& serializer, CommandedAngularZupt& self) +{ + (void)serializer; + (void)self; +} + +CmdResult commandedAngularZupt(C::mip_interface& device) +{ + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); +} +void insert(Serializer& serializer, const MagCaptureAutoCal& self) +{ + insert(serializer, self.function); + +} +void extract(Serializer& serializer, MagCaptureAutoCal& self) +{ + extract(serializer, self.function); + +} + +CmdResult writeMagCaptureAutoCal(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult saveMagCaptureAutoCal(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const GravityNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, GravityNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const GravityNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, GravityNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const PressureAltitudeNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.noise); + + } +} +void extract(Serializer& serializer, PressureAltitudeNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.noise); + + } +} + +void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self) +{ + insert(serializer, self.noise); + +} +void extract(Serializer& serializer, PressureAltitudeNoise::Response& self) +{ + extract(serializer, self.noise); + +} + +CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, noise); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), CMD_PRESSURE_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut); + extract(deserializer, *noiseOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult savePressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadPressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultPressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const HardIronOffsetNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, HardIronOffsetNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const SoftIronMatrixNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 9; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, SoftIronMatrixNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 9; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self) +{ + for(unsigned int i=0; i < 9; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self) +{ + for(unsigned int i=0; i < 9; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (9 == 0)); + for(unsigned int i=0; i < 9; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, MagNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const MagNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, MagNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeMagNoise(C::mip_interface& device, const float* noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const InclinationSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.inclination); + + } +} +void extract(Serializer& serializer, InclinationSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.inclination); + + } +} + +void insert(Serializer& serializer, const InclinationSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.inclination); + +} +void extract(Serializer& serializer, InclinationSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.inclination); + +} + +CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + insert(serializer, inclination); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + assert(inclinationOut); + extract(deserializer, *inclinationOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagneticDeclinationSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.declination); + + } +} +void extract(Serializer& serializer, MagneticDeclinationSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.declination); + + } +} + +void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.declination); + +} +void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.declination); + +} + +CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + insert(serializer, declination); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + assert(declinationOut); + extract(deserializer, *declinationOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagFieldMagnitudeSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.magnitude); + + } +} +void extract(Serializer& serializer, MagFieldMagnitudeSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.magnitude); + + } +} + +void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.magnitude); + +} +void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.magnitude); + +} + +CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); - insert(serializer, threshold); + insert(serializer, magnitude); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2195,24 +3082,24 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { Serializer deserializer(buffer, responseLength); - assert(enableOut); - extract(deserializer, *enableOut); + assert(sourceOut); + extract(deserializer, *sourceOut); - assert(thresholdOut); - extract(deserializer, *thresholdOut); + assert(magnitudeOut); + extract(deserializer, *magnitudeOut); if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveAutoAngularZupt(C::mip_interface& device) +CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2220,9 +3107,9 @@ CmdResult saveAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::SAVE); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoAngularZupt(C::mip_interface& device) +CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2230,9 +3117,9 @@ CmdResult loadAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::LOAD); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoAngularZupt(C::mip_interface& device) +CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2240,37 +3127,7 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device) insert(serializer, FunctionSelector::RESET); assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert(Serializer& serializer, const CommandedZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); -} -void insert(Serializer& serializer, const CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} -void extract(Serializer& serializer, CommandedAngularZupt& self) -{ - (void)serializer; - (void)self; -} - -CmdResult commandedAngularZupt(C::mip_interface& device) -{ - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert(Serializer& serializer, const ReferencePosition& self) { @@ -4326,239 +5183,6 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const HardIronOffsetNoise& self) -{ - insert(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); - - } -} -void extract(Serializer& serializer, HardIronOffsetNoise& self) -{ - extract(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); - - } -} - -void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self) -{ - insert(serializer, self.x); - - insert(serializer, self.y); - - insert(serializer, self.z); - -} -void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) -{ - extract(serializer, self.x); - - extract(serializer, self.y); - - extract(serializer, self.z); - -} - -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::WRITE); - insert(serializer, x); - - insert(serializer, y); - - insert(serializer, z); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::READ); - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(xOut); - extract(deserializer, *xOut); - - assert(yOut); - extract(deserializer, *yOut); - - assert(zOut); - extract(deserializer, *zOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -CmdResult saveHardIronOffsetNoise(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::SAVE); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult loadHardIronOffsetNoise(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::LOAD); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::RESET); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -void insert(Serializer& serializer, const MagneticDeclinationSource& self) -{ - insert(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - insert(serializer, self.source); - - insert(serializer, self.declination); - - } -} -void extract(Serializer& serializer, MagneticDeclinationSource& self) -{ - extract(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - extract(serializer, self.source); - - extract(serializer, self.declination); - - } -} - -void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self) -{ - insert(serializer, self.source); - - insert(serializer, self.declination); - -} -void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) -{ - extract(serializer, self.source); - - extract(serializer, self.declination); - -} - -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource source, float declination) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::WRITE); - insert(serializer, source); - - insert(serializer, declination); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource* sourceOut, float* declinationOut) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::READ); - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(sourceOut); - extract(deserializer, *sourceOut); - - assert(declinationOut); - extract(deserializer, *declinationOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -CmdResult saveMagneticDeclinationSource(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::SAVE); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult loadMagneticDeclinationSource(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::LOAD); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::RESET); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert(Serializer& serializer, const SetInitialHeading& self) { insert(serializer, self.heading); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index b941c6b77..43baa0bb4 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -162,13 +162,13 @@ static const mip_filter_reference_frame MIP_FILTER_REFERENCE_FRAME_LLH = 2; /// void insert_mip_filter_reference_frame(struct mip_serializer* serializer, const mip_filter_reference_frame self); void extract_mip_filter_reference_frame(struct mip_serializer* serializer, mip_filter_reference_frame* self); -typedef uint8_t mip_filter_mag_declination_source; -static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE_NONE = 1; ///< Magnetic field is assumed to have an declination angle equal to zero. -static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE_WMM = 2; ///< Magnetic field is assumed to conform to the World Magnetic Model, calculated using current location estimate as an input to the model. -static const mip_filter_mag_declination_source MIP_FILTER_MAG_DECLINATION_SOURCE_MANUAL = 3; ///< Magnetic field is assumed to have the declination angle specified by the user. +typedef uint8_t mip_filter_mag_param_source; +static const mip_filter_mag_param_source MIP_FILTER_MAG_PARAM_SOURCE_NONE = 1; ///< No source. See command documentation for default behavior +static const mip_filter_mag_param_source MIP_FILTER_MAG_PARAM_SOURCE_WMM = 2; ///< Magnetic field is assumed to conform to the World Magnetic Model, calculated using current location estimate as an input to the model. +static const mip_filter_mag_param_source MIP_FILTER_MAG_PARAM_SOURCE_MANUAL = 3; ///< Magnetic field is assumed to have the parameter specified by the user. -void insert_mip_filter_mag_declination_source(struct mip_serializer* serializer, const mip_filter_mag_declination_source self); -void extract_mip_filter_mag_declination_source(struct mip_serializer* serializer, mip_filter_mag_declination_source* self); +void insert_mip_filter_mag_param_source(struct mip_serializer* serializer, const mip_filter_mag_param_source self); +void extract_mip_filter_mag_param_source(struct mip_serializer* serializer, mip_filter_mag_param_source* self); typedef uint8_t mip_filter_adaptive_measurement; static const mip_filter_adaptive_measurement MIP_FILTER_ADAPTIVE_MEASUREMENT_DISABLED = 0; ///< No adaptive measurement @@ -869,19 +869,16 @@ mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device /// /// Each of the noise values must be greater than 0.0. /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ struct mip_filter_accel_noise_command { mip_function_selector function; - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_noise_command mip_filter_accel_noise_command; @@ -890,17 +887,15 @@ void extract_mip_filter_accel_noise_command(struct mip_serializer* serializer, m struct mip_filter_accel_noise_response { - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_noise_response mip_filter_accel_noise_response; void insert_mip_filter_accel_noise_response(struct mip_serializer* serializer, const mip_filter_accel_noise_response* self); void extract_mip_filter_accel_noise_response(struct mip_serializer* serializer, mip_filter_accel_noise_response* self); -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, float x, float y, float z); -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); @@ -912,19 +907,16 @@ mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); /// /// Each of the noise values must be greater than 0.0 /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ struct mip_filter_gyro_noise_command { mip_function_selector function; - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_noise_command mip_filter_gyro_noise_command; @@ -933,17 +925,15 @@ void extract_mip_filter_gyro_noise_command(struct mip_serializer* serializer, mi struct mip_filter_gyro_noise_response { - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_noise_response mip_filter_gyro_noise_response; void insert_mip_filter_gyro_noise_response(struct mip_serializer* serializer, const mip_filter_gyro_noise_response* self); void extract_mip_filter_gyro_noise_response(struct mip_serializer* serializer, mip_filter_gyro_noise_response* self); -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, float x, float y, float z); -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); @@ -953,7 +943,7 @@ mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); ///@defgroup c_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [C] /// Accelerometer Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -961,12 +951,8 @@ mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); struct mip_filter_accel_bias_model_command { mip_function_selector function; - float x_beta; ///< Accel Bias Beta [1/second] - float y_beta; ///< Accel Bias Beta [1/second] - float z_beta; ///< Accel Bias Beta [1/second] - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3]; ///< Accel Bias Beta [1/second] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_bias_model_command mip_filter_accel_bias_model_command; @@ -975,20 +961,16 @@ void extract_mip_filter_accel_bias_model_command(struct mip_serializer* serializ struct mip_filter_accel_bias_model_response { - float x_beta; ///< Accel Bias Beta [1/second] - float y_beta; ///< Accel Bias Beta [1/second] - float z_beta; ///< Accel Bias Beta [1/second] - float x; ///< Accel Noise 1-sigma [meters/second^2] - float y; ///< Accel Noise 1-sigma [meters/second^2] - float z; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3]; ///< Accel Bias Beta [1/second] + float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_bias_model_response mip_filter_accel_bias_model_response; void insert_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, const mip_filter_accel_bias_model_response* self); void extract_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, mip_filter_accel_bias_model_response* self); -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise); +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device); @@ -998,7 +980,7 @@ mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device) ///@defgroup c_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [C] /// Gyroscope Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -1006,12 +988,8 @@ mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device) struct mip_filter_gyro_bias_model_command { mip_function_selector function; - float x_beta; ///< Gyro Bias Beta [1/second] - float y_beta; ///< Gyro Bias Beta [1/second] - float z_beta; ///< Gyro Bias Beta [1/second] - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3]; ///< Gyro Bias Beta [1/second] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_bias_model_command mip_filter_gyro_bias_model_command; @@ -1020,20 +998,16 @@ void extract_mip_filter_gyro_bias_model_command(struct mip_serializer* serialize struct mip_filter_gyro_bias_model_response { - float x_beta; ///< Gyro Bias Beta [1/second] - float y_beta; ///< Gyro Bias Beta [1/second] - float z_beta; ///< Gyro Bias Beta [1/second] - float x; ///< Gyro Noise 1-sigma [meters/second^2] - float y; ///< Gyro Noise 1-sigma [meters/second^2] - float z; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3]; ///< Gyro Bias Beta [1/second] + float noise[3]; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_bias_model_response mip_filter_gyro_bias_model_response; void insert_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self); void extract_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, mip_filter_gyro_bias_model_response* self); -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, float x_beta, float y_beta, float z_beta, float x, float y, float z); -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* x_beta_out, float* y_beta_out, float* z_beta_out, float* x_out, float* y_out, float* z_out); +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise); +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); @@ -1041,50 +1015,89 @@ mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [C] -/// Altitude Aiding Control -/// /// Select altitude input for absolute altitude and/or vertical velocity. The primary altitude reading is always GNSS. -/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during GNSS outages. +/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during outages. /// -/// Possible altitude aiding selector values: -/// -/// 0x00 - No altitude aiding (disable) -/// 0x01 - Enable pressure sensor aiding(1) -/// -/// 1. Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. +/// Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. /// /// ///@{ +typedef uint8_t mip_filter_altitude_aiding_command_aiding_selector; +static const mip_filter_altitude_aiding_command_aiding_selector MIP_FILTER_ALTITUDE_AIDING_COMMAND_AIDING_SELECTOR_NONE = 0; ///< No altitude aiding +static const mip_filter_altitude_aiding_command_aiding_selector MIP_FILTER_ALTITUDE_AIDING_COMMAND_AIDING_SELECTOR_PRESURE = 1; ///< Enable pressure sensor aiding + struct mip_filter_altitude_aiding_command { mip_function_selector function; - uint8_t aiding_selector; ///< See above + mip_filter_altitude_aiding_command_aiding_selector selector; ///< See above }; typedef struct mip_filter_altitude_aiding_command mip_filter_altitude_aiding_command; void insert_mip_filter_altitude_aiding_command(struct mip_serializer* serializer, const mip_filter_altitude_aiding_command* self); void extract_mip_filter_altitude_aiding_command(struct mip_serializer* serializer, mip_filter_altitude_aiding_command* self); +void insert_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, const mip_filter_altitude_aiding_command_aiding_selector self); +void extract_mip_filter_altitude_aiding_command_aiding_selector(struct mip_serializer* serializer, mip_filter_altitude_aiding_command_aiding_selector* self); + struct mip_filter_altitude_aiding_response { - uint8_t aiding_selector; ///< See above + mip_filter_altitude_aiding_command_aiding_selector selector; ///< See above }; typedef struct mip_filter_altitude_aiding_response mip_filter_altitude_aiding_response; void insert_mip_filter_altitude_aiding_response(struct mip_serializer* serializer, const mip_filter_altitude_aiding_response* self); void extract_mip_filter_altitude_aiding_response(struct mip_serializer* serializer, mip_filter_altitude_aiding_response* self); -mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, uint8_t aiding_selector); -mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, uint8_t* aiding_selector_out); +mip_cmd_result mip_filter_write_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector selector); +mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, mip_filter_altitude_aiding_command_aiding_selector* selector_out); mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device); mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device); mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_pitch_roll_aiding (0x0D,0x4B) Pitch Roll Aiding [C] +/// Select pitch/roll aiding input. Pitch/roll reading is always derived from GNSS corrected inertial solution. +/// Aiding inputs are used to improve that solution during periods of low dynamics and GNSS outages. +/// +///@{ + +typedef uint8_t mip_filter_pitch_roll_aiding_command_aiding_source; +static const mip_filter_pitch_roll_aiding_command_aiding_source MIP_FILTER_PITCH_ROLL_AIDING_COMMAND_AIDING_SOURCE_NONE = 0; ///< No pitch/roll aiding +static const mip_filter_pitch_roll_aiding_command_aiding_source MIP_FILTER_PITCH_ROLL_AIDING_COMMAND_AIDING_SOURCE_GRAVITY_VEC = 1; ///< Enable gravity vector aiding + +struct mip_filter_pitch_roll_aiding_command +{ + mip_function_selector function; + mip_filter_pitch_roll_aiding_command_aiding_source source; ///< Controls the aiding source + +}; +typedef struct mip_filter_pitch_roll_aiding_command mip_filter_pitch_roll_aiding_command; +void insert_mip_filter_pitch_roll_aiding_command(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command* self); +void extract_mip_filter_pitch_roll_aiding_command(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_command* self); + +void insert_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_command_aiding_source self); +void extract_mip_filter_pitch_roll_aiding_command_aiding_source(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_command_aiding_source* self); + +struct mip_filter_pitch_roll_aiding_response +{ + mip_filter_pitch_roll_aiding_command_aiding_source source; ///< Controls the aiding source + +}; +typedef struct mip_filter_pitch_roll_aiding_response mip_filter_pitch_roll_aiding_response; +void insert_mip_filter_pitch_roll_aiding_response(struct mip_serializer* serializer, const mip_filter_pitch_roll_aiding_response* self); +void extract_mip_filter_pitch_roll_aiding_response(struct mip_serializer* serializer, mip_filter_pitch_roll_aiding_response* self); + +mip_cmd_result mip_filter_write_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source source); +mip_cmd_result mip_filter_read_pitch_roll_aiding(struct mip_interface* device, mip_filter_pitch_roll_aiding_command_aiding_source* source_out); +mip_cmd_result mip_filter_save_pitch_roll_aiding(struct mip_interface* device); +mip_cmd_result mip_filter_load_pitch_roll_aiding(struct mip_interface* device); +mip_cmd_result mip_filter_default_pitch_roll_aiding(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_auto_zupt (0x0D,0x1E) Auto Zupt [C] -/// Zero Velocity Update /// The ZUPT is triggered when the scalar magnitude of the GNSS reported velocity vector is equal-to or less than the threshold value. /// The device will NACK threshold values that are less than zero (i.e.negative.) /// @@ -1156,7 +1169,6 @@ mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_commanded_zupt (0x0D,0x22) Commanded Zupt [C] -/// Commanded Zero Velocity Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1166,7 +1178,6 @@ mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_commanded_angular_zupt (0x0D,0x23) Commanded Angular Zupt [C] -/// Commanded Zero Angular Rate Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1175,6 +1186,329 @@ mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_capture_auto_cal (0x0D,0x27) Mag Capture Auto Cal [C] +/// This command captures the current value of the auto-calibration, applies it to the current fixed hard and soft iron calibration coefficients, and replaces the current fixed hard and soft iron calibration coefficients with the new values. +/// This may be used in place of (or in addition to) a manual hard and soft iron calibration utility. This command also resets the auto-calibration coefficients. +/// Function selector SAVE is the same as issuing the 0x0C, 0x3A and 0x0C, 0x3B commands with the SAVE function selector. +/// +///@{ + +struct mip_filter_mag_capture_auto_cal_command +{ + mip_function_selector function; + +}; +typedef struct mip_filter_mag_capture_auto_cal_command mip_filter_mag_capture_auto_cal_command; +void insert_mip_filter_mag_capture_auto_cal_command(struct mip_serializer* serializer, const mip_filter_mag_capture_auto_cal_command* self); +void extract_mip_filter_mag_capture_auto_cal_command(struct mip_serializer* serializer, mip_filter_mag_capture_auto_cal_command* self); + +mip_cmd_result mip_filter_write_mag_capture_auto_cal(struct mip_interface* device); +mip_cmd_result mip_filter_save_mag_capture_auto_cal(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_gravity_noise (0x0D,0x28) Gravity Noise [C] +/// Set the expected gravity noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// Note: Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. Changing this value modifies how the filter responds to dynamic input and can be used to tune filter performance. +/// Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_gravity_noise_command +{ + mip_function_selector function; + float noise[3]; ///< Gravity Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_gravity_noise_command mip_filter_gravity_noise_command; +void insert_mip_filter_gravity_noise_command(struct mip_serializer* serializer, const mip_filter_gravity_noise_command* self); +void extract_mip_filter_gravity_noise_command(struct mip_serializer* serializer, mip_filter_gravity_noise_command* self); + +struct mip_filter_gravity_noise_response +{ + float noise[3]; ///< Gravity Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_gravity_noise_response mip_filter_gravity_noise_response; +void insert_mip_filter_gravity_noise_response(struct mip_serializer* serializer, const mip_filter_gravity_noise_response* self); +void extract_mip_filter_gravity_noise_response(struct mip_serializer* serializer, mip_filter_gravity_noise_response* self); + +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_gravity_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_gravity_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_gravity_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_pressure_altitude_noise (0x0D,0x29) Pressure Altitude Noise [C] +/// Set the expected pressure altitude noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// The noise value must be greater than 0.0 +/// +/// This noise value represents pressure altitude model noise in the Estimation Filter. +/// A lower value will increase responsiveness of the sensor to pressure changes, however height estimates will be more susceptible to error from air pressure fluctuations not due to changes in altitude. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_pressure_altitude_noise_command +{ + mip_function_selector function; + float noise; ///< Pressure Altitude Noise 1-sigma [m] + +}; +typedef struct mip_filter_pressure_altitude_noise_command mip_filter_pressure_altitude_noise_command; +void insert_mip_filter_pressure_altitude_noise_command(struct mip_serializer* serializer, const mip_filter_pressure_altitude_noise_command* self); +void extract_mip_filter_pressure_altitude_noise_command(struct mip_serializer* serializer, mip_filter_pressure_altitude_noise_command* self); + +struct mip_filter_pressure_altitude_noise_response +{ + float noise; ///< Pressure Altitude Noise 1-sigma [m] + +}; +typedef struct mip_filter_pressure_altitude_noise_response mip_filter_pressure_altitude_noise_response; +void insert_mip_filter_pressure_altitude_noise_response(struct mip_serializer* serializer, const mip_filter_pressure_altitude_noise_response* self); +void extract_mip_filter_pressure_altitude_noise_response(struct mip_serializer* serializer, mip_filter_pressure_altitude_noise_response* self); + +mip_cmd_result mip_filter_write_pressure_altitude_noise(struct mip_interface* device, float noise); +mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_pressure_altitude_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_pressure_altitude_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_pressure_altitude_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [C] +/// Set the expected hard iron offset noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise values represent process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_hard_iron_offset_noise_command +{ + mip_function_selector function; + float noise[3]; ///< Hard Iron Offset Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_command mip_filter_hard_iron_offset_noise_command; +void insert_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self); +void extract_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self); + +struct mip_filter_hard_iron_offset_noise_response +{ + float noise[3]; ///< Hard Iron Offset Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_offset_noise_response; +void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); +void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_soft_iron_matrix_noise (0x0D,0x2C) Soft Iron Matrix Noise [C] +/// Set the expected soft iron matrix noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct mip_filter_soft_iron_matrix_noise_command +{ + mip_function_selector function; + float noise[9]; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + +}; +typedef struct mip_filter_soft_iron_matrix_noise_command mip_filter_soft_iron_matrix_noise_command; +void insert_mip_filter_soft_iron_matrix_noise_command(struct mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_command* self); +void extract_mip_filter_soft_iron_matrix_noise_command(struct mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_command* self); + +struct mip_filter_soft_iron_matrix_noise_response +{ + float noise[9]; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + +}; +typedef struct mip_filter_soft_iron_matrix_noise_response mip_filter_soft_iron_matrix_noise_response; +void insert_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_response* self); +void extract_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_response* self); + +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_soft_iron_matrix_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_soft_iron_matrix_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_noise (0x0D,0x42) Mag Noise [C] +/// Set the expected magnetometer noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 (gauss) +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions +/// +///@{ + +struct mip_filter_mag_noise_command +{ + mip_function_selector function; + float noise[3]; ///< Mag Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_mag_noise_command mip_filter_mag_noise_command; +void insert_mip_filter_mag_noise_command(struct mip_serializer* serializer, const mip_filter_mag_noise_command* self); +void extract_mip_filter_mag_noise_command(struct mip_serializer* serializer, mip_filter_mag_noise_command* self); + +struct mip_filter_mag_noise_response +{ + float noise[3]; ///< Mag Noise 1-sigma [gauss] + +}; +typedef struct mip_filter_mag_noise_response mip_filter_mag_noise_response; +void insert_mip_filter_mag_noise_response(struct mip_serializer* serializer, const mip_filter_mag_noise_response* self); +void extract_mip_filter_mag_noise_response(struct mip_serializer* serializer, mip_filter_mag_noise_response* self); + +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_save_mag_noise(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_noise(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_noise(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_inclination_source (0x0D,0x4C) Inclination Source [C] +/// Set/Get the local magnetic field inclination angle source. +/// +/// This can be used to correct for the local value of inclination (dip angle) of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct mip_filter_inclination_source_command +{ + mip_function_selector function; + mip_filter_mag_param_source source; ///< Inclination Source + float inclination; ///< Inclination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_inclination_source_command mip_filter_inclination_source_command; +void insert_mip_filter_inclination_source_command(struct mip_serializer* serializer, const mip_filter_inclination_source_command* self); +void extract_mip_filter_inclination_source_command(struct mip_serializer* serializer, mip_filter_inclination_source_command* self); + +struct mip_filter_inclination_source_response +{ + mip_filter_mag_param_source source; ///< Inclination Source + float inclination; ///< Inclination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_inclination_source_response mip_filter_inclination_source_response; +void insert_mip_filter_inclination_source_response(struct mip_serializer* serializer, const mip_filter_inclination_source_response* self); +void extract_mip_filter_inclination_source_response(struct mip_serializer* serializer, mip_filter_inclination_source_response* self); + +mip_cmd_result mip_filter_write_inclination_source(struct mip_interface* device, mip_filter_mag_param_source source, float inclination); +mip_cmd_result mip_filter_read_inclination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* inclination_out); +mip_cmd_result mip_filter_save_inclination_source(struct mip_interface* device); +mip_cmd_result mip_filter_load_inclination_source(struct mip_interface* device); +mip_cmd_result mip_filter_default_inclination_source(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [C] +/// Set/Get the local magnetic field declination angle source. +/// +/// This can be used to correct for the local value of declination of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct mip_filter_magnetic_declination_source_command +{ + mip_function_selector function; + mip_filter_mag_param_source source; ///< Magnetic field declination angle source + float declination; ///< Declination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_magnetic_declination_source_command mip_filter_magnetic_declination_source_command; +void insert_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self); +void extract_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self); + +struct mip_filter_magnetic_declination_source_response +{ + mip_filter_mag_param_source source; ///< Magnetic field declination angle source + float declination; ///< Declination angle [radians] (only required if source = MANUAL) + +}; +typedef struct mip_filter_magnetic_declination_source_response mip_filter_magnetic_declination_source_response; +void insert_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self); +void extract_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self); + +mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source source, float declination); +mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* declination_out); +mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device); +mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device); +mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_mag_field_magnitude_source (0x0D,0x4D) Mag Field Magnitude Source [C] +/// Set/Get the local magnetic field magnitude source. +/// +/// This is used to specify the local magnitude of the earth's magnetic field. +/// Having a correct value for magnitude is important for best performance of the auto-mag calibration feature and for the magnetometer adaptive magnitude. If you do not have an accurate value for the local magnetic field magnitude, it is recommended that you leave the auto-mag calibration feature off. +/// +///@{ + +struct mip_filter_mag_field_magnitude_source_command +{ + mip_function_selector function; + mip_filter_mag_param_source source; ///< Magnetic Field Magnitude Source + float magnitude; ///< Magnitude [gauss] (only required if source = MANUAL) + +}; +typedef struct mip_filter_mag_field_magnitude_source_command mip_filter_mag_field_magnitude_source_command; +void insert_mip_filter_mag_field_magnitude_source_command(struct mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_command* self); +void extract_mip_filter_mag_field_magnitude_source_command(struct mip_serializer* serializer, mip_filter_mag_field_magnitude_source_command* self); + +struct mip_filter_mag_field_magnitude_source_response +{ + mip_filter_mag_param_source source; ///< Magnetic Field Magnitude Source + float magnitude; ///< Magnitude [gauss] (only required if source = MANUAL) + +}; +typedef struct mip_filter_mag_field_magnitude_source_response mip_filter_mag_field_magnitude_source_response; +void insert_mip_filter_mag_field_magnitude_source_response(struct mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_response* self); +void extract_mip_filter_mag_field_magnitude_source_response(struct mip_serializer* serializer, mip_filter_mag_field_magnitude_source_response* self); + +mip_cmd_result mip_filter_write_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source source, float magnitude); +mip_cmd_result mip_filter_read_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* magnitude_out); +mip_cmd_result mip_filter_save_mag_field_magnitude_source(struct mip_interface* device); +mip_cmd_result mip_filter_load_mag_field_magnitude_source(struct mip_interface* device); +mip_cmd_result mip_filter_default_mag_field_magnitude_source(struct mip_interface* device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_reference_position (0x0D,0x26) Reference Position [C] /// Set the Lat/Long/Alt reference position for the sensor. /// @@ -1871,85 +2205,6 @@ mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [C] -/// Set the expected hard iron offset noise 1-sigma values -/// -/// This function can be used to tune the filter performance in the target application. -/// -/// Each of the noise values must be greater than 0.0 -/// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. -/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. -/// Default values provide good performance for most laboratory conditions. -/// -/// -///@{ - -struct mip_filter_hard_iron_offset_noise_command -{ - mip_function_selector function; - float x; ///< HI Offset Noise 1-sima [gauss] - float y; ///< HI Offset Noise 1-sima [gauss] - float z; ///< HI Offset Noise 1-sima [gauss] - -}; -typedef struct mip_filter_hard_iron_offset_noise_command mip_filter_hard_iron_offset_noise_command; -void insert_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self); -void extract_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self); - -struct mip_filter_hard_iron_offset_noise_response -{ - float x; ///< HI Offset Noise 1-sima [gauss] - float y; ///< HI Offset Noise 1-sima [gauss] - float z; ///< HI Offset Noise 1-sima [gauss] - -}; -typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_offset_noise_response; -void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); -void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); - -mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, float x, float y, float z); -mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* x_out, float* y_out, float* z_out); -mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); -mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); -mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [C] -/// Source for magnetic declination angle, and user supplied value for manual selection. -/// -///@{ - -struct mip_filter_magnetic_declination_source_command -{ - mip_function_selector function; - mip_filter_mag_declination_source source; ///< Magnetic field declination angle source - float declination; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - -}; -typedef struct mip_filter_magnetic_declination_source_command mip_filter_magnetic_declination_source_command; -void insert_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self); -void extract_mip_filter_magnetic_declination_source_command(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self); - -struct mip_filter_magnetic_declination_source_response -{ - mip_filter_mag_declination_source source; ///< Magnetic field declination angle source - float declination; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - -}; -typedef struct mip_filter_magnetic_declination_source_response mip_filter_magnetic_declination_source_response; -void insert_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self); -void extract_mip_filter_magnetic_declination_source_response(struct mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self); - -mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source source, float declination); -mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_declination_source* source_out, float* declination_out); -mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device); -mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device); -mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// ///@defgroup c_filter_set_initial_heading (0x0D,0x03) Set Initial Heading [C] /// Set the initial heading angle. /// diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 2f88df9ed..0b090b55e 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -160,11 +160,11 @@ enum class FilterReferenceFrame : uint8_t LLH = 2, ///< WGS84 Latitude, longitude, and height above ellipsoid }; -enum class FilterMagDeclinationSource : uint8_t +enum class FilterMagParamSource : uint8_t { - NONE = 1, ///< Magnetic field is assumed to have an declination angle equal to zero. + NONE = 1, ///< No source. See command documentation for default behavior WMM = 2, ///< Magnetic field is assumed to conform to the World Magnetic Model, calculated using current location estimate as an input to the model. - MANUAL = 3, ///< Magnetic field is assumed to have the declination angle specified by the user. + MANUAL = 3, ///< Magnetic field is assumed to have the parameter specified by the user. }; enum class FilterAdaptiveMeasurement : uint8_t @@ -1050,10 +1050,9 @@ CmdResult defaultAutoInitControl(C::mip_interface& device); /// /// Each of the noise values must be greater than 0.0. /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ @@ -1069,18 +1068,14 @@ struct AccelNoise static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] }; }; @@ -1090,8 +1085,8 @@ void extract(Serializer& serializer, AccelNoise& self); void insert(Serializer& serializer, const AccelNoise::Response& self); void extract(Serializer& serializer, AccelNoise::Response& self); -CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise); +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut); CmdResult saveAccelNoise(C::mip_interface& device); CmdResult loadAccelNoise(C::mip_interface& device); CmdResult defaultAccelNoise(C::mip_interface& device); @@ -1103,10 +1098,9 @@ CmdResult defaultAccelNoise(C::mip_interface& device); /// /// Each of the noise values must be greater than 0.0 /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ @@ -1122,18 +1116,14 @@ struct GyroNoise static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] }; }; @@ -1143,8 +1133,8 @@ void extract(Serializer& serializer, GyroNoise& self); void insert(Serializer& serializer, const GyroNoise::Response& self); void extract(Serializer& serializer, GyroNoise::Response& self); -CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise); +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut); CmdResult saveGyroNoise(C::mip_interface& device); CmdResult loadGyroNoise(C::mip_interface& device); CmdResult defaultGyroNoise(C::mip_interface& device); @@ -1154,7 +1144,7 @@ CmdResult defaultGyroNoise(C::mip_interface& device); ///@defgroup cpp_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [CPP] /// Accelerometer Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -1171,24 +1161,16 @@ struct AccelBiasModel static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x_beta = 0; ///< Accel Bias Beta [1/second] - float y_beta = 0; ///< Accel Bias Beta [1/second] - float z_beta = 0; ///< Accel Bias Beta [1/second] - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Accel Bias Beta [1/second] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; - float x_beta = 0; ///< Accel Bias Beta [1/second] - float y_beta = 0; ///< Accel Bias Beta [1/second] - float z_beta = 0; ///< Accel Bias Beta [1/second] - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Accel Bias Beta [1/second] + float noise[3] = {0}; ///< Accel Noise 1-sigma [meters/second^2] }; }; @@ -1198,8 +1180,8 @@ void extract(Serializer& serializer, AccelBiasModel& self); void insert(Serializer& serializer, const AccelBiasModel::Response& self); void extract(Serializer& serializer, AccelBiasModel::Response& self); -CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); -CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); CmdResult saveAccelBiasModel(C::mip_interface& device); CmdResult loadAccelBiasModel(C::mip_interface& device); CmdResult defaultAccelBiasModel(C::mip_interface& device); @@ -1209,7 +1191,7 @@ CmdResult defaultAccelBiasModel(C::mip_interface& device); ///@defgroup cpp_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [CPP] /// Gyroscope Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ @@ -1226,24 +1208,16 @@ struct GyroBiasModel static const bool HAS_RESET_FUNCTION = true; FunctionSelector function = static_cast(0); - float x_beta = 0; ///< Gyro Bias Beta [1/second] - float y_beta = 0; ///< Gyro Bias Beta [1/second] - float z_beta = 0; ///< Gyro Bias Beta [1/second] - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Gyro Bias Beta [1/second] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; - float x_beta = 0; ///< Gyro Bias Beta [1/second] - float y_beta = 0; ///< Gyro Bias Beta [1/second] - float z_beta = 0; ///< Gyro Bias Beta [1/second] - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + float beta[3] = {0}; ///< Gyro Bias Beta [1/second] + float noise[3] = {0}; ///< Gyro Noise 1-sigma [rad/second] }; }; @@ -1253,8 +1227,8 @@ void extract(Serializer& serializer, GyroBiasModel& self); void insert(Serializer& serializer, const GyroBiasModel::Response& self); void extract(Serializer& serializer, GyroBiasModel::Response& self); -CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); -CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); CmdResult saveGyroBiasModel(C::mip_interface& device); CmdResult loadGyroBiasModel(C::mip_interface& device); CmdResult defaultGyroBiasModel(C::mip_interface& device); @@ -1262,17 +1236,10 @@ CmdResult defaultGyroBiasModel(C::mip_interface& device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [CPP] -/// Altitude Aiding Control -/// /// Select altitude input for absolute altitude and/or vertical velocity. The primary altitude reading is always GNSS. -/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during GNSS outages. -/// -/// Possible altitude aiding selector values: -/// -/// 0x00 - No altitude aiding (disable) -/// 0x01 - Enable pressure sensor aiding(1) +/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during outages. /// -/// 1. Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. +/// Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. /// /// ///@{ @@ -1288,15 +1255,21 @@ struct AltitudeAiding static const bool HAS_LOAD_FUNCTION = true; static const bool HAS_RESET_FUNCTION = true; + enum class AidingSelector : uint8_t + { + NONE = 0, ///< No altitude aiding + PRESURE = 1, ///< Enable pressure sensor aiding + }; + FunctionSelector function = static_cast(0); - uint8_t aiding_selector = 0; ///< See above + AidingSelector selector = static_cast(0); ///< See above struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; - uint8_t aiding_selector = 0; ///< See above + AidingSelector selector = static_cast(0); ///< See above }; }; @@ -1306,16 +1279,64 @@ void extract(Serializer& serializer, AltitudeAiding& self); void insert(Serializer& serializer, const AltitudeAiding::Response& self); void extract(Serializer& serializer, AltitudeAiding::Response& self); -CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector); -CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut); +CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); +CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); CmdResult saveAltitudeAiding(C::mip_interface& device); CmdResult loadAltitudeAiding(C::mip_interface& device); CmdResult defaultAltitudeAiding(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_pitch_roll_aiding (0x0D,0x4B) Pitch Roll Aiding [CPP] +/// Select pitch/roll aiding input. Pitch/roll reading is always derived from GNSS corrected inertial solution. +/// Aiding inputs are used to improve that solution during periods of low dynamics and GNSS outages. +/// +///@{ + +struct PitchRollAiding +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + enum class AidingSource : uint8_t + { + NONE = 0, ///< No pitch/roll aiding + GRAVITY_VEC = 1, ///< Enable gravity vector aiding + }; + + FunctionSelector function = static_cast(0); + AidingSource source = static_cast(0); ///< Controls the aiding source + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + + AidingSource source = static_cast(0); ///< Controls the aiding source + + }; +}; +void insert(Serializer& serializer, const PitchRollAiding& self); +void extract(Serializer& serializer, PitchRollAiding& self); + +void insert(Serializer& serializer, const PitchRollAiding::Response& self); +void extract(Serializer& serializer, PitchRollAiding::Response& self); + +CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); +CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); +CmdResult savePitchRollAiding(C::mip_interface& device); +CmdResult loadPitchRollAiding(C::mip_interface& device); +CmdResult defaultPitchRollAiding(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_auto_zupt (0x0D,0x1E) Auto Zupt [CPP] -/// Zero Velocity Update /// The ZUPT is triggered when the scalar magnitude of the GNSS reported velocity vector is equal-to or less than the threshold value. /// The device will NACK threshold values that are less than zero (i.e.negative.) /// @@ -1407,7 +1428,6 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_commanded_zupt (0x0D,0x22) Commanded Zupt [CPP] -/// Commanded Zero Velocity Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1429,7 +1449,6 @@ CmdResult commandedZupt(C::mip_interface& device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_commanded_angular_zupt (0x0D,0x23) Commanded Angular Zupt [CPP] -/// Commanded Zero Angular Rate Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ @@ -1450,6 +1469,417 @@ CmdResult commandedAngularZupt(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_capture_auto_cal (0x0D,0x27) Mag Capture Auto Cal [CPP] +/// This command captures the current value of the auto-calibration, applies it to the current fixed hard and soft iron calibration coefficients, and replaces the current fixed hard and soft iron calibration coefficients with the new values. +/// This may be used in place of (or in addition to) a manual hard and soft iron calibration utility. This command also resets the auto-calibration coefficients. +/// Function selector SAVE is the same as issuing the 0x0C, 0x3A and 0x0C, 0x3B commands with the SAVE function selector. +/// +///@{ + +struct MagCaptureAutoCal +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = false; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = false; + static const bool HAS_RESET_FUNCTION = false; + + FunctionSelector function = static_cast(0); + +}; +void insert(Serializer& serializer, const MagCaptureAutoCal& self); +void extract(Serializer& serializer, MagCaptureAutoCal& self); + +CmdResult writeMagCaptureAutoCal(C::mip_interface& device); +CmdResult saveMagCaptureAutoCal(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_gravity_noise (0x0D,0x28) Gravity Noise [CPP] +/// Set the expected gravity noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// Note: Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. Changing this value modifies how the filter responds to dynamic input and can be used to tune filter performance. +/// Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct GravityNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[3] = {0}; ///< Gravity Noise 1-sigma [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; + + float noise[3] = {0}; ///< Gravity Noise 1-sigma [gauss] + + }; +}; +void insert(Serializer& serializer, const GravityNoise& self); +void extract(Serializer& serializer, GravityNoise& self); + +void insert(Serializer& serializer, const GravityNoise::Response& self); +void extract(Serializer& serializer, GravityNoise::Response& self); + +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise); +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveGravityNoise(C::mip_interface& device); +CmdResult loadGravityNoise(C::mip_interface& device); +CmdResult defaultGravityNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_pressure_altitude_noise (0x0D,0x29) Pressure Altitude Noise [CPP] +/// Set the expected pressure altitude noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// The noise value must be greater than 0.0 +/// +/// This noise value represents pressure altitude model noise in the Estimation Filter. +/// A lower value will increase responsiveness of the sensor to pressure changes, however height estimates will be more susceptible to error from air pressure fluctuations not due to changes in altitude. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct PressureAltitudeNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + + float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] + + }; +}; +void insert(Serializer& serializer, const PressureAltitudeNoise& self); +void extract(Serializer& serializer, PressureAltitudeNoise& self); + +void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self); +void extract(Serializer& serializer, PressureAltitudeNoise::Response& self); + +CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise); +CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); +CmdResult savePressureAltitudeNoise(C::mip_interface& device); +CmdResult loadPressureAltitudeNoise(C::mip_interface& device); +CmdResult defaultPressureAltitudeNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [CPP] +/// Set the expected hard iron offset noise 1-sigma values. This function can be used to tune the filter performance in the target application. +/// +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise values represent process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct HardIronOffsetNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[3] = {0}; ///< Hard Iron Offset Noise 1-sigma [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + + float noise[3] = {0}; ///< Hard Iron Offset Noise 1-sigma [gauss] + + }; +}; +void insert(Serializer& serializer, const HardIronOffsetNoise& self); +void extract(Serializer& serializer, HardIronOffsetNoise& self); + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveHardIronOffsetNoise(C::mip_interface& device); +CmdResult loadHardIronOffsetNoise(C::mip_interface& device); +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_soft_iron_matrix_noise (0x0D,0x2C) Soft Iron Matrix Noise [CPP] +/// Set the expected soft iron matrix noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions. +/// +///@{ + +struct SoftIronMatrixNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[9] = {0}; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; + + float noise[9] = {0}; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + + }; +}; +void insert(Serializer& serializer, const SoftIronMatrixNoise& self); +void extract(Serializer& serializer, SoftIronMatrixNoise& self); + +void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self); +void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self); + +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveSoftIronMatrixNoise(C::mip_interface& device); +CmdResult loadSoftIronMatrixNoise(C::mip_interface& device); +CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_noise (0x0D,0x42) Mag Noise [CPP] +/// Set the expected magnetometer noise 1-sigma values. +/// This function can be used to tune the filter performance in the target application. +/// +/// Noise values must be greater than 0.0 (gauss) +/// +/// The noise value represents process noise in the Estimation Filter. +/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. Default values provide good performance for most laboratory conditions +/// +///@{ + +struct MagNoise +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + float noise[3] = {0}; ///< Mag Noise 1-sigma [gauss] + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; + + float noise[3] = {0}; ///< Mag Noise 1-sigma [gauss] + + }; +}; +void insert(Serializer& serializer, const MagNoise& self); +void extract(Serializer& serializer, MagNoise& self); + +void insert(Serializer& serializer, const MagNoise::Response& self); +void extract(Serializer& serializer, MagNoise::Response& self); + +CmdResult writeMagNoise(C::mip_interface& device, const float* noise); +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveMagNoise(C::mip_interface& device); +CmdResult loadMagNoise(C::mip_interface& device); +CmdResult defaultMagNoise(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_inclination_source (0x0D,0x4C) Inclination Source [CPP] +/// Set/Get the local magnetic field inclination angle source. +/// +/// This can be used to correct for the local value of inclination (dip angle) of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct InclinationSource +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Inclination Source + float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; + + FilterMagParamSource source = static_cast(0); ///< Inclination Source + float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) + + }; +}; +void insert(Serializer& serializer, const InclinationSource& self); +void extract(Serializer& serializer, InclinationSource& self); + +void insert(Serializer& serializer, const InclinationSource::Response& self); +void extract(Serializer& serializer, InclinationSource::Response& self); + +CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); +CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); +CmdResult saveInclinationSource(C::mip_interface& device); +CmdResult loadInclinationSource(C::mip_interface& device); +CmdResult defaultInclinationSource(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [CPP] +/// Set/Get the local magnetic field declination angle source. +/// +/// This can be used to correct for the local value of declination of the earthmagnetic field. +/// Having a correct value is important for best performance of the auto-mag calibration feature. If you do not have an accurate inclination angle source, it is recommended that you leave the auto-mag calibration feature off. +/// +/// +///@{ + +struct MagneticDeclinationSource +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source + float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; + + FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source + float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) + + }; +}; +void insert(Serializer& serializer, const MagneticDeclinationSource& self); +void extract(Serializer& serializer, MagneticDeclinationSource& self); + +void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); +void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); + +CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); +CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); +CmdResult saveMagneticDeclinationSource(C::mip_interface& device); +CmdResult loadMagneticDeclinationSource(C::mip_interface& device); +CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_mag_field_magnitude_source (0x0D,0x4D) Mag Field Magnitude Source [CPP] +/// Set/Get the local magnetic field magnitude source. +/// +/// This is used to specify the local magnitude of the earth's magnetic field. +/// Having a correct value for magnitude is important for best performance of the auto-mag calibration feature and for the magnetometer adaptive magnitude. If you do not have an accurate value for the local magnetic field magnitude, it is recommended that you leave the auto-mag calibration feature off. +/// +///@{ + +struct MagFieldMagnitudeSource +{ + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; + + static const bool HAS_WRITE_FUNCTION = true; + static const bool HAS_READ_FUNCTION = true; + static const bool HAS_SAVE_FUNCTION = true; + static const bool HAS_LOAD_FUNCTION = true; + static const bool HAS_RESET_FUNCTION = true; + + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source + float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; + + FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source + float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) + + }; +}; +void insert(Serializer& serializer, const MagFieldMagnitudeSource& self); +void extract(Serializer& serializer, MagFieldMagnitudeSource& self); + +void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self); +void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self); + +CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); +CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); +CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device); +CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device); +CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_reference_position (0x0D,0x26) Reference Position [CPP] /// Set the Lat/Long/Alt reference position for the sensor. /// @@ -2332,105 +2762,6 @@ CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [CPP] -/// Set the expected hard iron offset noise 1-sigma values -/// -/// This function can be used to tune the filter performance in the target application. -/// -/// Each of the noise values must be greater than 0.0 -/// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. -/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. -/// Default values provide good performance for most laboratory conditions. -/// -/// -///@{ - -struct HardIronOffsetNoise -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - FunctionSelector function = static_cast(0); - float x = 0; ///< HI Offset Noise 1-sima [gauss] - float y = 0; ///< HI Offset Noise 1-sima [gauss] - float z = 0; ///< HI Offset Noise 1-sima [gauss] - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; - - float x = 0; ///< HI Offset Noise 1-sima [gauss] - float y = 0; ///< HI Offset Noise 1-sima [gauss] - float z = 0; ///< HI Offset Noise 1-sima [gauss] - - }; -}; -void insert(Serializer& serializer, const HardIronOffsetNoise& self); -void extract(Serializer& serializer, HardIronOffsetNoise& self); - -void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); -void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); - -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); -CmdResult saveHardIronOffsetNoise(C::mip_interface& device); -CmdResult loadHardIronOffsetNoise(C::mip_interface& device); -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [CPP] -/// Source for magnetic declination angle, and user supplied value for manual selection. -/// -///@{ - -struct MagneticDeclinationSource -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - FunctionSelector function = static_cast(0); - FilterMagDeclinationSource source = static_cast(0); ///< Magnetic field declination angle source - float declination = 0; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; - - FilterMagDeclinationSource source = static_cast(0); ///< Magnetic field declination angle source - float declination = 0; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - - }; -}; -void insert(Serializer& serializer, const MagneticDeclinationSource& self); -void extract(Serializer& serializer, MagneticDeclinationSource& self); - -void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); -void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); - -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource source, float declination); -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource* sourceOut, float* declinationOut); -CmdResult saveMagneticDeclinationSource(C::mip_interface& device); -CmdResult loadMagneticDeclinationSource(C::mip_interface& device); -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_set_initial_heading (0x0D,0x03) Set Initial Heading [CPP] /// Set the initial heading angle. /// From dee6ae6db461741203589be02bae5bab9dd8b4de Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 23 Jun 2023 16:54:49 -0400 Subject: [PATCH 072/252] Generate MIP definitions from branches/dev@55074. --- src/mip/definitions/commands_filter.c | 2 +- src/mip/definitions/commands_filter.cpp | 2 +- src/mip/definitions/commands_filter.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index d71700893..7cc6aed8f 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -2750,7 +2750,7 @@ mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* dev assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index ec4f673ad..c88cb0eb8 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -2443,7 +2443,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), CMD_PRESSURE_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 636788352..6ba1b1b3f 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -1572,7 +1572,7 @@ struct PressureAltitudeNoise struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] From 880252ceca4480fcc96cbfe0cdd61433dfa28b53 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Fri, 23 Jun 2023 17:30:23 -0400 Subject: [PATCH 073/252] Generate MIP definitions from branches/dev@55074. (#67) Co-authored-by: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> --- src/mip/definitions/commands_filter.c | 2 +- src/mip/definitions/commands_filter.cpp | 2 +- src/mip/definitions/commands_filter.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index d71700893..7cc6aed8f 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -2750,7 +2750,7 @@ mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* dev assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index ec4f673ad..c88cb0eb8 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -2443,7 +2443,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), CMD_PRESSURE_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 636788352..6ba1b1b3f 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -1572,7 +1572,7 @@ struct PressureAltitudeNoise struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] From 468a0b5c2d0c8ec2d00a0e2b301cfef2a38138b8 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:10:46 -0400 Subject: [PATCH 074/252] MIP SDK: - Create vector types and move MipDescriptorRate to common file. --- src/mip/definitions/common.c | 48 ++++++ src/mip/definitions/common.h | 250 ++++++++++++++++++++++++++++++ src/mip/definitions/descriptors.c | 13 -- src/mip/definitions/descriptors.h | 14 -- 4 files changed, 298 insertions(+), 27 deletions(-) create mode 100644 src/mip/definitions/common.c create mode 100644 src/mip/definitions/common.h diff --git a/src/mip/definitions/common.c b/src/mip/definitions/common.c new file mode 100644 index 000000000..0ddc9f530 --- /dev/null +++ b/src/mip/definitions/common.c @@ -0,0 +1,48 @@ + +#include "common.h" + +#include "../utils/serialization.h" + +#ifdef __cplusplus +namespace mip { +extern "C" { +#endif // __cplusplus + + +void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self) +{ + insert_u8(serializer, self->descriptor); + insert_u16(serializer, self->decimation); +} + +void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self) +{ + extract_u8(serializer, &self->descriptor); + extract_u16(serializer, &self->decimation); +} + +#define IMPLEMENT_MIP_VECTOR_FUNCTIONS(n,type,name) \ +void insert_mip_##name(mip_serializer* serializer, const mip_##name* self) \ +{ \ + for(unsigned int i=0; iv[i]); \ +} \ +void extract_mip_##name(mip_serializer* serializer, mip_##name* self) \ +{ \ + for(unsigned int i=0; iv[i]); \ +} + +IMPLEMENT_MIP_VECTOR_FUNCTIONS(3,float,vector3f) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(4,float,vector4f) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(9,float,matrix3f) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(3,double,vector3d) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(4,double,vector4d) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(9,double,matrix3d) + +#undef IMPLEMENT_MIP_VECTOR_FUNCTIONS + +#ifdef __cplusplus +} // namespace mip +} // extern "C" +#endif // __cplusplus diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h new file mode 100644 index 000000000..686d0142b --- /dev/null +++ b/src/mip/definitions/common.h @@ -0,0 +1,250 @@ +#pragma once + +#include +#include +#include +#include "../utils/serialization.h" + +#ifdef __cplusplus + +#include +#include + +namespace mip { +namespace C { +extern "C" { +#endif // __cplusplus + + +typedef struct mip_descriptor_rate +{ + uint8_t descriptor; + uint16_t decimation; +} mip_descriptor_rate; + +void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self); +void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self); + +#define DECLARE_MIP_VECTOR_TYPE(n,type,name) \ +typedef struct mip_##name \ +{ \ + type v[n]; \ +} mip_##name; \ +\ +void insert_mip_##name(mip_serializer* serializer, const mip_##name* self); \ +void extract_mip_##name(mip_serializer* serializer, mip_##name* self); + +DECLARE_MIP_VECTOR_TYPE(3,float,vector3f) +DECLARE_MIP_VECTOR_TYPE(4,float,vector4f) +DECLARE_MIP_VECTOR_TYPE(9,float,matrix3f) +DECLARE_MIP_VECTOR_TYPE(3,double,vector3d) +DECLARE_MIP_VECTOR_TYPE(4,double,vector4d) +DECLARE_MIP_VECTOR_TYPE(9,double,matrix3d) + +#undef DECLARE_MIP_VECTOR_TYPE + +//typedef struct mip_vector3f +//{ +// float m_data[3]; +//} mip_vector3f; +// +//void insert_mip_vector3(mip_serializer* serializer, const mip_vector3* self); +//void extract_mip_vector3(mip_serializer* serializer, mip_vector3* self); +// +//typedef struct mip_vector4f +//{ +// float m_data[4]; +//} mip_vector4f; +// +//void insert_mip_vector3(mip_serializer* serializer, const mip_vector3* self); +//void extract_mip_vector3(mip_serializer* serializer, mip_vector3* self); +// +//typedef struct mip_matrix3 +//{ +// float m[9]; +//} mip_matrix3; +// +//void insert_mip_matrix3(mip_serializer* serializer, const mip_matrix3* self); +//void extract_mip_matrix3(mip_serializer* serializer, mip_matrix3* self); + + +#ifdef __cplusplus + +} // extern "C" +} // namespace "C" + + +using DescriptorRate = C::mip_descriptor_rate; + +inline void insert(Serializer& serializer, const DescriptorRate& self) { return C::insert_mip_descriptor_rate(&serializer, &self); } +inline void extract(Serializer& serializer, DescriptorRate& self) { return C::extract_mip_descriptor_rate(&serializer, &self); } + + +template +using Vector = T[N]; + +using Vector3f = Vector; +using Vector4f = Vector; +using Matrix3f = Vector; +using Vector3d = Vector; +using Vector4d = Vector; +using Matrix3d = Vector; + +using Quatf = float[4]; + +template +void insert(Serializer& serializer, const Vector& v) { for(size_t i=0; i +void extract(Serializer& serializer, Vector& v) { for(size_t i=0; i +//class Vector +//{ +//public: +// Vector() { *this = T(0); } +// Vector(const Vector&) = default; +// Vector(const T (&ptr)[N]) { copyFrom(ptr); } +// +// Vector& operator=(const Vector&) = default; +// Vector& operator=(const T* ptr) { copyFrom(ptr); return this; } +// Vector& operator=(T value) { for(size_t i=0; i +// void copyFrom(const U* ptr) { for(size_t i=0; i +// void copyTo(U* ptr) const { for(size_t i=0; i +//class Vector2 : public Vector +//{ +// using Vector::Vector; +// +// template +// Vector2(U x_, U y_) { x()=x_; y()=y_;} +// +// T& x() { return this->m_data[0]; } +// T& y() { return this->m_data[1]; } +// +// T x() const { return this->m_data[0]; } +// T y() const { return this->m_data[1]; } +//}; +// +/////@brief A 3D vector which provides .x(), .y() and .z() accessors. +///// +//template +//class Vector3 : public Vector +//{ +// //using Vector::Vector; +// template +// Vector3(const U* ptr) { copyFrom(ptr); } +// +// template +// Vector3(U x_, U y_, U z_) { x()=x_; y()=y_; z()=z_; } +// +// T& x() { return this->m_data[0]; } +// T& y() { return this->m_data[1]; } +// T& z() { return this->m_data[2]; } +// +// T x() const { return this->m_data[0]; } +// T y() const { return this->m_data[1]; } +// T z() const { return this->m_data[2]; } +//}; +// +/////@brief A 4D vector which provides .x(), .y(), .z(), and .w() accessors. +///// +//template +//class Vector4 : public Vector +//{ +// using Vector::Vector; +// +// template +// Vector4(U x_, U y_, U z_, U w_) { x()=x_; y()=y_; z()=z_; w()=w_; } +// +// T& x() { return this->m_data[0]; } +// T& y() { return this->m_data[1]; } +// T& z() { return this->m_data[2]; } +// T& w() { return this->m_data[3]; } +// +// T x() const { return this->m_data[0]; } +// T y() const { return this->m_data[1]; } +// T z() const { return this->m_data[2]; } +// T w() const { return this->m_data[3]; } +//}; +// +/////@brief A quaternion which provides .x(), .y(), .z(), and .w() accessors. +///// +//template +//class Quaternion : public Vector4 +//{ +// using Vector4::Vector; +//}; +// +//typedef Vector2 Vector2f; +//typedef Vector3 Vector3f; +//typedef Vector4 Vector4f; +// +//typedef Vector2 Vector2d; +//typedef Vector3 Vector3d; +//typedef Vector4 Vector4d; +// +//typedef Quaternion Quatf; +//typedef Vector Matrix3f; +// +//template +//void insert(Serializer& serializer, const Vector& self) +//{ +// for(size_t i=0; i +//void extract(Serializer& serializer, Vector& self) +//{ +// for(size_t i=0; i +//void insert(Serializer& serializer, const Quaternion& self) +//{ +// insert(serializer, self.w()); // w comes first in mip +// insert(serializer, self.x()); +// insert(serializer, self.y()); +// insert(serializer, self.z()); +//} +// +//template +//void extract(Serializer& serializer, Quaternion& self) +//{ +// extract(serializer, self.w()); // w comes first in mip +// extract(serializer, self.x()); +// extract(serializer, self.y()); +// extract(serializer, self.z()); +//} + + +} // namespace mip + +#endif // __cplusplus diff --git a/src/mip/definitions/descriptors.c b/src/mip/definitions/descriptors.c index bc32cb598..1ca874df7 100644 --- a/src/mip/definitions/descriptors.c +++ b/src/mip/definitions/descriptors.c @@ -152,19 +152,6 @@ void extract_mip_function_selector(mip_serializer* serializer, enum mip_function } -void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self) -{ - insert_u8(serializer, self->descriptor); - insert_u16(serializer, self->decimation); -} - -void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self) -{ - extract_u8(serializer, &self->descriptor); - extract_u16(serializer, &self->decimation); -} - - #ifdef __cplusplus } // namespace mip } // extern "C" diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index 4f6da0545..1f98f4b43 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -52,14 +52,6 @@ typedef enum mip_function_selector mip_function_selector; void insert_mip_function_selector(mip_serializer* serializer, enum mip_function_selector self); void extract_mip_function_selector(mip_serializer* serializer, enum mip_function_selector* self); -typedef struct mip_descriptor_rate -{ - uint8_t descriptor; - uint16_t decimation; -} mip_descriptor_rate; - -void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self); -void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self); #ifdef __cplusplus @@ -107,8 +99,6 @@ enum class FunctionSelector : uint8_t RESET = C::MIP_FUNCTION_RESET, }; -using DescriptorRate = C::mip_descriptor_rate; - static constexpr uint8_t INVALID_FIELD_DESCRIPTOR = C::MIP_INVALID_FIELD_DESCRIPTOR; static constexpr uint8_t INVALID_DESCRIPTOR_SET = C::MIP_INVALID_DESCRIPTOR_SET; @@ -124,10 +114,6 @@ inline bool isResponseFieldDescriptor(uint8_t fieldDescriptor) { return C::mip inline bool isReservedFieldDescriptor(uint8_t fieldDescriptor) { return C::mip_is_reserved_cmd_field_descriptor(fieldDescriptor); } inline bool isSharedDataFieldDescriptor(uint8_t fieldDescriptor) { return C::mip_is_shared_data_field_descriptor(fieldDescriptor); } - -inline void insert(Serializer& serializer, const DescriptorRate& self) { return C::insert_mip_descriptor_rate(&serializer, &self); } -inline void extract(Serializer& serializer, DescriptorRate& self) { return C::extract_mip_descriptor_rate(&serializer, &self); } - } // namespace mip #endif // __cplusplus From 7c05b2ccaf73f2a2acd46c181b09ff93e58e1571 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:12:54 -0400 Subject: [PATCH 075/252] MIP SDK: - Add mip_quatf definition. --- src/mip/definitions/common.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 686d0142b..508500f9c 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -41,6 +41,8 @@ DECLARE_MIP_VECTOR_TYPE(3,double,vector3d) DECLARE_MIP_VECTOR_TYPE(4,double,vector4d) DECLARE_MIP_VECTOR_TYPE(9,double,matrix3d) +typedef mip_vector4f mip_quatf; + #undef DECLARE_MIP_VECTOR_TYPE //typedef struct mip_vector3f From c0df4ec22b384228cddf2c6a67c1a4c2ab7ad80a Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:15:50 -0400 Subject: [PATCH 076/252] MIP SDK: - Add mip_quatf definition (take 2). --- src/mip/definitions/common.c | 1 + src/mip/definitions/common.h | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mip/definitions/common.c b/src/mip/definitions/common.c index 0ddc9f530..54b909ee6 100644 --- a/src/mip/definitions/common.c +++ b/src/mip/definitions/common.c @@ -39,6 +39,7 @@ IMPLEMENT_MIP_VECTOR_FUNCTIONS(9,float,matrix3f) IMPLEMENT_MIP_VECTOR_FUNCTIONS(3,double,vector3d) IMPLEMENT_MIP_VECTOR_FUNCTIONS(4,double,vector4d) IMPLEMENT_MIP_VECTOR_FUNCTIONS(9,double,matrix3d) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(4,float,quatf) #undef IMPLEMENT_MIP_VECTOR_FUNCTIONS diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 508500f9c..822ff9637 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -40,8 +40,7 @@ DECLARE_MIP_VECTOR_TYPE(9,float,matrix3f) DECLARE_MIP_VECTOR_TYPE(3,double,vector3d) DECLARE_MIP_VECTOR_TYPE(4,double,vector4d) DECLARE_MIP_VECTOR_TYPE(9,double,matrix3d) - -typedef mip_vector4f mip_quatf; +DECLARE_MIP_VECTOR_TYPE(4,float,quatf) #undef DECLARE_MIP_VECTOR_TYPE From ad6091ffbe977c5739a8ebd45ec638b46ca074c1 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:22:47 -0400 Subject: [PATCH 077/252] MIP SDK: - Add missing #include --- src/mip/definitions/descriptors.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index 1f98f4b43..e95ca1294 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -9,6 +9,7 @@ #include #include +#include namespace mip { namespace C { From 9742729fe5554d17fd3905efefe022d6dc1f5487 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:24:36 -0400 Subject: [PATCH 078/252] MIP SDK: - Update CMakeLists.txt to include definitions/common.*. --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a500aa44..849f6f12b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,6 +124,8 @@ set(MIP_SOURCES "${MIP_DIR}/mip_result.c" "${MIP_DIR}/mip_result.h" "${MIP_DIR}/mip_types.h" + "${MIP_DIR}/definitions/common.c" + "${MIP_DIR}/definitions/common.h" "${MIP_DIR}/definitions/descriptors.c" "${MIP_DIR}/definitions/descriptors.h" "${MIP_DIR}/mip.hpp" From 694010fef9836d8bbc2f99fe3a87999c7ad4ab90 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 11 Jul 2023 13:02:16 -0400 Subject: [PATCH 079/252] Minor tweaks to common definitions files. --- src/mip/definitions/common.c | 32 +++++++++++--------------------- src/mip/definitions/common.h | 25 +++++++++++-------------- 2 files changed, 22 insertions(+), 35 deletions(-) diff --git a/src/mip/definitions/common.c b/src/mip/definitions/common.c index 54b909ee6..686d4885a 100644 --- a/src/mip/definitions/common.c +++ b/src/mip/definitions/common.c @@ -3,11 +3,6 @@ #include "../utils/serialization.h" -#ifdef __cplusplus -namespace mip { -extern "C" { -#endif // __cplusplus - void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self) { @@ -22,28 +17,23 @@ void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate } #define IMPLEMENT_MIP_VECTOR_FUNCTIONS(n,type,name) \ -void insert_mip_##name(mip_serializer* serializer, const mip_##name* self) \ +void insert_##name(mip_serializer* serializer, const name self) \ { \ for(unsigned int i=0; iv[i]); \ + insert_##type(serializer, self[i]); \ } \ -void extract_mip_##name(mip_serializer* serializer, mip_##name* self) \ +void extract_##name(mip_serializer* serializer, name self) \ { \ for(unsigned int i=0; iv[i]); \ + extract_##type(serializer, &self[i]); \ } -IMPLEMENT_MIP_VECTOR_FUNCTIONS(3,float,vector3f) -IMPLEMENT_MIP_VECTOR_FUNCTIONS(4,float,vector4f) -IMPLEMENT_MIP_VECTOR_FUNCTIONS(9,float,matrix3f) -IMPLEMENT_MIP_VECTOR_FUNCTIONS(3,double,vector3d) -IMPLEMENT_MIP_VECTOR_FUNCTIONS(4,double,vector4d) -IMPLEMENT_MIP_VECTOR_FUNCTIONS(9,double,matrix3d) -IMPLEMENT_MIP_VECTOR_FUNCTIONS(4,float,quatf) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(3, float, mip_vector3f) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(4, float, mip_vector4f) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(9, float, mip_matrix3f) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(3, double, mip_vector3d) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(4, double, mip_vector4d) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(9, double, mip_matrix3d) +IMPLEMENT_MIP_VECTOR_FUNCTIONS(4, float, mip_quatf) #undef IMPLEMENT_MIP_VECTOR_FUNCTIONS - -#ifdef __cplusplus -} // namespace mip -} // extern "C" -#endif // __cplusplus diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 822ff9637..17f6d8390 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -26,21 +26,18 @@ void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self); #define DECLARE_MIP_VECTOR_TYPE(n,type,name) \ -typedef struct mip_##name \ -{ \ - type v[n]; \ -} mip_##name; \ +typedef type name[n]; \ \ -void insert_mip_##name(mip_serializer* serializer, const mip_##name* self); \ -void extract_mip_##name(mip_serializer* serializer, mip_##name* self); +void insert_##name(mip_serializer* serializer, const name self); \ +void extract_##name(mip_serializer* serializer, name self); -DECLARE_MIP_VECTOR_TYPE(3,float,vector3f) -DECLARE_MIP_VECTOR_TYPE(4,float,vector4f) -DECLARE_MIP_VECTOR_TYPE(9,float,matrix3f) -DECLARE_MIP_VECTOR_TYPE(3,double,vector3d) -DECLARE_MIP_VECTOR_TYPE(4,double,vector4d) -DECLARE_MIP_VECTOR_TYPE(9,double,matrix3d) -DECLARE_MIP_VECTOR_TYPE(4,float,quatf) +DECLARE_MIP_VECTOR_TYPE(3, float, mip_vector3f) +DECLARE_MIP_VECTOR_TYPE(4, float, mip_vector4f) +DECLARE_MIP_VECTOR_TYPE(9, float, mip_matrix3f) +DECLARE_MIP_VECTOR_TYPE(3, double, mip_vector3d) +DECLARE_MIP_VECTOR_TYPE(4, double, mip_vector4d) +DECLARE_MIP_VECTOR_TYPE(9, double, mip_matrix3d) +DECLARE_MIP_VECTOR_TYPE(4, float, mip_quatf) #undef DECLARE_MIP_VECTOR_TYPE @@ -91,7 +88,7 @@ using Vector3d = Vector; using Vector4d = Vector; using Matrix3d = Vector; -using Quatf = float[4]; +using Quatf = Vector4f; template void insert(Serializer& serializer, const Vector& v) { for(size_t i=0; i Date: Tue, 11 Jul 2023 13:06:49 -0400 Subject: [PATCH 080/252] Generate MIP definitions from branches/cv7_ins@55121. --- src/mip/definitions/commands_3dm.c | 103 +- src/mip/definitions/commands_3dm.cpp | 77 +- src/mip/definitions/commands_3dm.h | 137 +- src/mip/definitions/commands_3dm.hpp | 1622 +++++++++++++---- src/mip/definitions/commands_aiding.c | 78 +- src/mip/definitions/commands_aiding.cpp | 52 +- src/mip/definitions/commands_aiding.h | 96 +- src/mip/definitions/commands_aiding.hpp | 323 +++- src/mip/definitions/commands_base.c | 2 - src/mip/definitions/commands_base.cpp | 10 +- src/mip/definitions/commands_base.h | 16 +- src/mip/definitions/commands_base.hpp | 196 +- src/mip/definitions/commands_filter.c | 1117 +++++++++++- src/mip/definitions/commands_filter.cpp | 955 +++++++++- src/mip/definitions/commands_filter.h | 210 ++- src/mip/definitions/commands_filter.hpp | 2210 +++++++++++++++++------ src/mip/definitions/commands_gnss.cpp | 2 +- src/mip/definitions/commands_gnss.h | 6 +- src/mip/definitions/commands_gnss.hpp | 113 +- src/mip/definitions/commands_rtk.h | 13 + src/mip/definitions/commands_rtk.hpp | 226 ++- src/mip/definitions/commands_system.h | 2 + src/mip/definitions/commands_system.hpp | 40 +- src/mip/definitions/data_filter.h | 120 +- src/mip/definitions/data_filter.hpp | 543 +++--- src/mip/definitions/data_gnss.h | 44 +- src/mip/definitions/data_gnss.hpp | 557 +++--- src/mip/definitions/data_sensor.h | 50 +- src/mip/definitions/data_sensor.hpp | 183 +- src/mip/definitions/data_shared.h | 10 + src/mip/definitions/data_shared.hpp | 97 +- src/mip/definitions/data_system.h | 5 + src/mip/definitions/data_system.hpp | 29 +- 33 files changed, 6909 insertions(+), 2335 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index 3e4977659..3947bb398 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -99,7 +99,7 @@ void extract_mip_3dm_poll_imu_message_command(mip_serializer* serializer, mip_3d extract_bool(serializer, &self->suppress_ack); assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -140,7 +140,7 @@ void extract_mip_3dm_poll_gnss_message_command(mip_serializer* serializer, mip_3 extract_bool(serializer, &self->suppress_ack); assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -181,7 +181,7 @@ void extract_mip_3dm_poll_filter_message_command(mip_serializer* serializer, mip extract_bool(serializer, &self->suppress_ack); assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -227,7 +227,7 @@ void extract_mip_3dm_imu_message_format_command(mip_serializer* serializer, mip_ if( self->function == MIP_FUNCTION_WRITE ) { assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -247,7 +247,7 @@ void insert_mip_3dm_imu_message_format_response(mip_serializer* serializer, cons void extract_mip_3dm_imu_message_format_response(mip_serializer* serializer, mip_3dm_imu_message_format_response* self) { assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -359,7 +359,7 @@ void extract_mip_3dm_gps_message_format_command(mip_serializer* serializer, mip_ if( self->function == MIP_FUNCTION_WRITE ) { assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -379,7 +379,7 @@ void insert_mip_3dm_gps_message_format_response(mip_serializer* serializer, cons void extract_mip_3dm_gps_message_format_response(mip_serializer* serializer, mip_3dm_gps_message_format_response* self) { assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -491,7 +491,7 @@ void extract_mip_3dm_filter_message_format_command(mip_serializer* serializer, m if( self->function == MIP_FUNCTION_WRITE ) { assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -511,7 +511,7 @@ void insert_mip_3dm_filter_message_format_response(mip_serializer* serializer, c void extract_mip_3dm_filter_message_format_response(mip_serializer* serializer, mip_3dm_filter_message_format_response* self) { assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -682,7 +682,7 @@ void extract_mip_3dm_poll_data_command(mip_serializer* serializer, mip_3dm_poll_ extract_bool(serializer, &self->suppress_ack); assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_u8(serializer, &self->descriptors[i]); @@ -788,7 +788,7 @@ void extract_mip_3dm_message_format_command(mip_serializer* serializer, mip_3dm_ if( self->function == MIP_FUNCTION_WRITE ) { assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -812,7 +812,7 @@ void extract_mip_3dm_message_format_response(mip_serializer* serializer, mip_3dm extract_u8(serializer, &self->desc_set); assert(self->num_descriptors); - extract_count(serializer, &self->num_descriptors, self->num_descriptors); + extract_count(serializer, &self->num_descriptors, sizeof(self->descriptors)/sizeof(self->descriptors[0])); for(unsigned int i=0; i < self->num_descriptors; i++) extract_mip_descriptor_rate(serializer, &self->descriptors[i]); @@ -931,7 +931,7 @@ void extract_mip_3dm_nmea_poll_data_command(mip_serializer* serializer, mip_3dm_ extract_bool(serializer, &self->suppress_ack); assert(self->count); - extract_count(serializer, &self->count, self->count); + extract_count(serializer, &self->count, sizeof(self->format_entries)/sizeof(self->format_entries[0])); for(unsigned int i=0; i < self->count; i++) extract_mip_nmea_message(serializer, &self->format_entries[i]); @@ -977,7 +977,7 @@ void extract_mip_3dm_nmea_message_format_command(mip_serializer* serializer, mip if( self->function == MIP_FUNCTION_WRITE ) { assert(self->count); - extract_count(serializer, &self->count, self->count); + extract_count(serializer, &self->count, sizeof(self->format_entries)/sizeof(self->format_entries[0])); for(unsigned int i=0; i < self->count; i++) extract_mip_nmea_message(serializer, &self->format_entries[i]); @@ -997,7 +997,7 @@ void insert_mip_3dm_nmea_message_format_response(mip_serializer* serializer, con void extract_mip_3dm_nmea_message_format_response(mip_serializer* serializer, mip_3dm_nmea_message_format_response* self) { assert(self->count); - extract_count(serializer, &self->count, self->count); + extract_count(serializer, &self->count, sizeof(self->format_entries)/sizeof(self->format_entries[0])); for(unsigned int i=0; i < self->count; i++) extract_mip_nmea_message(serializer, &self->format_entries[i]); @@ -1436,7 +1436,7 @@ void extract_mip_3dm_constellation_settings_command(mip_serializer* serializer, extract_u16(serializer, &self->max_channels); assert(self->config_count); - extract_count(serializer, &self->config_count, self->config_count); + extract_count(serializer, &self->config_count, sizeof(self->settings)/sizeof(self->settings[0])); for(unsigned int i=0; i < self->config_count; i++) extract_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); @@ -1464,7 +1464,7 @@ void extract_mip_3dm_constellation_settings_response(mip_serializer* serializer, extract_u16(serializer, &self->max_channels_use); assert(self->config_count); - extract_count(serializer, &self->config_count, self->config_count); + extract_count(serializer, &self->config_count, sizeof(self->settings)/sizeof(self->settings[0])); for(unsigned int i=0; i < self->config_count; i++) extract_mip_3dm_constellation_settings_command_settings(serializer, &self->settings[i]); @@ -1641,7 +1641,7 @@ void extract_mip_3dm_gnss_sbas_settings_command(mip_serializer* serializer, mip_ extract_mip_3dm_gnss_sbas_settings_command_sbasoptions(serializer, &self->sbas_options); assert(self->num_included_prns); - extract_count(serializer, &self->num_included_prns, self->num_included_prns); + extract_count(serializer, &self->num_included_prns, sizeof(self->included_prns)/sizeof(self->included_prns[0])); for(unsigned int i=0; i < self->num_included_prns; i++) extract_u16(serializer, &self->included_prns[i]); @@ -1669,7 +1669,7 @@ void extract_mip_3dm_gnss_sbas_settings_response(mip_serializer* serializer, mip extract_mip_3dm_gnss_sbas_settings_command_sbasoptions(serializer, &self->sbas_options); assert(self->num_included_prns); - extract_count(serializer, &self->num_included_prns, self->num_included_prns); + extract_count(serializer, &self->num_included_prns, sizeof(self->included_prns)/sizeof(self->included_prns[0])); for(unsigned int i=0; i < self->num_included_prns; i++) extract_u16(serializer, &self->included_prns[i]); @@ -3791,7 +3791,7 @@ void extract_mip_3dm_accel_bias_response(mip_serializer* serializer, mip_3dm_acc } -mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, const float* bias) +mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, mip_vector3f bias) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3799,15 +3799,14 @@ mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, const floa insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, bias[i]); + insert_mip_vector3f(&serializer, &bias[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, float* bias_out) +mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, mip_vector3f bias_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3825,9 +3824,8 @@ mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, float* bias mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(bias_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &bias_out[i]); + extract_mip_vector3f(&deserializer, &bias_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -3906,7 +3904,7 @@ void extract_mip_3dm_gyro_bias_response(mip_serializer* serializer, mip_3dm_gyro } -mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, const float* bias) +mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, mip_vector3f bias) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3914,15 +3912,14 @@ mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, const float insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, bias[i]); + insert_mip_vector3f(&serializer, &bias[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, float* bias_out) +mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, mip_vector3f bias_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3940,9 +3937,8 @@ mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, float* bias_ mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(bias_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &bias_out[i]); + extract_mip_vector3f(&deserializer, &bias_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4009,7 +4005,7 @@ void extract_mip_3dm_capture_gyro_bias_response(mip_serializer* serializer, mip_ } -mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, float* bias_out) +mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, mip_vector3f bias_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4027,9 +4023,8 @@ mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(bias_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &bias_out[i]); + extract_mip_vector3f(&deserializer, &bias_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4072,7 +4067,7 @@ void extract_mip_3dm_mag_hard_iron_offset_response(mip_serializer* serializer, m } -mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, const float* offset) +mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4080,15 +4075,14 @@ mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, offset[i]); + insert_mip_vector3f(&serializer, &offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, float* offset_out) +mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4106,9 +4100,8 @@ mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, f mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &offset_out[i]); + extract_mip_vector3f(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4187,7 +4180,7 @@ void extract_mip_3dm_mag_soft_iron_matrix_response(mip_serializer* serializer, m } -mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, const float* offset) +mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4195,15 +4188,14 @@ mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(offset || (9 == 0)); for(unsigned int i=0; i < 9; i++) - insert_float(&serializer, offset[i]); + insert_mip_matrix3f(&serializer, &offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, float* offset_out) +mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4221,9 +4213,8 @@ mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, f mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(offset_out || (9 == 0)); for(unsigned int i=0; i < 9; i++) - extract_float(&deserializer, &offset_out[i]); + extract_mip_matrix3f(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4544,7 +4535,7 @@ void extract_mip_3dm_sensor_2_vehicle_transform_quaternion_response(mip_serializ } -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, const float* q) +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4552,15 +4543,14 @@ mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_in insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(q || (4 == 0)); for(unsigned int i=0; i < 4; i++) - insert_float(&serializer, q[i]); + insert_mip_quatf(&serializer, &q[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, float* q_out) +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4578,9 +4568,8 @@ mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_int mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(q_out || (4 == 0)); for(unsigned int i=0; i < 4; i++) - extract_float(&deserializer, &q_out[i]); + extract_mip_quatf(&deserializer, &q_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4659,7 +4648,7 @@ void extract_mip_3dm_sensor_2_vehicle_transform_dcm_response(mip_serializer* ser } -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, const float* dcm) +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4667,15 +4656,14 @@ mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) - insert_float(&serializer, dcm[i]); + insert_mip_matrix3f(&serializer, &dcm[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, float* dcm_out) +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4693,9 +4681,8 @@ mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(dcm_out || (9 == 0)); for(unsigned int i=0; i < 9; i++) - extract_float(&deserializer, &dcm_out[i]); + extract_mip_matrix3f(&deserializer, &dcm_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index a5b1051ce..2d9ced80d 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -66,7 +66,7 @@ void extract(Serializer& serializer, PollImuMessage& self) { extract(serializer, self.suppress_ack); - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -103,7 +103,7 @@ void extract(Serializer& serializer, PollGnssMessage& self) { extract(serializer, self.suppress_ack); - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -140,7 +140,7 @@ void extract(Serializer& serializer, PollFilterMessage& self) { extract(serializer, self.suppress_ack); - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -182,7 +182,7 @@ void extract(Serializer& serializer, ImuMessageFormat& self) if( self.function == FunctionSelector::WRITE ) { - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -199,7 +199,7 @@ void insert(Serializer& serializer, const ImuMessageFormat::Response& self) } void extract(Serializer& serializer, ImuMessageFormat::Response& self) { - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -295,7 +295,7 @@ void extract(Serializer& serializer, GpsMessageFormat& self) if( self.function == FunctionSelector::WRITE ) { - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -312,7 +312,7 @@ void insert(Serializer& serializer, const GpsMessageFormat::Response& self) } void extract(Serializer& serializer, GpsMessageFormat::Response& self) { - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -408,7 +408,7 @@ void extract(Serializer& serializer, FilterMessageFormat& self) if( self.function == FunctionSelector::WRITE ) { - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -425,7 +425,7 @@ void insert(Serializer& serializer, const FilterMessageFormat::Response& self) } void extract(Serializer& serializer, FilterMessageFormat::Response& self) { - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -643,7 +643,7 @@ void extract(Serializer& serializer, PollData& self) extract(serializer, self.suppress_ack); - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -743,7 +743,7 @@ void extract(Serializer& serializer, MessageFormat& self) if( self.function == FunctionSelector::WRITE ) { - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -764,7 +764,7 @@ void extract(Serializer& serializer, MessageFormat::Response& self) { extract(serializer, self.desc_set); - C::extract_count(&serializer, &self.num_descriptors, self.num_descriptors); + C::extract_count(&serializer, &self.num_descriptors, sizeof(self.descriptors)/sizeof(self.descriptors[0])); for(unsigned int i=0; i < self.num_descriptors; i++) extract(serializer, self.descriptors[i]); @@ -867,7 +867,7 @@ void extract(Serializer& serializer, NmeaPollData& self) { extract(serializer, self.suppress_ack); - C::extract_count(&serializer, &self.count, self.count); + C::extract_count(&serializer, &self.count, sizeof(self.format_entries)/sizeof(self.format_entries[0])); for(unsigned int i=0; i < self.count; i++) extract(serializer, self.format_entries[i]); @@ -909,7 +909,7 @@ void extract(Serializer& serializer, NmeaMessageFormat& self) if( self.function == FunctionSelector::WRITE ) { - C::extract_count(&serializer, &self.count, self.count); + C::extract_count(&serializer, &self.count, sizeof(self.format_entries)/sizeof(self.format_entries[0])); for(unsigned int i=0; i < self.count; i++) extract(serializer, self.format_entries[i]); @@ -926,7 +926,7 @@ void insert(Serializer& serializer, const NmeaMessageFormat::Response& self) } void extract(Serializer& serializer, NmeaMessageFormat::Response& self) { - C::extract_count(&serializer, &self.count, self.count); + C::extract_count(&serializer, &self.count, sizeof(self.format_entries)/sizeof(self.format_entries[0])); for(unsigned int i=0; i < self.count; i++) extract(serializer, self.format_entries[i]); @@ -1309,7 +1309,7 @@ void extract(Serializer& serializer, ConstellationSettings& self) { extract(serializer, self.max_channels); - C::extract_count(&serializer, &self.config_count, self.config_count); + C::extract_count(&serializer, &self.config_count, sizeof(self.settings)/sizeof(self.settings[0])); for(unsigned int i=0; i < self.config_count; i++) extract(serializer, self.settings[i]); @@ -1334,7 +1334,7 @@ void extract(Serializer& serializer, ConstellationSettings::Response& self) extract(serializer, self.max_channels_use); - C::extract_count(&serializer, &self.config_count, self.config_count); + C::extract_count(&serializer, &self.config_count, sizeof(self.settings)/sizeof(self.settings[0])); for(unsigned int i=0; i < self.config_count; i++) extract(serializer, self.settings[i]); @@ -1473,7 +1473,7 @@ void extract(Serializer& serializer, GnssSbasSettings& self) extract(serializer, self.sbas_options); - C::extract_count(&serializer, &self.num_included_prns, self.num_included_prns); + C::extract_count(&serializer, &self.num_included_prns, sizeof(self.included_prns)/sizeof(self.included_prns[0])); for(unsigned int i=0; i < self.num_included_prns; i++) extract(serializer, self.included_prns[i]); @@ -1498,7 +1498,7 @@ void extract(Serializer& serializer, GnssSbasSettings::Response& self) extract(serializer, self.sbas_options); - C::extract_count(&serializer, &self.num_included_prns, self.num_included_prns); + C::extract_count(&serializer, &self.num_included_prns, sizeof(self.included_prns)/sizeof(self.included_prns[0])); for(unsigned int i=0; i < self.num_included_prns; i++) extract(serializer, self.included_prns[i]); @@ -3313,13 +3313,12 @@ void extract(Serializer& serializer, AccelBias::Response& self) } -CmdResult writeAccelBias(C::mip_interface& device, const float* bias) +CmdResult writeAccelBias(C::mip_interface& device, Vector3f bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, bias[i]); @@ -3327,7 +3326,7 @@ CmdResult writeAccelBias(C::mip_interface& device, const float* bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBias(C::mip_interface& device, float* biasOut) +CmdResult readAccelBias(C::mip_interface& device, Vector3f biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3342,7 +3341,6 @@ CmdResult readAccelBias(C::mip_interface& device, float* biasOut) { Serializer deserializer(buffer, responseLength); - assert(biasOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, biasOut[i]); @@ -3417,13 +3415,12 @@ void extract(Serializer& serializer, GyroBias::Response& self) } -CmdResult writeGyroBias(C::mip_interface& device, const float* bias) +CmdResult writeGyroBias(C::mip_interface& device, Vector3f bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, bias[i]); @@ -3431,7 +3428,7 @@ CmdResult writeGyroBias(C::mip_interface& device, const float* bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBias(C::mip_interface& device, float* biasOut) +CmdResult readGyroBias(C::mip_interface& device, Vector3f biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3446,7 +3443,6 @@ CmdResult readGyroBias(C::mip_interface& device, float* biasOut) { Serializer deserializer(buffer, responseLength); - assert(biasOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, biasOut[i]); @@ -3509,7 +3505,7 @@ void extract(Serializer& serializer, CaptureGyroBias::Response& self) } -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) +CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, Vector3f biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3525,7 +3521,6 @@ CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, fl { Serializer deserializer(buffer, responseLength); - assert(biasOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, biasOut[i]); @@ -3570,13 +3565,12 @@ void extract(Serializer& serializer, MagHardIronOffset::Response& self) } -CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) +CmdResult writeMagHardIronOffset(C::mip_interface& device, Vector3f offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, offset[i]); @@ -3584,7 +3578,7 @@ CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) +CmdResult readMagHardIronOffset(C::mip_interface& device, Vector3f offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3599,7 +3593,6 @@ CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) { Serializer deserializer(buffer, responseLength); - assert(offsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, offsetOut[i]); @@ -3674,13 +3667,12 @@ void extract(Serializer& serializer, MagSoftIronMatrix::Response& self) } -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) +CmdResult writeMagSoftIronMatrix(C::mip_interface& device, Matrix3f offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(offset || (9 == 0)); for(unsigned int i=0; i < 9; i++) insert(serializer, offset[i]); @@ -3688,7 +3680,7 @@ CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) +CmdResult readMagSoftIronMatrix(C::mip_interface& device, Matrix3f offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3703,7 +3695,6 @@ CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) { Serializer deserializer(buffer, responseLength); - assert(offsetOut || (9 == 0)); for(unsigned int i=0; i < 9; i++) extract(deserializer, offsetOut[i]); @@ -3998,13 +3989,12 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response } -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) +CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf q) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(q || (4 == 0)); for(unsigned int i=0; i < 4; i++) insert(serializer, q[i]); @@ -4012,7 +4002,7 @@ CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) +CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf qOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4027,7 +4017,6 @@ CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* { Serializer deserializer(buffer, responseLength); - assert(qOut || (4 == 0)); for(unsigned int i=0; i < 4; i++) extract(deserializer, qOut[i]); @@ -4102,13 +4091,12 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self) } -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) +CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) insert(serializer, dcm[i]); @@ -4116,7 +4104,7 @@ CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) +CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4131,7 +4119,6 @@ CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut { Serializer deserializer(buffer, responseLength); - assert(dcmOut || (9 == 0)); for(unsigned int i=0; i < 9; i++) extract(deserializer, dcmOut[i]); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 44a09e0fd..03953d412 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -212,7 +213,7 @@ struct mip_3dm_poll_imu_message_command { bool suppress_ack; ///< Suppress the usual ACK/NACK reply. uint8_t num_descriptors; ///< Number of descriptors in the descriptor list. - mip_descriptor_rate* descriptors; ///< Descriptor list. + mip_descriptor_rate descriptors[83]; ///< Descriptor list. }; typedef struct mip_3dm_poll_imu_message_command mip_3dm_poll_imu_message_command; @@ -220,6 +221,7 @@ void insert_mip_3dm_poll_imu_message_command(struct mip_serializer* serializer, void extract_mip_3dm_poll_imu_message_command(struct mip_serializer* serializer, mip_3dm_poll_imu_message_command* self); mip_cmd_result mip_3dm_poll_imu_message(struct mip_interface* device, bool suppress_ack, uint8_t num_descriptors, const mip_descriptor_rate* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -239,7 +241,7 @@ struct mip_3dm_poll_gnss_message_command { bool suppress_ack; ///< Suppress the usual ACK/NACK reply. uint8_t num_descriptors; ///< Number of descriptors in the descriptor list. - mip_descriptor_rate* descriptors; ///< Descriptor list. + mip_descriptor_rate descriptors[83]; ///< Descriptor list. }; typedef struct mip_3dm_poll_gnss_message_command mip_3dm_poll_gnss_message_command; @@ -247,6 +249,7 @@ void insert_mip_3dm_poll_gnss_message_command(struct mip_serializer* serializer, void extract_mip_3dm_poll_gnss_message_command(struct mip_serializer* serializer, mip_3dm_poll_gnss_message_command* self); mip_cmd_result mip_3dm_poll_gnss_message(struct mip_interface* device, bool suppress_ack, uint8_t num_descriptors, const mip_descriptor_rate* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -266,7 +269,7 @@ struct mip_3dm_poll_filter_message_command { bool suppress_ack; ///< Suppress the usual ACK/NACK reply. uint8_t num_descriptors; ///< Number of descriptors in the format list. - mip_descriptor_rate* descriptors; ///< Descriptor format list. + mip_descriptor_rate descriptors[83]; ///< Descriptor format list. }; typedef struct mip_3dm_poll_filter_message_command mip_3dm_poll_filter_message_command; @@ -274,6 +277,7 @@ void insert_mip_3dm_poll_filter_message_command(struct mip_serializer* serialize void extract_mip_3dm_poll_filter_message_command(struct mip_serializer* serializer, mip_3dm_poll_filter_message_command* self); mip_cmd_result mip_3dm_poll_filter_message(struct mip_interface* device, bool suppress_ack, uint8_t num_descriptors, const mip_descriptor_rate* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -288,7 +292,7 @@ struct mip_3dm_imu_message_format_command { mip_function_selector function; uint8_t num_descriptors; ///< Number of descriptors - mip_descriptor_rate* descriptors; ///< Descriptor format list. + mip_descriptor_rate descriptors[82]; ///< Descriptor format list. }; typedef struct mip_3dm_imu_message_format_command mip_3dm_imu_message_format_command; @@ -298,7 +302,7 @@ void extract_mip_3dm_imu_message_format_command(struct mip_serializer* serialize struct mip_3dm_imu_message_format_response { uint8_t num_descriptors; ///< Number of descriptors - mip_descriptor_rate* descriptors; ///< Descriptor format list. + mip_descriptor_rate descriptors[82]; ///< Descriptor format list. }; typedef struct mip_3dm_imu_message_format_response mip_3dm_imu_message_format_response; @@ -310,6 +314,7 @@ mip_cmd_result mip_3dm_read_imu_message_format(struct mip_interface* device, uin mip_cmd_result mip_3dm_save_imu_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_load_imu_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_default_imu_message_format(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -324,7 +329,7 @@ struct mip_3dm_gps_message_format_command { mip_function_selector function; uint8_t num_descriptors; ///< Number of descriptors - mip_descriptor_rate* descriptors; ///< Descriptor format list. + mip_descriptor_rate descriptors[82]; ///< Descriptor format list. }; typedef struct mip_3dm_gps_message_format_command mip_3dm_gps_message_format_command; @@ -334,7 +339,7 @@ void extract_mip_3dm_gps_message_format_command(struct mip_serializer* serialize struct mip_3dm_gps_message_format_response { uint8_t num_descriptors; ///< Number of descriptors - mip_descriptor_rate* descriptors; ///< Descriptor format list. + mip_descriptor_rate descriptors[82]; ///< Descriptor format list. }; typedef struct mip_3dm_gps_message_format_response mip_3dm_gps_message_format_response; @@ -346,6 +351,7 @@ mip_cmd_result mip_3dm_read_gps_message_format(struct mip_interface* device, uin mip_cmd_result mip_3dm_save_gps_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_load_gps_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_default_gps_message_format(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -360,7 +366,7 @@ struct mip_3dm_filter_message_format_command { mip_function_selector function; uint8_t num_descriptors; ///< Number of descriptors (limited by payload size) - mip_descriptor_rate* descriptors; + mip_descriptor_rate descriptors[82]; }; typedef struct mip_3dm_filter_message_format_command mip_3dm_filter_message_format_command; @@ -370,7 +376,7 @@ void extract_mip_3dm_filter_message_format_command(struct mip_serializer* serial struct mip_3dm_filter_message_format_response { uint8_t num_descriptors; ///< Number of descriptors (limited by payload size) - mip_descriptor_rate* descriptors; + mip_descriptor_rate descriptors[82]; }; typedef struct mip_3dm_filter_message_format_response mip_3dm_filter_message_format_response; @@ -382,6 +388,7 @@ mip_cmd_result mip_3dm_read_filter_message_format(struct mip_interface* device, mip_cmd_result mip_3dm_save_filter_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_load_filter_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_default_filter_message_format(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -403,6 +410,7 @@ void insert_mip_3dm_imu_get_base_rate_response(struct mip_serializer* serializer void extract_mip_3dm_imu_get_base_rate_response(struct mip_serializer* serializer, mip_3dm_imu_get_base_rate_response* self); mip_cmd_result mip_3dm_imu_get_base_rate(struct mip_interface* device, uint16_t* rate_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -424,6 +432,7 @@ void insert_mip_3dm_gps_get_base_rate_response(struct mip_serializer* serializer void extract_mip_3dm_gps_get_base_rate_response(struct mip_serializer* serializer, mip_3dm_gps_get_base_rate_response* self); mip_cmd_result mip_3dm_gps_get_base_rate(struct mip_interface* device, uint16_t* rate_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -445,6 +454,7 @@ void insert_mip_3dm_filter_get_base_rate_response(struct mip_serializer* seriali void extract_mip_3dm_filter_get_base_rate_response(struct mip_serializer* serializer, mip_3dm_filter_get_base_rate_response* self); mip_cmd_result mip_3dm_filter_get_base_rate(struct mip_interface* device, uint16_t* rate_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -465,7 +475,7 @@ struct mip_3dm_poll_data_command uint8_t desc_set; ///< Data descriptor set. Must be supported. bool suppress_ack; ///< Suppress the usual ACK/NACK reply. uint8_t num_descriptors; ///< Number of descriptors in the format list. - uint8_t* descriptors; ///< Descriptor format list. + uint8_t descriptors[82]; ///< Descriptor format list. }; typedef struct mip_3dm_poll_data_command mip_3dm_poll_data_command; @@ -473,6 +483,7 @@ void insert_mip_3dm_poll_data_command(struct mip_serializer* serializer, const m void extract_mip_3dm_poll_data_command(struct mip_serializer* serializer, mip_3dm_poll_data_command* self); mip_cmd_result mip_3dm_poll_data(struct mip_interface* device, uint8_t desc_set, bool suppress_ack, uint8_t num_descriptors, const uint8_t* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -501,6 +512,7 @@ void insert_mip_3dm_get_base_rate_response(struct mip_serializer* serializer, co void extract_mip_3dm_get_base_rate_response(struct mip_serializer* serializer, mip_3dm_get_base_rate_response* self); mip_cmd_result mip_3dm_get_base_rate(struct mip_interface* device, uint8_t desc_set, uint16_t* rate_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -516,7 +528,7 @@ struct mip_3dm_message_format_command mip_function_selector function; uint8_t desc_set; ///< Data descriptor set. Must be supported. When function is SAVE, LOAD, or DEFAULT, can be 0 to apply to all descriptor sets. uint8_t num_descriptors; ///< Number of descriptors (limited by payload size) - mip_descriptor_rate* descriptors; ///< List of descriptors and decimations. + mip_descriptor_rate descriptors[82]; ///< List of descriptors and decimations. }; typedef struct mip_3dm_message_format_command mip_3dm_message_format_command; @@ -527,7 +539,7 @@ struct mip_3dm_message_format_response { uint8_t desc_set; ///< Echoes the descriptor set from the command. uint8_t num_descriptors; ///< Number of descriptors in the list. - mip_descriptor_rate* descriptors; ///< List of descriptors and decimations. + mip_descriptor_rate descriptors[82]; ///< List of descriptors and decimations. }; typedef struct mip_3dm_message_format_response mip_3dm_message_format_response; @@ -539,6 +551,7 @@ mip_cmd_result mip_3dm_read_message_format(struct mip_interface* device, uint8_t mip_cmd_result mip_3dm_save_message_format(struct mip_interface* device, uint8_t desc_set); mip_cmd_result mip_3dm_load_message_format(struct mip_interface* device, uint8_t desc_set); mip_cmd_result mip_3dm_default_message_format(struct mip_interface* device, uint8_t desc_set); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -557,7 +570,7 @@ struct mip_3dm_nmea_poll_data_command { bool suppress_ack; ///< Suppress the usual ACK/NACK reply. uint8_t count; ///< Number of format entries (limited by payload size) - mip_nmea_message* format_entries; ///< List of format entries. + mip_nmea_message format_entries[40]; ///< List of format entries. }; typedef struct mip_3dm_nmea_poll_data_command mip_3dm_nmea_poll_data_command; @@ -565,6 +578,7 @@ void insert_mip_3dm_nmea_poll_data_command(struct mip_serializer* serializer, co void extract_mip_3dm_nmea_poll_data_command(struct mip_serializer* serializer, mip_3dm_nmea_poll_data_command* self); mip_cmd_result mip_3dm_nmea_poll_data(struct mip_interface* device, bool suppress_ack, uint8_t count, const mip_nmea_message* format_entries); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -577,7 +591,7 @@ struct mip_3dm_nmea_message_format_command { mip_function_selector function; uint8_t count; ///< Number of format entries (limited by payload size) - mip_nmea_message* format_entries; ///< List of format entries. + mip_nmea_message format_entries[40]; ///< List of format entries. }; typedef struct mip_3dm_nmea_message_format_command mip_3dm_nmea_message_format_command; @@ -587,7 +601,7 @@ void extract_mip_3dm_nmea_message_format_command(struct mip_serializer* serializ struct mip_3dm_nmea_message_format_response { uint8_t count; ///< Number of format entries (limited by payload size) - mip_nmea_message* format_entries; ///< List of format entries. + mip_nmea_message format_entries[40]; ///< List of format entries. }; typedef struct mip_3dm_nmea_message_format_response mip_3dm_nmea_message_format_response; @@ -599,6 +613,7 @@ mip_cmd_result mip_3dm_read_nmea_message_format(struct mip_interface* device, ui mip_cmd_result mip_3dm_save_nmea_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_load_nmea_message_format(struct mip_interface* device); mip_cmd_result mip_3dm_default_nmea_message_format(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -623,6 +638,7 @@ void extract_mip_3dm_device_settings_command(struct mip_serializer* serializer, mip_cmd_result mip_3dm_save_device_settings(struct mip_interface* device); mip_cmd_result mip_3dm_load_device_settings(struct mip_interface* device); mip_cmd_result mip_3dm_default_device_settings(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -669,6 +685,7 @@ mip_cmd_result mip_3dm_read_uart_baudrate(struct mip_interface* device, uint32_t mip_cmd_result mip_3dm_save_uart_baudrate(struct mip_interface* device); mip_cmd_result mip_3dm_load_uart_baudrate(struct mip_interface* device); mip_cmd_result mip_3dm_default_uart_baudrate(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -699,6 +716,7 @@ void insert_mip_3dm_factory_streaming_command_action(struct mip_serializer* seri void extract_mip_3dm_factory_streaming_command_action(struct mip_serializer* serializer, mip_3dm_factory_streaming_command_action* self); mip_cmd_result mip_3dm_factory_streaming(struct mip_interface* device, mip_3dm_factory_streaming_command_action action, uint8_t reserved); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -742,6 +760,7 @@ mip_cmd_result mip_3dm_read_datastream_control(struct mip_interface* device, uin mip_cmd_result mip_3dm_save_datastream_control(struct mip_interface* device, uint8_t desc_set); mip_cmd_result mip_3dm_load_datastream_control(struct mip_interface* device, uint8_t desc_set); mip_cmd_result mip_3dm_default_datastream_control(struct mip_interface* device, uint8_t desc_set); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -795,7 +814,7 @@ struct mip_3dm_constellation_settings_command mip_function_selector function; uint16_t max_channels; uint8_t config_count; - mip_3dm_constellation_settings_command_settings* settings; + mip_3dm_constellation_settings_command_settings settings[42]; }; typedef struct mip_3dm_constellation_settings_command mip_3dm_constellation_settings_command; @@ -816,7 +835,7 @@ struct mip_3dm_constellation_settings_response uint16_t max_channels_available; ///< Maximum channels available uint16_t max_channels_use; ///< Maximum channels to use uint8_t config_count; ///< Number of constellation configurations - mip_3dm_constellation_settings_command_settings* settings; ///< Constellation Settings + mip_3dm_constellation_settings_command_settings settings[42]; ///< Constellation Settings }; typedef struct mip_3dm_constellation_settings_response mip_3dm_constellation_settings_response; @@ -828,6 +847,7 @@ mip_cmd_result mip_3dm_read_constellation_settings(struct mip_interface* device, mip_cmd_result mip_3dm_save_constellation_settings(struct mip_interface* device); mip_cmd_result mip_3dm_load_constellation_settings(struct mip_interface* device); mip_cmd_result mip_3dm_default_constellation_settings(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -852,7 +872,7 @@ struct mip_3dm_gnss_sbas_settings_command uint8_t enable_sbas; ///< 0 - SBAS Disabled, 1 - SBAS enabled mip_3dm_gnss_sbas_settings_command_sbasoptions sbas_options; ///< SBAS options, see definition uint8_t num_included_prns; ///< Number of SBAS PRNs to include in search (0 = include all) - uint16_t* included_prns; ///< List of specific SBAS PRNs to search for + uint16_t included_prns[39]; ///< List of specific SBAS PRNs to search for }; typedef struct mip_3dm_gnss_sbas_settings_command mip_3dm_gnss_sbas_settings_command; @@ -867,7 +887,7 @@ struct mip_3dm_gnss_sbas_settings_response uint8_t enable_sbas; ///< 0 - SBAS Disabled, 1 - SBAS enabled mip_3dm_gnss_sbas_settings_command_sbasoptions sbas_options; ///< SBAS options, see definition uint8_t num_included_prns; ///< Number of SBAS PRNs to include in search (0 = include all) - uint16_t* included_prns; ///< List of specific SBAS PRNs to search for + uint16_t included_prns[39]; ///< List of specific SBAS PRNs to search for }; typedef struct mip_3dm_gnss_sbas_settings_response mip_3dm_gnss_sbas_settings_response; @@ -879,6 +899,7 @@ mip_cmd_result mip_3dm_read_gnss_sbas_settings(struct mip_interface* device, uin mip_cmd_result mip_3dm_save_gnss_sbas_settings(struct mip_interface* device); mip_cmd_result mip_3dm_load_gnss_sbas_settings(struct mip_interface* device); mip_cmd_result mip_3dm_default_gnss_sbas_settings(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -930,6 +951,7 @@ mip_cmd_result mip_3dm_read_gnss_assisted_fix(struct mip_interface* device, mip_ mip_cmd_result mip_3dm_save_gnss_assisted_fix(struct mip_interface* device); mip_cmd_result mip_3dm_load_gnss_assisted_fix(struct mip_interface* device); mip_cmd_result mip_3dm_default_gnss_assisted_fix(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -966,6 +988,7 @@ void extract_mip_3dm_gnss_time_assistance_response(struct mip_serializer* serial mip_cmd_result mip_3dm_write_gnss_time_assistance(struct mip_interface* device, double tow, uint16_t week_number, float accuracy); mip_cmd_result mip_3dm_read_gnss_time_assistance(struct mip_interface* device, double* tow_out, uint16_t* week_number_out, float* accuracy_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1021,6 +1044,7 @@ mip_cmd_result mip_3dm_read_imu_lowpass_filter(struct mip_interface* device, uin mip_cmd_result mip_3dm_save_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); mip_cmd_result mip_3dm_load_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); mip_cmd_result mip_3dm_default_imu_lowpass_filter(struct mip_interface* device, uint8_t target_descriptor); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1063,6 +1087,7 @@ mip_cmd_result mip_3dm_read_pps_source(struct mip_interface* device, mip_3dm_pps mip_cmd_result mip_3dm_save_pps_source(struct mip_interface* device); mip_cmd_result mip_3dm_load_pps_source(struct mip_interface* device); mip_cmd_result mip_3dm_default_pps_source(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1161,6 +1186,7 @@ mip_cmd_result mip_3dm_read_gpio_config(struct mip_interface* device, uint8_t pi mip_cmd_result mip_3dm_save_gpio_config(struct mip_interface* device, uint8_t pin); mip_cmd_result mip_3dm_load_gpio_config(struct mip_interface* device, uint8_t pin); mip_cmd_result mip_3dm_default_gpio_config(struct mip_interface* device, uint8_t pin); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1207,6 +1233,7 @@ void extract_mip_3dm_gpio_state_response(struct mip_serializer* serializer, mip_ mip_cmd_result mip_3dm_write_gpio_state(struct mip_interface* device, uint8_t pin, bool state); mip_cmd_result mip_3dm_read_gpio_state(struct mip_interface* device, uint8_t pin, bool* state_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1251,6 +1278,7 @@ mip_cmd_result mip_3dm_read_odometer(struct mip_interface* device, mip_3dm_odome mip_cmd_result mip_3dm_save_odometer(struct mip_interface* device); mip_cmd_result mip_3dm_load_odometer(struct mip_interface* device); mip_cmd_result mip_3dm_default_odometer(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1314,6 +1342,7 @@ void insert_mip_3dm_get_event_support_response(struct mip_serializer* serializer void extract_mip_3dm_get_event_support_response(struct mip_serializer* serializer, mip_3dm_get_event_support_response* self); mip_cmd_result mip_3dm_get_event_support(struct mip_interface* device, mip_3dm_get_event_support_command_query query, uint8_t* max_instances_out, uint8_t* num_entries_out, uint8_t num_entries_out_max, mip_3dm_get_event_support_command_info* entries_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1366,6 +1395,7 @@ mip_cmd_result mip_3dm_read_event_control(struct mip_interface* device, uint8_t mip_cmd_result mip_3dm_save_event_control(struct mip_interface* device, uint8_t instance); mip_cmd_result mip_3dm_load_event_control(struct mip_interface* device, uint8_t instance); mip_cmd_result mip_3dm_default_event_control(struct mip_interface* device, uint8_t instance); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1414,6 +1444,7 @@ void insert_mip_3dm_get_event_trigger_status_response(struct mip_serializer* ser void extract_mip_3dm_get_event_trigger_status_response(struct mip_serializer* serializer, mip_3dm_get_event_trigger_status_response* self); mip_cmd_result mip_3dm_get_event_trigger_status(struct mip_interface* device, uint8_t requested_count, const uint8_t* requested_instances, uint8_t* count_out, uint8_t count_out_max, mip_3dm_get_event_trigger_status_command_entry* triggers_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1452,6 +1483,7 @@ void insert_mip_3dm_get_event_action_status_response(struct mip_serializer* seri void extract_mip_3dm_get_event_action_status_response(struct mip_serializer* serializer, mip_3dm_get_event_action_status_response* self); mip_cmd_result mip_3dm_get_event_action_status(struct mip_interface* device, uint8_t requested_count, const uint8_t* requested_instances, uint8_t* count_out, uint8_t count_out_max, mip_3dm_get_event_action_status_command_entry* actions_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1576,6 +1608,7 @@ mip_cmd_result mip_3dm_read_event_trigger(struct mip_interface* device, uint8_t mip_cmd_result mip_3dm_save_event_trigger(struct mip_interface* device, uint8_t instance); mip_cmd_result mip_3dm_load_event_trigger(struct mip_interface* device, uint8_t instance); mip_cmd_result mip_3dm_default_event_trigger(struct mip_interface* device, uint8_t instance); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1662,6 +1695,7 @@ mip_cmd_result mip_3dm_read_event_action(struct mip_interface* device, uint8_t i mip_cmd_result mip_3dm_save_event_action(struct mip_interface* device, uint8_t instance); mip_cmd_result mip_3dm_load_event_action(struct mip_interface* device, uint8_t instance); mip_cmd_result mip_3dm_default_event_action(struct mip_interface* device, uint8_t instance); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1675,7 +1709,7 @@ mip_cmd_result mip_3dm_default_event_action(struct mip_interface* device, uint8_ struct mip_3dm_accel_bias_command { mip_function_selector function; - float bias[3]; ///< accelerometer bias in the sensor frame (x,y,z) [g] + mip_vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] }; typedef struct mip_3dm_accel_bias_command mip_3dm_accel_bias_command; @@ -1684,18 +1718,19 @@ void extract_mip_3dm_accel_bias_command(struct mip_serializer* serializer, mip_3 struct mip_3dm_accel_bias_response { - float bias[3]; ///< accelerometer bias in the sensor frame (x,y,z) [g] + mip_vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] }; typedef struct mip_3dm_accel_bias_response mip_3dm_accel_bias_response; void insert_mip_3dm_accel_bias_response(struct mip_serializer* serializer, const mip_3dm_accel_bias_response* self); void extract_mip_3dm_accel_bias_response(struct mip_serializer* serializer, mip_3dm_accel_bias_response* self); -mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, const float* bias); -mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, float* bias_out); +mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, mip_vector3f bias); +mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, mip_vector3f bias_out); mip_cmd_result mip_3dm_save_accel_bias(struct mip_interface* device); mip_cmd_result mip_3dm_load_accel_bias(struct mip_interface* device); mip_cmd_result mip_3dm_default_accel_bias(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1709,7 +1744,7 @@ mip_cmd_result mip_3dm_default_accel_bias(struct mip_interface* device); struct mip_3dm_gyro_bias_command { mip_function_selector function; - float bias[3]; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + mip_vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] }; typedef struct mip_3dm_gyro_bias_command mip_3dm_gyro_bias_command; @@ -1718,18 +1753,19 @@ void extract_mip_3dm_gyro_bias_command(struct mip_serializer* serializer, mip_3d struct mip_3dm_gyro_bias_response { - float bias[3]; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + mip_vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] }; typedef struct mip_3dm_gyro_bias_response mip_3dm_gyro_bias_response; void insert_mip_3dm_gyro_bias_response(struct mip_serializer* serializer, const mip_3dm_gyro_bias_response* self); void extract_mip_3dm_gyro_bias_response(struct mip_serializer* serializer, mip_3dm_gyro_bias_response* self); -mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, const float* bias); -mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, float* bias_out); +mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, mip_vector3f bias); +mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, mip_vector3f bias_out); mip_cmd_result mip_3dm_save_gyro_bias(struct mip_interface* device); mip_cmd_result mip_3dm_load_gyro_bias(struct mip_interface* device); mip_cmd_result mip_3dm_default_gyro_bias(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1754,14 +1790,15 @@ void extract_mip_3dm_capture_gyro_bias_command(struct mip_serializer* serializer struct mip_3dm_capture_gyro_bias_response { - float bias[3]; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + mip_vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] }; typedef struct mip_3dm_capture_gyro_bias_response mip_3dm_capture_gyro_bias_response; void insert_mip_3dm_capture_gyro_bias_response(struct mip_serializer* serializer, const mip_3dm_capture_gyro_bias_response* self); void extract_mip_3dm_capture_gyro_bias_response(struct mip_serializer* serializer, mip_3dm_capture_gyro_bias_response* self); -mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, float* bias_out); +mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, mip_vector3f bias_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1779,7 +1816,7 @@ mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t struct mip_3dm_mag_hard_iron_offset_command { mip_function_selector function; - float offset[3]; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] + mip_vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] }; typedef struct mip_3dm_mag_hard_iron_offset_command mip_3dm_mag_hard_iron_offset_command; @@ -1788,18 +1825,19 @@ void extract_mip_3dm_mag_hard_iron_offset_command(struct mip_serializer* seriali struct mip_3dm_mag_hard_iron_offset_response { - float offset[3]; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] + mip_vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] }; typedef struct mip_3dm_mag_hard_iron_offset_response mip_3dm_mag_hard_iron_offset_response; void insert_mip_3dm_mag_hard_iron_offset_response(struct mip_serializer* serializer, const mip_3dm_mag_hard_iron_offset_response* self); void extract_mip_3dm_mag_hard_iron_offset_response(struct mip_serializer* serializer, mip_3dm_mag_hard_iron_offset_response* self); -mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, const float* offset); -mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, float* offset_out); +mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset); +mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset_out); mip_cmd_result mip_3dm_save_mag_hard_iron_offset(struct mip_interface* device); mip_cmd_result mip_3dm_load_mag_hard_iron_offset(struct mip_interface* device); mip_cmd_result mip_3dm_default_mag_hard_iron_offset(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1821,7 +1859,7 @@ mip_cmd_result mip_3dm_default_mag_hard_iron_offset(struct mip_interface* device struct mip_3dm_mag_soft_iron_matrix_command { mip_function_selector function; - float offset[9]; ///< soft iron matrix [dimensionless] + mip_matrix3f offset; ///< soft iron matrix [dimensionless] }; typedef struct mip_3dm_mag_soft_iron_matrix_command mip_3dm_mag_soft_iron_matrix_command; @@ -1830,18 +1868,19 @@ void extract_mip_3dm_mag_soft_iron_matrix_command(struct mip_serializer* seriali struct mip_3dm_mag_soft_iron_matrix_response { - float offset[9]; ///< soft iron matrix [dimensionless] + mip_matrix3f offset; ///< soft iron matrix [dimensionless] }; typedef struct mip_3dm_mag_soft_iron_matrix_response mip_3dm_mag_soft_iron_matrix_response; void insert_mip_3dm_mag_soft_iron_matrix_response(struct mip_serializer* serializer, const mip_3dm_mag_soft_iron_matrix_response* self); void extract_mip_3dm_mag_soft_iron_matrix_response(struct mip_serializer* serializer, mip_3dm_mag_soft_iron_matrix_response* self); -mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, const float* offset); -mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, float* offset_out); +mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset); +mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset_out); mip_cmd_result mip_3dm_save_mag_soft_iron_matrix(struct mip_interface* device); mip_cmd_result mip_3dm_load_mag_soft_iron_matrix(struct mip_interface* device); mip_cmd_result mip_3dm_default_mag_soft_iron_matrix(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1874,6 +1913,7 @@ mip_cmd_result mip_3dm_read_coning_sculling_enable(struct mip_interface* device, mip_cmd_result mip_3dm_save_coning_sculling_enable(struct mip_interface* device); mip_cmd_result mip_3dm_load_coning_sculling_enable(struct mip_interface* device); mip_cmd_result mip_3dm_default_coning_sculling_enable(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1934,6 +1974,7 @@ mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_euler(struct mip_interfac mip_cmd_result mip_3dm_save_sensor_2_vehicle_transform_euler(struct mip_interface* device); mip_cmd_result mip_3dm_load_sensor_2_vehicle_transform_euler(struct mip_interface* device); mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_euler(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1977,7 +2018,7 @@ mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_euler(struct mip_inter struct mip_3dm_sensor_2_vehicle_transform_quaternion_command { mip_function_selector function; - float q[4]; ///< Unit length quaternion representing transform [w, i, j, k] + mip_quatf q; ///< Unit length quaternion representing transform [w, i, j, k] }; typedef struct mip_3dm_sensor_2_vehicle_transform_quaternion_command mip_3dm_sensor_2_vehicle_transform_quaternion_command; @@ -1986,18 +2027,19 @@ void extract_mip_3dm_sensor_2_vehicle_transform_quaternion_command(struct mip_se struct mip_3dm_sensor_2_vehicle_transform_quaternion_response { - float q[4]; ///< Unit length quaternion representing transform [w, i, j, k] + mip_quatf q; ///< Unit length quaternion representing transform [w, i, j, k] }; typedef struct mip_3dm_sensor_2_vehicle_transform_quaternion_response mip_3dm_sensor_2_vehicle_transform_quaternion_response; void insert_mip_3dm_sensor_2_vehicle_transform_quaternion_response(struct mip_serializer* serializer, const mip_3dm_sensor_2_vehicle_transform_quaternion_response* self); void extract_mip_3dm_sensor_2_vehicle_transform_quaternion_response(struct mip_serializer* serializer, mip_3dm_sensor_2_vehicle_transform_quaternion_response* self); -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, const float* q); -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, float* q_out); +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q); +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q_out); mip_cmd_result mip_3dm_save_sensor_2_vehicle_transform_quaternion(struct mip_interface* device); mip_cmd_result mip_3dm_load_sensor_2_vehicle_transform_quaternion(struct mip_interface* device); mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_quaternion(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2039,7 +2081,7 @@ mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_quaternion(struct mip_ struct mip_3dm_sensor_2_vehicle_transform_dcm_command { mip_function_selector function; - float dcm[9]; ///< 3 x 3 direction cosine matrix, stored in row-major order + mip_matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order }; typedef struct mip_3dm_sensor_2_vehicle_transform_dcm_command mip_3dm_sensor_2_vehicle_transform_dcm_command; @@ -2048,18 +2090,19 @@ void extract_mip_3dm_sensor_2_vehicle_transform_dcm_command(struct mip_serialize struct mip_3dm_sensor_2_vehicle_transform_dcm_response { - float dcm[9]; ///< 3 x 3 direction cosine matrix, stored in row-major order + mip_matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order }; typedef struct mip_3dm_sensor_2_vehicle_transform_dcm_response mip_3dm_sensor_2_vehicle_transform_dcm_response; void insert_mip_3dm_sensor_2_vehicle_transform_dcm_response(struct mip_serializer* serializer, const mip_3dm_sensor_2_vehicle_transform_dcm_response* self); void extract_mip_3dm_sensor_2_vehicle_transform_dcm_response(struct mip_serializer* serializer, mip_3dm_sensor_2_vehicle_transform_dcm_response* self); -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, const float* dcm); -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, float* dcm_out); +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm); +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm_out); mip_cmd_result mip_3dm_save_sensor_2_vehicle_transform_dcm(struct mip_interface* device); mip_cmd_result mip_3dm_load_sensor_2_vehicle_transform_dcm(struct mip_interface* device); mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_dcm(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2102,6 +2145,7 @@ mip_cmd_result mip_3dm_read_complementary_filter(struct mip_interface* device, b mip_cmd_result mip_3dm_save_complementary_filter(struct mip_interface* device); mip_cmd_result mip_3dm_load_complementary_filter(struct mip_interface* device); mip_cmd_result mip_3dm_default_complementary_filter(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2143,6 +2187,7 @@ mip_cmd_result mip_3dm_read_sensor_range(struct mip_interface* device, mip_senso mip_cmd_result mip_3dm_save_sensor_range(struct mip_interface* device, mip_sensor_range_type sensor); mip_cmd_result mip_3dm_load_sensor_range(struct mip_interface* device, mip_sensor_range_type sensor); mip_cmd_result mip_3dm_default_sensor_range(struct mip_interface* device, mip_sensor_range_type sensor); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2185,6 +2230,7 @@ void insert_mip_3dm_calibrated_sensor_ranges_response(struct mip_serializer* ser void extract_mip_3dm_calibrated_sensor_ranges_response(struct mip_serializer* serializer, mip_3dm_calibrated_sensor_ranges_response* self); mip_cmd_result mip_3dm_calibrated_sensor_ranges(struct mip_interface* device, mip_sensor_range_type sensor, uint8_t* num_ranges_out, uint8_t num_ranges_out_max, mip_3dm_calibrated_sensor_ranges_command_entry* ranges_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2238,6 +2284,7 @@ mip_cmd_result mip_3dm_read_lowpass_filter(struct mip_interface* device, uint8_t mip_cmd_result mip_3dm_save_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); mip_cmd_result mip_3dm_load_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); mip_cmd_result mip_3dm_default_lowpass_filter(struct mip_interface* device, uint8_t desc_set, uint8_t field_desc); + ///@} /// diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index d354ccd76..bf45ba209 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -205,20 +206,28 @@ enum class SensorRangeType : uint8_t struct PollImuMessage { + bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. + uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. + DescriptorRate descriptors[83]; ///< Descriptor list. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000030; - bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. - uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. - DescriptorRate* descriptors = {nullptr}; ///< Descriptor list. + auto as_tuple() const + { + return std::make_tuple(suppress_ack,num_descriptors,descriptors); + } + typedef void Response; }; void insert(Serializer& serializer, const PollImuMessage& self); void extract(Serializer& serializer, PollImuMessage& self); CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -236,20 +245,28 @@ CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t num struct PollGnssMessage { + bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. + uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. + DescriptorRate descriptors[83]; ///< Descriptor list. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000030; - bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. - uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. - DescriptorRate* descriptors = {nullptr}; ///< Descriptor list. + auto as_tuple() const + { + return std::make_tuple(suppress_ack,num_descriptors,descriptors); + } + typedef void Response; }; void insert(Serializer& serializer, const PollGnssMessage& self); void extract(Serializer& serializer, PollGnssMessage& self); CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -267,20 +284,28 @@ CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t nu struct PollFilterMessage { + bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. + uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. + DescriptorRate descriptors[83]; ///< Descriptor format list. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000030; - bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. - uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. - DescriptorRate* descriptors = {nullptr}; ///< Descriptor format list. + auto as_tuple() const + { + return std::make_tuple(suppress_ack,num_descriptors,descriptors); + } + typedef void Response; }; void insert(Serializer& serializer, const PollFilterMessage& self); void extract(Serializer& serializer, PollFilterMessage& self); CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -293,26 +318,50 @@ CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t struct ImuMessageFormat { + FunctionSelector function = static_cast(0); + uint8_t num_descriptors = 0; ///< Number of descriptors + DescriptorRate descriptors[82]; ///< Descriptor format list. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; + + auto as_tuple() const + { + return std::make_tuple(num_descriptors,descriptors); + } - FunctionSelector function = static_cast(0); - uint8_t num_descriptors = 0; ///< Number of descriptors - DescriptorRate* descriptors = {nullptr}; ///< Descriptor format list. + + static ImuMessageFormat create_sld_all(::mip::FunctionSelector function) + { + ImuMessageFormat cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors - DescriptorRate* descriptors = {nullptr}; ///< Descriptor format list. + DescriptorRate descriptors[82]; ///< Descriptor format list. + + + auto as_tuple() + { + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + } }; }; @@ -327,6 +376,7 @@ CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptors CmdResult saveImuMessageFormat(C::mip_interface& device); CmdResult loadImuMessageFormat(C::mip_interface& device); CmdResult defaultImuMessageFormat(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -339,26 +389,50 @@ CmdResult defaultImuMessageFormat(C::mip_interface& device); struct GpsMessageFormat { + FunctionSelector function = static_cast(0); + uint8_t num_descriptors = 0; ///< Number of descriptors + DescriptorRate descriptors[82]; ///< Descriptor format list. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; - FunctionSelector function = static_cast(0); - uint8_t num_descriptors = 0; ///< Number of descriptors - DescriptorRate* descriptors = {nullptr}; ///< Descriptor format list. + auto as_tuple() const + { + return std::make_tuple(num_descriptors,descriptors); + } + + + static GpsMessageFormat create_sld_all(::mip::FunctionSelector function) + { + GpsMessageFormat cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors - DescriptorRate* descriptors = {nullptr}; ///< Descriptor format list. + DescriptorRate descriptors[82]; ///< Descriptor format list. + + + auto as_tuple() + { + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + } }; }; @@ -373,6 +447,7 @@ CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptors CmdResult saveGpsMessageFormat(C::mip_interface& device); CmdResult loadGpsMessageFormat(C::mip_interface& device); CmdResult defaultGpsMessageFormat(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -385,26 +460,50 @@ CmdResult defaultGpsMessageFormat(C::mip_interface& device); struct FilterMessageFormat { + FunctionSelector function = static_cast(0); + uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) + DescriptorRate descriptors[82]; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; + + auto as_tuple() const + { + return std::make_tuple(num_descriptors,descriptors); + } - FunctionSelector function = static_cast(0); - uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) - DescriptorRate* descriptors = {nullptr}; + + static FilterMessageFormat create_sld_all(::mip::FunctionSelector function) + { + FilterMessageFormat cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) - DescriptorRate* descriptors = {nullptr}; + DescriptorRate descriptors[82]; + + + auto as_tuple() + { + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + } }; }; @@ -419,6 +518,7 @@ CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescript CmdResult saveFilterMessageFormat(C::mip_interface& device); CmdResult loadFilterMessageFormat(C::mip_interface& device); CmdResult defaultFilterMessageFormat(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -432,19 +532,34 @@ CmdResult defaultFilterMessageFormat(C::mip_interface& device); struct ImuGetBaseRate { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] + + auto as_tuple() + { + return std::make_tuple(std::ref(rate)); + } + }; }; void insert(Serializer& serializer, const ImuGetBaseRate& self); @@ -454,6 +569,7 @@ void insert(Serializer& serializer, const ImuGetBaseRate::Response& self); void extract(Serializer& serializer, ImuGetBaseRate::Response& self); CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -467,19 +583,34 @@ CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); struct GpsGetBaseRate { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] + + auto as_tuple() + { + return std::make_tuple(std::ref(rate)); + } + }; }; void insert(Serializer& serializer, const GpsGetBaseRate& self); @@ -489,6 +620,7 @@ void insert(Serializer& serializer, const GpsGetBaseRate::Response& self); void extract(Serializer& serializer, GpsGetBaseRate::Response& self); CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -502,19 +634,34 @@ CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); struct FilterGetBaseRate { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] + + auto as_tuple() + { + return std::make_tuple(std::ref(rate)); + } + }; }; void insert(Serializer& serializer, const FilterGetBaseRate& self); @@ -524,6 +671,7 @@ void insert(Serializer& serializer, const FilterGetBaseRate::Response& self); void extract(Serializer& serializer, FilterGetBaseRate::Response& self); CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -541,21 +689,29 @@ CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); struct PollData { + uint8_t desc_set = 0; ///< Data descriptor set. Must be supported. + bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. + uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. + uint8_t descriptors[82] = {0}; ///< Descriptor format list. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x000000C0; - uint8_t desc_set = 0; ///< Data descriptor set. Must be supported. - bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. - uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. - uint8_t* descriptors = {nullptr}; ///< Descriptor format list. + auto as_tuple() const + { + return std::make_tuple(desc_set,suppress_ack,num_descriptors,descriptors); + } + typedef void Response; }; void insert(Serializer& serializer, const PollData& self); void extract(Serializer& serializer, PollData& self); CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -566,21 +722,36 @@ CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, struct GetBaseRate { + uint8_t desc_set = 0; ///< This is the data descriptor set. It must be a supported descriptor. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; - uint8_t desc_set = 0; ///< This is the data descriptor set. It must be a supported descriptor. + auto as_tuple() const + { + return std::make_tuple(desc_set); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; ///< Echoes the parameter in the command. uint16_t rate = 0; ///< Base rate in Hz (0 = variable, unknown, or user-defined rate. Data will be sent when received). + + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(rate)); + } + }; }; void insert(Serializer& serializer, const GetBaseRate& self); @@ -590,6 +761,7 @@ void insert(Serializer& serializer, const GetBaseRate::Response& self); void extract(Serializer& serializer, GetBaseRate::Response& self); CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -602,28 +774,53 @@ CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateO struct MessageFormat { + FunctionSelector function = static_cast(0); + uint8_t desc_set = 0; ///< Data descriptor set. Must be supported. When function is SAVE, LOAD, or DEFAULT, can be 0 to apply to all descriptor sets. + uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) + DescriptorRate descriptors[82]; ///< List of descriptors and decimations. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000030; - FunctionSelector function = static_cast(0); - uint8_t desc_set = 0; ///< Data descriptor set. Must be supported. When function is SAVE, LOAD, or DEFAULT, can be 0 to apply to all descriptor sets. - uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) - DescriptorRate* descriptors = {nullptr}; ///< List of descriptors and decimations. + auto as_tuple() const + { + return std::make_tuple(desc_set,num_descriptors,descriptors); + } + + + static MessageFormat create_sld_all(::mip::FunctionSelector function) + { + MessageFormat cmd; + cmd.function = function; + cmd.desc_set = 0; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000030; uint8_t desc_set = 0; ///< Echoes the descriptor set from the command. uint8_t num_descriptors = 0; ///< Number of descriptors in the list. - DescriptorRate* descriptors = {nullptr}; ///< List of descriptors and decimations. + DescriptorRate descriptors[82]; ///< List of descriptors and decimations. + + + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(num_descriptors),std::ref(descriptors)); + } }; }; @@ -638,6 +835,7 @@ CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet); CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet); CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -654,20 +852,28 @@ CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); struct NmeaPollData { + bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. + uint8_t count = 0; ///< Number of format entries (limited by payload size) + NmeaMessage format_entries[40]; ///< List of format entries. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000030; - bool suppress_ack = 0; ///< Suppress the usual ACK/NACK reply. - uint8_t count = 0; ///< Number of format entries (limited by payload size) - NmeaMessage* format_entries = {nullptr}; ///< List of format entries. + auto as_tuple() const + { + return std::make_tuple(suppress_ack,count,format_entries); + } + typedef void Response; }; void insert(Serializer& serializer, const NmeaPollData& self); void extract(Serializer& serializer, NmeaPollData& self); CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -678,26 +884,50 @@ CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count struct NmeaMessageFormat { + FunctionSelector function = static_cast(0); + uint8_t count = 0; ///< Number of format entries (limited by payload size) + NmeaMessage format_entries[40]; ///< List of format entries. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; + + auto as_tuple() const + { + return std::make_tuple(count,format_entries); + } - FunctionSelector function = static_cast(0); - uint8_t count = 0; ///< Number of format entries (limited by payload size) - NmeaMessage* format_entries = {nullptr}; ///< List of format entries. + + static NmeaMessageFormat create_sld_all(::mip::FunctionSelector function) + { + NmeaMessageFormat cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of format entries (limited by payload size) - NmeaMessage* format_entries = {nullptr}; ///< List of format entries. + NmeaMessage format_entries[40]; ///< List of format entries. + + + auto as_tuple() + { + return std::make_tuple(std::ref(count),std::ref(format_entries)); + } }; }; @@ -712,6 +942,7 @@ CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uin CmdResult saveNmeaMessageFormat(C::mip_interface& device); CmdResult loadNmeaMessageFormat(C::mip_interface& device); CmdResult defaultNmeaMessageFormat(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -726,17 +957,33 @@ CmdResult defaultNmeaMessageFormat(C::mip_interface& device); struct DeviceSettings { + FunctionSelector function = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; - static const bool HAS_WRITE_FUNCTION = false; - static const bool HAS_READ_FUNCTION = false; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x0000; + static const uint32_t READ_PARAMS = 0x0000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(); + } - FunctionSelector function = static_cast(0); + static DeviceSettings create_sld_all(::mip::FunctionSelector function) + { + DeviceSettings cmd; + cmd.function = function; + return cmd; + } + + typedef void Response; }; void insert(Serializer& serializer, const DeviceSettings& self); void extract(Serializer& serializer, DeviceSettings& self); @@ -744,6 +991,7 @@ void extract(Serializer& serializer, DeviceSettings& self); CmdResult saveDeviceSettings(C::mip_interface& device); CmdResult loadDeviceSettings(C::mip_interface& device); CmdResult defaultDeviceSettings(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -768,25 +1016,49 @@ CmdResult defaultDeviceSettings(C::mip_interface& device); struct UartBaudrate { + FunctionSelector function = static_cast(0); + uint32_t baud = 0; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(baud); + } - FunctionSelector function = static_cast(0); - uint32_t baud = 0; + + static UartBaudrate create_sld_all(::mip::FunctionSelector function) + { + UartBaudrate cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint32_t baud = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(baud)); + } + }; }; void insert(Serializer& serializer, const UartBaudrate& self); @@ -800,6 +1072,7 @@ CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut); CmdResult saveUartBaudrate(C::mip_interface& device); CmdResult loadUartBaudrate(C::mip_interface& device); CmdResult defaultUartBaudrate(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -813,11 +1086,6 @@ CmdResult defaultUartBaudrate(C::mip_interface& device); struct FactoryStreaming { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; - - static const bool HAS_FUNCTION_SELECTOR = false; - enum class Action : uint8_t { OVERWRITE = 0, ///< Replaces the message format(s), removing any existing descriptors. @@ -828,11 +1096,24 @@ struct FactoryStreaming Action action = static_cast(0); uint8_t reserved = 0; ///< Reserved. Set to 0x00. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(action,reserved); + } + + typedef void Response; }; void insert(Serializer& serializer, const FactoryStreaming& self); void extract(Serializer& serializer, FactoryStreaming& self); CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -848,15 +1129,6 @@ CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action ac struct DatastreamControl { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - static const uint8_t LEGACY_IMU_STREAM = 0x01; static const uint8_t LEGACY_GNSS_STREAM = 0x02; static const uint8_t LEGACY_FILTER_STREAM = 0x03; @@ -865,14 +1137,48 @@ struct DatastreamControl uint8_t desc_set = 0; ///< The descriptor set of the stream to control. When function is SAVE, LOAD, or DEFAULT, can be ALL_STREAMS(0) to apply to all descriptor sets. On Generation 5 products, this must be one of the above legacy constants. bool enable = 0; ///< True or false to enable or disable the stream. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(desc_set,enable); + } + + + static DatastreamControl create_sld_all(::mip::FunctionSelector function) + { + DatastreamControl cmd; + cmd.function = function; + cmd.desc_set = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; bool enabled = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(enabled)); + } + }; }; void insert(Serializer& serializer, const DatastreamControl& self); @@ -886,6 +1192,7 @@ CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet); CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet); CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -913,15 +1220,6 @@ CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); struct ConstellationSettings { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class ConstellationId : uint8_t { GPS = 0, ///< GPS (G1-G32) @@ -969,17 +1267,50 @@ struct ConstellationSettings FunctionSelector function = static_cast(0); uint16_t max_channels = 0; uint8_t config_count = 0; - Settings* settings = {nullptr}; + Settings settings[42]; + + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000030; + + auto as_tuple() const + { + return std::make_tuple(max_channels,config_count,settings); + } + + + static ConstellationSettings create_sld_all(::mip::FunctionSelector function) + { + ConstellationSettings cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x000000C0; uint16_t max_channels_available = 0; ///< Maximum channels available uint16_t max_channels_use = 0; ///< Maximum channels to use uint8_t config_count = 0; ///< Number of constellation configurations - Settings* settings = {nullptr}; ///< Constellation Settings + Settings settings[42]; ///< Constellation Settings + + + auto as_tuple() + { + return std::make_tuple(std::ref(max_channels_available),std::ref(max_channels_use),std::ref(config_count),std::ref(settings)); + } }; }; @@ -997,6 +1328,7 @@ CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChann CmdResult saveConstellationSettings(C::mip_interface& device); CmdResult loadConstellationSettings(C::mip_interface& device); CmdResult defaultConstellationSettings(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1010,15 +1342,6 @@ CmdResult defaultConstellationSettings(C::mip_interface& device); struct GnssSbasSettings { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - struct SBASOptions : Bitfield { enum _enumType : uint16_t @@ -1054,17 +1377,50 @@ struct GnssSbasSettings uint8_t enable_sbas = 0; ///< 0 - SBAS Disabled, 1 - SBAS enabled SBASOptions sbas_options; ///< SBAS options, see definition uint8_t num_included_prns = 0; ///< Number of SBAS PRNs to include in search (0 = include all) - uint16_t* included_prns = {nullptr}; ///< List of specific SBAS PRNs to search for + uint16_t included_prns[39] = {0}; ///< List of specific SBAS PRNs to search for + + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x800F; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x000000C0; + + auto as_tuple() const + { + return std::make_tuple(enable_sbas,sbas_options,num_included_prns,included_prns); + } + + + static GnssSbasSettings create_sld_all(::mip::FunctionSelector function) + { + GnssSbasSettings cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x000000C0; uint8_t enable_sbas = 0; ///< 0 - SBAS Disabled, 1 - SBAS enabled SBASOptions sbas_options; ///< SBAS options, see definition uint8_t num_included_prns = 0; ///< Number of SBAS PRNs to include in search (0 = include all) - uint16_t* included_prns = {nullptr}; ///< List of specific SBAS PRNs to search for + uint16_t included_prns[39] = {0}; ///< List of specific SBAS PRNs to search for + + + auto as_tuple() + { + return std::make_tuple(std::ref(enable_sbas),std::ref(sbas_options),std::ref(num_included_prns),std::ref(included_prns)); + } }; }; @@ -1079,6 +1435,7 @@ CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, CmdResult saveGnssSbasSettings(C::mip_interface& device); CmdResult loadGnssSbasSettings(C::mip_interface& device); CmdResult defaultGnssSbasSettings(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1099,15 +1456,6 @@ CmdResult defaultGnssSbasSettings(C::mip_interface& device); struct GnssAssistedFix { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class AssistedFixOption : uint8_t { NONE = 0, ///< No assisted fix (default) @@ -1118,14 +1466,47 @@ struct GnssAssistedFix AssistedFixOption option = static_cast(0); ///< Assisted fix options uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(option,flags); + } + + + static GnssAssistedFix create_sld_all(::mip::FunctionSelector function) + { + GnssAssistedFix cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; AssistedFixOption option = static_cast(0); ///< Assisted fix options uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) + + auto as_tuple() + { + return std::make_tuple(std::ref(option),std::ref(flags)); + } + }; }; void insert(Serializer& serializer, const GnssAssistedFix& self); @@ -1139,6 +1520,7 @@ CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::Assiste CmdResult saveGnssAssistedFix(C::mip_interface& device); CmdResult loadGnssAssistedFix(C::mip_interface& device); CmdResult defaultGnssAssistedFix(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1152,29 +1534,53 @@ CmdResult defaultGnssAssistedFix(C::mip_interface& device); struct GnssTimeAssistance { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = false; - static const bool HAS_LOAD_FUNCTION = false; - static const bool HAS_RESET_FUNCTION = false; - FunctionSelector function = static_cast(0); double tow = 0; ///< GPS Time of week [seconds] uint16_t week_number = 0; ///< GPS Weeks since 1980 [weeks] float accuracy = 0; ///< Accuracy of time information [seconds] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x0000; + static const uint32_t LOAD_PARAMS = 0x0000; + static const uint32_t DEFAULT_PARAMS = 0x0000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(tow,week_number,accuracy); + } + + + static GnssTimeAssistance create_sld_all(::mip::FunctionSelector function) + { + GnssTimeAssistance cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; double tow = 0; ///< GPS Time of week [seconds] uint16_t week_number = 0; ///< GPS Weeks since 1980 [weeks] float accuracy = 0; ///< Accuracy of time information [seconds] + + auto as_tuple() + { + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(accuracy)); + } + }; }; void insert(Serializer& serializer, const GnssTimeAssistance& self); @@ -1185,6 +1591,7 @@ void extract(Serializer& serializer, GnssTimeAssistance::Response& self); CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy); CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1210,15 +1617,6 @@ CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint1 struct ImuLowpassFilter { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); uint8_t target_descriptor = 0; ///< Field descriptor of filtered quantity within the Sensor data set. Supported values are accel (0x04), gyro (0x05), mag (0x06), and pressure (0x17), provided the data is supported by the device. Except with the READ function selector, this can be 0 to apply to all of the above quantities. bool enable = 0; ///< The target data will be filtered if this is true. @@ -1226,17 +1624,51 @@ struct ImuLowpassFilter uint16_t frequency = 0; ///< -3dB cutoff frequency in Hz. Will not affect filtering if 'manual' is false. uint8_t reserved = 0; ///< Reserved, set to 0x00. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x801F; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(target_descriptor,enable,manual,frequency,reserved); + } + + + static ImuLowpassFilter create_sld_all(::mip::FunctionSelector function) + { + ImuLowpassFilter cmd; + cmd.function = function; + cmd.target_descriptor = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t target_descriptor = 0; bool enable = 0; ///< True if the filter is currently enabled. bool manual = 0; ///< True if the filter cutoff was manually configured. uint16_t frequency = 0; ///< The cutoff frequency of the filter. If the filter is in auto mode, this value is unspecified. uint8_t reserved = 0; ///< Reserved and must be ignored. + + auto as_tuple() + { + return std::make_tuple(std::ref(target_descriptor),std::ref(enable),std::ref(manual),std::ref(frequency),std::ref(reserved)); + } + }; }; void insert(Serializer& serializer, const ImuLowpassFilter& self); @@ -1250,6 +1682,7 @@ CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1260,15 +1693,6 @@ CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescri struct PpsSource { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Source : uint8_t { DISABLED = 0, ///< PPS output is disabled. Not valid for PPS source command. @@ -1281,13 +1705,46 @@ struct PpsSource FunctionSelector function = static_cast(0); Source source = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(source); + } + + + static PpsSource create_sld_all(::mip::FunctionSelector function) + { + PpsSource cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); + + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } + }; }; void insert(Serializer& serializer, const PpsSource& self); @@ -1301,6 +1758,7 @@ CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut); CmdResult savePpsSource(C::mip_interface& device); CmdResult loadPpsSource(C::mip_interface& device); CmdResult defaultPpsSource(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1327,15 +1785,6 @@ CmdResult defaultPpsSource(C::mip_interface& device); struct GpioConfig { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Feature : uint8_t { UNUSED = 0, ///< The pin is not used. It may be technically possible to read the pin state in this mode, but this is not guaranteed to be true of all devices or pins. @@ -1405,16 +1854,50 @@ struct GpioConfig Behavior behavior = static_cast(0); ///< Select an appropriate value from the enumeration based on the selected feature (e.g. for PPS, select one of the values prefixed with PPS_.) PinMode pin_mode; ///< GPIO configuration. May be restricted depending on device, pin, feature, and behavior. See device user manual. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x800F; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(pin,feature,behavior,pin_mode); + } + + + static GpioConfig create_sld_all(::mip::FunctionSelector function) + { + GpioConfig cmd; + cmd.function = function; + cmd.pin = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t pin = 0; ///< GPIO pin number counting from 1. For save, load, and default function selectors, this can be 0 to select all pins. Feature feature = static_cast(0); ///< Determines how the pin will be used. Behavior behavior = static_cast(0); ///< Select an appropriate value from the enumeration based on the selected feature (e.g. for PPS, select one of the values prefixed with PPS_.) PinMode pin_mode; ///< GPIO configuration. May be restricted depending on device, pin, feature, and behavior. See device user manual. + + auto as_tuple() + { + return std::make_tuple(std::ref(pin),std::ref(feature),std::ref(behavior),std::ref(pin_mode)); + } + }; }; void insert(Serializer& serializer, const GpioConfig& self); @@ -1428,6 +1911,7 @@ CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feat CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin); CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin); CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1453,27 +1937,51 @@ CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); struct GpioState { + FunctionSelector function = static_cast(0); + uint8_t pin = 0; ///< GPIO pin number counting from 1. Cannot be 0. + bool state = 0; ///< The pin state. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = false; - static const bool HAS_LOAD_FUNCTION = false; - static const bool HAS_RESET_FUNCTION = false; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x0000; + static const uint32_t LOAD_PARAMS = 0x0000; + static const uint32_t DEFAULT_PARAMS = 0x0000; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(pin,state); + } - FunctionSelector function = static_cast(0); - uint8_t pin = 0; ///< GPIO pin number counting from 1. Cannot be 0. - bool state = 0; ///< The pin state. + + static GpioState create_sld_all(::mip::FunctionSelector function) + { + GpioState cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t pin = 0; ///< GPIO pin number counting from 1. Cannot be 0. bool state = 0; ///< The pin state. + + auto as_tuple() + { + return std::make_tuple(std::ref(pin),std::ref(state)); + } + }; }; void insert(Serializer& serializer, const GpioState& self); @@ -1484,6 +1992,7 @@ void extract(Serializer& serializer, GpioState::Response& self); CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state); CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1495,15 +2004,6 @@ CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); struct Odometer { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Mode : uint8_t { DISABLED = 0, ///< Encoder is disabled. @@ -1515,15 +2015,48 @@ struct Odometer float scaling = 0; ///< Encoder pulses per meter of distance traveled [pulses/m]. Distance traveled is computed using the formula d = p / N * 2R * pi, where d is distance, p is the number of pulses received, N is the encoder resolution, and R is the wheel radius. By simplifying all of the parameters into one, the formula d = p / S is obtained, where s is the odometer scaling factor passed to this command. S is equivalent to N / (2R * pi) and has units of pulses / meter. N is in units of "A" pulses per revolution and R is in meters. Make this value negative if the odometer is mounted so that it rotates backwards. float uncertainty = 0; ///< Uncertainty in encoder counts to distance translation (1-sigma value) [m/m]. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(mode,scaling,uncertainty); + } + + + static Odometer create_sld_all(::mip::FunctionSelector function) + { + Odometer cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; Mode mode = static_cast(0); ///< Mode setting. float scaling = 0; ///< Encoder pulses per meter of distance traveled [pulses/m]. Distance traveled is computed using the formula d = p / N * 2R * pi, where d is distance, p is the number of pulses received, N is the encoder resolution, and R is the wheel radius. By simplifying all of the parameters into one, the formula d = p / S is obtained, where s is the odometer scaling factor passed to this command. S is equivalent to N / (2R * pi) and has units of pulses / meter. N is in units of "A" pulses per revolution and R is in meters. Make this value negative if the odometer is mounted so that it rotates backwards. float uncertainty = 0; ///< Uncertainty in encoder counts to distance translation (1-sigma value) [m/m]. + + auto as_tuple() + { + return std::make_tuple(std::ref(mode),std::ref(scaling),std::ref(uncertainty)); + } + }; }; void insert(Serializer& serializer, const Odometer& self); @@ -1537,6 +2070,7 @@ CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* CmdResult saveOdometer(C::mip_interface& device); CmdResult loadOdometer(C::mip_interface& device); CmdResult defaultOdometer(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1563,11 +2097,6 @@ CmdResult defaultOdometer(C::mip_interface& device); struct GetEventSupport { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; - - static const bool HAS_FUNCTION_SELECTOR = false; - enum class Query : uint8_t { TRIGGER_TYPES = 1, ///< Query the supported trigger types and max count for each. @@ -1582,16 +2111,36 @@ struct GetEventSupport }; Query query = static_cast(0); ///< What type of information to retrieve. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(query); + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x000000C0; Query query = static_cast(0); ///< Query type specified in the command. uint8_t max_instances = 0; ///< Number of slots available. The 'instance' number for the configuration or control commands must be between 1 and this value. uint8_t num_entries = 0; ///< Number of supported types. Info entries[126]; ///< List of supported types. + + auto as_tuple() + { + return std::make_tuple(std::ref(query),std::ref(max_instances),std::ref(num_entries),std::ref(entries)); + } + }; }; void insert(Serializer& serializer, const GetEventSupport& self); @@ -1604,6 +2153,7 @@ void insert(Serializer& serializer, const GetEventSupport::Response& self); void extract(Serializer& serializer, GetEventSupport::Response& self); CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1623,15 +2173,6 @@ CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query struct EventControl { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Mode : uint8_t { DISABLED = 0, ///< Trigger is disabled. @@ -1644,14 +2185,48 @@ struct EventControl uint8_t instance = 0; ///< Trigger instance to affect. 0 can be used to apply the mode to all configured triggers, except when the function selector is READ. Mode mode = static_cast(0); ///< How to change the trigger state. Except when instance is 0, the corresponding trigger must be configured, i.e. not have type 0. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(instance,mode); + } + + + static EventControl create_sld_all(::mip::FunctionSelector function) + { + EventControl cmd; + cmd.function = function; + cmd.instance = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Trigger instance to affect. 0 can be used to apply the mode to all configured triggers, except when the function selector is READ. Mode mode = static_cast(0); ///< How to change the trigger state. Except when instance is 0, the corresponding trigger must be configured, i.e. not have type 0. + + auto as_tuple() + { + return std::make_tuple(std::ref(instance),std::ref(mode)); + } + }; }; void insert(Serializer& serializer, const EventControl& self); @@ -1665,6 +2240,7 @@ CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventCont CmdResult saveEventControl(C::mip_interface& device, uint8_t instance); CmdResult loadEventControl(C::mip_interface& device, uint8_t instance); CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1674,11 +2250,6 @@ CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance); struct GetEventTriggerStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct Status : Bitfield { enum _enumType : uint8_t @@ -1719,14 +2290,34 @@ struct GetEventTriggerStatus uint8_t requested_count = 0; ///< Number of entries requested. If 0, requests all trigger slots. uint8_t requested_instances[20] = {0}; ///< List of trigger instances to query. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; + + auto as_tuple() const + { + return std::make_tuple(requested_count,requested_instances); + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of entries requested. If requested_count was 0, this is the number of supported trigger slots. Entry triggers[20]; ///< A list of the configured triggers. Entries are in the order requested, or in increasing order if count was 0. + + auto as_tuple() + { + return std::make_tuple(std::ref(count),std::ref(triggers)); + } + }; }; void insert(Serializer& serializer, const GetEventTriggerStatus& self); @@ -1739,6 +2330,7 @@ void insert(Serializer& serializer, const GetEventTriggerStatus::Response& self) void extract(Serializer& serializer, GetEventTriggerStatus::Response& self); CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1748,11 +2340,6 @@ CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount struct GetEventActionStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct Entry { uint8_t action_type = 0; ///< Configured action type. @@ -1762,14 +2349,34 @@ struct GetEventActionStatus uint8_t requested_count = 0; ///< Number of entries requested. If 0, requests all action slots. uint8_t requested_instances[20] = {0}; ///< List of action instances to query. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; + + auto as_tuple() const + { + return std::make_tuple(requested_count,requested_instances); + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of entries requested. If requested_count was 0, this is the number of supported action slots. Entry actions[20]; ///< A list of the configured actions. Entries are in the order requested, or in increasing order if count was 0. + + auto as_tuple() + { + return std::make_tuple(std::ref(count),std::ref(actions)); + } + }; }; void insert(Serializer& serializer, const GetEventActionStatus& self); @@ -1782,6 +2389,7 @@ void insert(Serializer& serializer, const GetEventActionStatus::Response& self); void extract(Serializer& serializer, GetEventActionStatus::Response& self); CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1792,15 +2400,6 @@ CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, struct EventTrigger { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - struct GpioParams { enum class Mode : uint8_t @@ -1879,15 +2478,49 @@ struct EventTrigger Type type = static_cast(0); ///< Type of trigger to configure. Parameters parameters; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(instance,type,parameters); + } + + + static EventTrigger create_sld_all(::mip::FunctionSelector function) + { + EventTrigger cmd; + cmd.function = function; + cmd.instance = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Trigger number. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all instances. Type type = static_cast(0); ///< Type of trigger to configure. Parameters parameters; + + auto as_tuple() + { + return std::make_tuple(std::ref(instance),std::ref(type),std::ref(parameters)); + } + }; }; void insert(Serializer& serializer, const EventTrigger& self); @@ -1910,6 +2543,7 @@ CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrig CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance); CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance); CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1920,15 +2554,6 @@ CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); struct EventAction { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - struct GpioParams { enum class Mode : uint8_t @@ -1973,16 +2598,50 @@ struct EventAction Type type = static_cast(0); ///< Type of action to configure. Parameters parameters; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x800F; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(instance,trigger,type,parameters); + } + + + static EventAction create_sld_all(::mip::FunctionSelector function) + { + EventAction cmd; + cmd.function = function; + cmd.instance = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Action number. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all instances. uint8_t trigger = 0; ///< Trigger ID number. Type type = static_cast(0); ///< Type of action to configure. Parameters parameters; + + auto as_tuple() + { + return std::make_tuple(std::ref(instance),std::ref(trigger),std::ref(type),std::ref(parameters)); + } + }; }; void insert(Serializer& serializer, const EventAction& self); @@ -2002,6 +2661,7 @@ CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* t CmdResult saveEventAction(C::mip_interface& device, uint8_t instance); CmdResult loadEventAction(C::mip_interface& device, uint8_t instance); CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2014,24 +2674,48 @@ CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance); struct AccelBias { + FunctionSelector function = static_cast(0); + Vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(bias); + } - FunctionSelector function = static_cast(0); - float bias[3] = {0}; ///< accelerometer bias in the sensor frame (x,y,z) [g] + + static AccelBias create_sld_all(::mip::FunctionSelector function) + { + AccelBias cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; - float bias[3] = {0}; ///< accelerometer bias in the sensor frame (x,y,z) [g] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] + + + auto as_tuple() + { + return std::make_tuple(std::ref(bias)); + } }; }; @@ -2041,11 +2725,12 @@ void extract(Serializer& serializer, AccelBias& self); void insert(Serializer& serializer, const AccelBias::Response& self); void extract(Serializer& serializer, AccelBias::Response& self); -CmdResult writeAccelBias(C::mip_interface& device, const float* bias); -CmdResult readAccelBias(C::mip_interface& device, float* biasOut); +CmdResult writeAccelBias(C::mip_interface& device, Vector3f bias); +CmdResult readAccelBias(C::mip_interface& device, Vector3f biasOut); CmdResult saveAccelBias(C::mip_interface& device); CmdResult loadAccelBias(C::mip_interface& device); CmdResult defaultAccelBias(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2058,24 +2743,48 @@ CmdResult defaultAccelBias(C::mip_interface& device); struct GyroBias { + FunctionSelector function = static_cast(0); + Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float bias[3] = {0}; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + auto as_tuple() const + { + return std::make_tuple(bias); + } + + + static GyroBias create_sld_all(::mip::FunctionSelector function) + { + GyroBias cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; - float bias[3] = {0}; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + + + auto as_tuple() + { + return std::make_tuple(std::ref(bias)); + } }; }; @@ -2085,11 +2794,12 @@ void extract(Serializer& serializer, GyroBias& self); void insert(Serializer& serializer, const GyroBias::Response& self); void extract(Serializer& serializer, GyroBias::Response& self); -CmdResult writeGyroBias(C::mip_interface& device, const float* bias); -CmdResult readGyroBias(C::mip_interface& device, float* biasOut); +CmdResult writeGyroBias(C::mip_interface& device, Vector3f bias); +CmdResult readGyroBias(C::mip_interface& device, Vector3f biasOut); CmdResult saveGyroBias(C::mip_interface& device); CmdResult loadGyroBias(C::mip_interface& device); CmdResult defaultGyroBias(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2105,19 +2815,34 @@ CmdResult defaultGyroBias(C::mip_interface& device); struct CaptureGyroBias { + uint16_t averaging_time_ms = 0; ///< Averaging time [milliseconds] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - uint16_t averaging_time_ms = 0; ///< Averaging time [milliseconds] + auto as_tuple() const + { + return std::make_tuple(averaging_time_ms); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; - float bias[3] = {0}; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] + + + auto as_tuple() + { + return std::make_tuple(std::ref(bias)); + } }; }; @@ -2127,7 +2852,8 @@ void extract(Serializer& serializer, CaptureGyroBias& self); void insert(Serializer& serializer, const CaptureGyroBias::Response& self); void extract(Serializer& serializer, CaptureGyroBias::Response& self); -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); +CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, Vector3f biasOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2144,24 +2870,48 @@ CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, fl struct MagHardIronOffset { + FunctionSelector function = static_cast(0); + Vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float offset[3] = {0}; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] + auto as_tuple() const + { + return std::make_tuple(offset); + } + + + static MagHardIronOffset create_sld_all(::mip::FunctionSelector function) + { + MagHardIronOffset cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; - float offset[3] = {0}; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] + + + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } }; }; @@ -2171,11 +2921,12 @@ void extract(Serializer& serializer, MagHardIronOffset& self); void insert(Serializer& serializer, const MagHardIronOffset::Response& self); void extract(Serializer& serializer, MagHardIronOffset::Response& self); -CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); -CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); +CmdResult writeMagHardIronOffset(C::mip_interface& device, Vector3f offset); +CmdResult readMagHardIronOffset(C::mip_interface& device, Vector3f offsetOut); CmdResult saveMagHardIronOffset(C::mip_interface& device); CmdResult loadMagHardIronOffset(C::mip_interface& device); CmdResult defaultMagHardIronOffset(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2196,24 +2947,48 @@ CmdResult defaultMagHardIronOffset(C::mip_interface& device); struct MagSoftIronMatrix { + FunctionSelector function = static_cast(0); + Matrix3f offset; ///< soft iron matrix [dimensionless] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float offset[9] = {0}; ///< soft iron matrix [dimensionless] + auto as_tuple() const + { + return std::make_tuple(offset); + } + + + static MagSoftIronMatrix create_sld_all(::mip::FunctionSelector function) + { + MagSoftIronMatrix cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; - float offset[9] = {0}; ///< soft iron matrix [dimensionless] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Matrix3f offset; ///< soft iron matrix [dimensionless] + + + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } }; }; @@ -2223,11 +2998,12 @@ void extract(Serializer& serializer, MagSoftIronMatrix& self); void insert(Serializer& serializer, const MagSoftIronMatrix::Response& self); void extract(Serializer& serializer, MagSoftIronMatrix::Response& self); -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); -CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); +CmdResult writeMagSoftIronMatrix(C::mip_interface& device, Matrix3f offset); +CmdResult readMagSoftIronMatrix(C::mip_interface& device, Matrix3f offsetOut); CmdResult saveMagSoftIronMatrix(C::mip_interface& device); CmdResult loadMagSoftIronMatrix(C::mip_interface& device); CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2238,25 +3014,49 @@ CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); struct ConingScullingEnable { + FunctionSelector function = static_cast(0); + bool enable = 0; ///< If true, coning and sculling compensation is enabled. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - bool enable = 0; ///< If true, coning and sculling compensation is enabled. + auto as_tuple() const + { + return std::make_tuple(enable); + } + + + static ConingScullingEnable create_sld_all(::mip::FunctionSelector function) + { + ConingScullingEnable cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< If true, coning and sculling compensation is enabled. + + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } + }; }; void insert(Serializer& serializer, const ConingScullingEnable& self); @@ -2270,6 +3070,7 @@ CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); CmdResult saveConingScullingEnable(C::mip_interface& device); CmdResult loadConingScullingEnable(C::mip_interface& device); CmdResult defaultConingScullingEnable(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2304,29 +3105,53 @@ CmdResult defaultConingScullingEnable(C::mip_interface& device); struct Sensor2VehicleTransformEuler { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(roll,pitch,yaw); + } + + + static Sensor2VehicleTransformEuler create_sld_all(::mip::FunctionSelector function) + { + Sensor2VehicleTransformEuler cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] + + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + } + }; }; void insert(Serializer& serializer, const Sensor2VehicleTransformEuler& self); @@ -2340,6 +3165,7 @@ CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* roll CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device); CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device); CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2382,24 +3208,48 @@ CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); struct Sensor2VehicleTransformQuaternion { + FunctionSelector function = static_cast(0); + Quatf q; ///< Unit length quaternion representing transform [w, i, j, k] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float q[4] = {0}; ///< Unit length quaternion representing transform [w, i, j, k] + auto as_tuple() const + { + return std::make_tuple(q); + } + + + static Sensor2VehicleTransformQuaternion create_sld_all(::mip::FunctionSelector function) + { + Sensor2VehicleTransformQuaternion cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; - float q[4] = {0}; ///< Unit length quaternion representing transform [w, i, j, k] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Quatf q; ///< Unit length quaternion representing transform [w, i, j, k] + + + auto as_tuple() + { + return std::make_tuple(std::ref(q)); + } }; }; @@ -2409,11 +3259,12 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion& self); void insert(Serializer& serializer, const Sensor2VehicleTransformQuaternion::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response& self); -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); +CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf q); +CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf qOut); CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2454,24 +3305,48 @@ CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); struct Sensor2VehicleTransformDcm { + FunctionSelector function = static_cast(0); + Matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float dcm[9] = {0}; ///< 3 x 3 direction cosine matrix, stored in row-major order + auto as_tuple() const + { + return std::make_tuple(dcm); + } + + + static Sensor2VehicleTransformDcm create_sld_all(::mip::FunctionSelector function) + { + Sensor2VehicleTransformDcm cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; - float dcm[9] = {0}; ///< 3 x 3 direction cosine matrix, stored in row-major order + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order + + + auto as_tuple() + { + return std::make_tuple(std::ref(dcm)); + } }; }; @@ -2481,11 +3356,12 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm& self); void insert(Serializer& serializer, const Sensor2VehicleTransformDcm::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self); -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); +CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcm); +CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcmOut); CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device); CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device); CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2500,31 +3376,55 @@ CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); struct ComplementaryFilter { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); bool pitch_roll_enable = 0; ///< Enable Pitch/Roll corrections bool heading_enable = 0; ///< Enable Heading corrections (only available on devices with magnetometer) float pitch_roll_time_constant = 0; ///< Time constant associated with the pitch/roll corrections [s] float heading_time_constant = 0; ///< Time constant associated with the heading corrections [s] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x800F; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(pitch_roll_enable,heading_enable,pitch_roll_time_constant,heading_time_constant); + } + + + static ComplementaryFilter create_sld_all(::mip::FunctionSelector function) + { + ComplementaryFilter cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; bool pitch_roll_enable = 0; ///< Enable Pitch/Roll corrections bool heading_enable = 0; ///< Enable Heading corrections (only available on devices with magnetometer) float pitch_roll_time_constant = 0; ///< Time constant associated with the pitch/roll corrections [s] float heading_time_constant = 0; ///< Time constant associated with the heading corrections [s] + + auto as_tuple() + { + return std::make_tuple(std::ref(pitch_roll_enable),std::ref(heading_enable),std::ref(pitch_roll_time_constant),std::ref(heading_time_constant)); + } + }; }; void insert(Serializer& serializer, const ComplementaryFilter& self); @@ -2538,6 +3438,7 @@ CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnabl CmdResult saveComplementaryFilter(C::mip_interface& device); CmdResult loadComplementaryFilter(C::mip_interface& device); CmdResult defaultComplementaryFilter(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2555,27 +3456,52 @@ CmdResult defaultComplementaryFilter(C::mip_interface& device); struct SensorRange { + FunctionSelector function = static_cast(0); + SensorRangeType sensor = static_cast(0); ///< Which type of sensor will get the new range value. + uint8_t setting = 0; ///< Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine this value. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - SensorRangeType sensor = static_cast(0); ///< Which type of sensor will get the new range value. - uint8_t setting = 0; ///< Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine this value. + auto as_tuple() const + { + return std::make_tuple(sensor,setting); + } + + + static SensorRange create_sld_all(::mip::FunctionSelector function) + { + SensorRange cmd; + cmd.function = function; + cmd.sensor = ::mip::commands_3dm::SensorRangeType::ALL; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; SensorRangeType sensor = static_cast(0); ///< Which type of sensor will get the new range value. uint8_t setting = 0; ///< Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine this value. + + auto as_tuple() + { + return std::make_tuple(std::ref(sensor),std::ref(setting)); + } + }; }; void insert(Serializer& serializer, const SensorRange& self); @@ -2589,6 +3515,7 @@ CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor); CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor); CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2602,11 +3529,6 @@ CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); struct CalibratedSensorRanges { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct Entry { uint8_t setting = 0; ///< The value used in the 3DM Sensor Range command and response. @@ -2615,15 +3537,35 @@ struct CalibratedSensorRanges }; SensorRangeType sensor = static_cast(0); ///< The sensor to query. Cannot be ALL. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(sensor); + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000030; SensorRangeType sensor = static_cast(0); ///< The sensor type from the command. uint8_t num_ranges = 0; ///< Number of supported ranges. Entry ranges[50]; ///< List of possible range settings. + + auto as_tuple() + { + return std::make_tuple(std::ref(sensor),std::ref(num_ranges),std::ref(ranges)); + } + }; }; void insert(Serializer& serializer, const CalibratedSensorRanges& self); @@ -2636,6 +3578,7 @@ void insert(Serializer& serializer, const CalibratedSensorRanges::Response& self void extract(Serializer& serializer, CalibratedSensorRanges::Response& self); CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2659,15 +3602,6 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso struct LowpassFilter { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); uint8_t desc_set = 0; ///< Descriptor set of the quantity to be filtered. uint8_t field_desc = 0; ///< Field descriptor of the quantity to be filtered. @@ -2675,17 +3609,52 @@ struct LowpassFilter bool manual = 0; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. float frequency = 0; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x801F; + static const uint32_t READ_PARAMS = 0x8003; + static const uint32_t SAVE_PARAMS = 0x8003; + static const uint32_t LOAD_PARAMS = 0x8003; + static const uint32_t DEFAULT_PARAMS = 0x8003; + static const uint32_t ECHOED_PARAMS = 0x0003; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(desc_set,field_desc,enable,manual,frequency); + } + + + static LowpassFilter create_sld_all(::mip::FunctionSelector function) + { + LowpassFilter cmd; + cmd.function = function; + cmd.desc_set = 0; + cmd.field_desc = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; + static const uint32_t ECHOED_PARAMS = 0x0003; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; ///< Descriptor set of the quantity to be filtered. uint8_t field_desc = 0; ///< Field descriptor of the quantity to be filtered. bool enable = 0; ///< The filter will be enabled if this is true. bool manual = 0; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. float frequency = 0; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. + + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(field_desc),std::ref(enable),std::ref(manual),std::ref(frequency)); + } + }; }; void insert(Serializer& serializer, const LowpassFilter& self); @@ -2699,6 +3668,7 @@ CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); + ///@} /// diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 0bcbab587..8623aa0a0 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -182,10 +182,10 @@ void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const { insert_mip_function_selector(serializer, self->function); + insert_u8(serializer, self->frame_id); + if( self->function == MIP_FUNCTION_WRITE ) { - insert_u8(serializer, self->frame_id); - insert_mip_aiding_reference_frame_command_format(serializer, self->format); for(unsigned int i=0; i < 3; i++) @@ -200,10 +200,10 @@ void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_ { extract_mip_function_selector(serializer, &self->function); + extract_u8(serializer, &self->frame_id); + if( self->function == MIP_FUNCTION_WRITE ) { - extract_u8(serializer, &self->frame_id); - extract_mip_aiding_reference_frame_command_format(serializer, &self->format); for(unsigned int i=0; i < 3; i++) @@ -253,7 +253,7 @@ void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* se *self = tmp; } -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation) +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, mip_vector3f translation, mip_quatf rotation) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -265,19 +265,17 @@ mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, ui insert_mip_aiding_reference_frame_command_format(&serializer, format); - assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, translation[i]); + insert_mip_vector3f(&serializer, &translation[i]); - assert(rotation || (4 == 0)); for(unsigned int i=0; i < 4; i++) - insert_float(&serializer, rotation[i]); + insert_mip_quatf(&serializer, &rotation[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t* frame_id_out, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out) +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, mip_vector3f translation_out, mip_quatf rotation_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -285,6 +283,8 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + insert_u8(&serializer, frame_id); + assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); @@ -295,26 +295,23 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(frame_id_out); - extract_u8(&deserializer, frame_id_out); + extract_u8(&deserializer, &frame_id); assert(format_out); extract_mip_aiding_reference_frame_command_format(&deserializer, format_out); - assert(translation_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &translation_out[i]); + extract_mip_vector3f(&deserializer, &translation_out[i]); - assert(rotation_out || (4 == 0)); for(unsigned int i=0; i < 4; i++) - extract_float(&deserializer, &rotation_out[i]); + extract_mip_quatf(&deserializer, &rotation_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device) +mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -322,11 +319,13 @@ mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device) insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + insert_u8(&serializer, frame_id); + assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device) +mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -334,11 +333,13 @@ mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device) insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + insert_u8(&serializer, frame_id); + assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device) +mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -346,6 +347,8 @@ mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device) insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + insert_u8(&serializer, frame_id); + assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); @@ -511,7 +514,7 @@ void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* seri *self = tmp; } -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3d position, mip_vector3f uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -522,13 +525,11 @@ mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* insert_u8(&serializer, sensor_id); - assert(position || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_double(&serializer, position[i]); + insert_mip_vector3d(&serializer, &position[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, uncertainty[i]); + insert_mip_vector3f(&serializer, &uncertainty[i]); insert_mip_aiding_ecef_pos_command_valid_flags(&serializer, valid_flags); @@ -584,7 +585,7 @@ void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, mip_vector3f uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -601,9 +602,8 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* insert_double(&serializer, height); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, uncertainty[i]); + insert_mip_vector3f(&serializer, &uncertainty[i]); insert_mip_aiding_llh_pos_command_valid_flags(&serializer, valid_flags); @@ -653,7 +653,7 @@ void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* seri *self = tmp; } -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -664,13 +664,11 @@ mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* insert_u8(&serializer, sensor_id); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, velocity[i]); + insert_mip_vector3f(&serializer, &velocity[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, uncertainty[i]); + insert_mip_vector3f(&serializer, &uncertainty[i]); insert_mip_aiding_ecef_vel_command_valid_flags(&serializer, valid_flags); @@ -720,7 +718,7 @@ void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -731,13 +729,11 @@ mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* insert_u8(&serializer, sensor_id); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, velocity[i]); + insert_mip_vector3f(&serializer, &velocity[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, uncertainty[i]); + insert_mip_vector3f(&serializer, &uncertainty[i]); insert_mip_aiding_ned_vel_command_valid_flags(&serializer, valid_flags); @@ -787,7 +783,7 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct *self = tmp; } -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -798,13 +794,11 @@ mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* dev insert_u8(&serializer, sensor_id); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, velocity[i]); + insert_mip_vector3f(&serializer, &velocity[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, uncertainty[i]); + insert_mip_vector3f(&serializer, &uncertainty[i]); insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(&serializer, valid_flags); diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 67c77b306..2140d56ca 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -162,10 +162,10 @@ void insert(Serializer& serializer, const ReferenceFrame& self) { insert(serializer, self.function); + insert(serializer, self.frame_id); + if( self.function == FunctionSelector::WRITE ) { - insert(serializer, self.frame_id); - insert(serializer, self.format); for(unsigned int i=0; i < 3; i++) @@ -180,10 +180,10 @@ void extract(Serializer& serializer, ReferenceFrame& self) { extract(serializer, self.function); + extract(serializer, self.frame_id); + if( self.function == FunctionSelector::WRITE ) { - extract(serializer, self.frame_id); - extract(serializer, self.format); for(unsigned int i=0; i < 3; i++) @@ -222,7 +222,7 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) } -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation) +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, Vector3f translation, Quatf rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -232,11 +232,9 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen insert(serializer, format); - assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, translation[i]); - assert(rotation || (4 == 0)); for(unsigned int i=0; i < 4; i++) insert(serializer, rotation[i]); @@ -244,12 +242,14 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t* frameIdOut, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut) +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, Vector3f translationOut, Quatf rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::READ); + insert(serializer, frameId); + assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); @@ -259,17 +259,14 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t* frameIdOut, Refe { Serializer deserializer(buffer, responseLength); - assert(frameIdOut); - extract(deserializer, *frameIdOut); + extract(deserializer, frameId); assert(formatOut); extract(deserializer, *formatOut); - assert(translationOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, translationOut[i]); - assert(rotationOut || (4 == 0)); for(unsigned int i=0; i < 4; i++) extract(deserializer, rotationOut[i]); @@ -278,32 +275,38 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t* frameIdOut, Refe } return result; } -CmdResult saveReferenceFrame(C::mip_interface& device) +CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::SAVE); + insert(serializer, frameId); + assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadReferenceFrame(C::mip_interface& device) +CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::LOAD); + insert(serializer, frameId); + assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultReferenceFrame(C::mip_interface& device) +CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::RESET); + insert(serializer, frameId); + assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); @@ -436,7 +439,7 @@ void extract(Serializer& serializer, EcefPos& self) } -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3d position, Vector3f uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -445,11 +448,9 @@ CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, insert(serializer, sensorId); - assert(position || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, position[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -496,7 +497,7 @@ void extract(Serializer& serializer, LlhPos& self) } -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, Vector3f uncertainty, LlhPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -511,7 +512,6 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d insert(serializer, height); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -552,7 +552,7 @@ void extract(Serializer& serializer, EcefVel& self) } -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -561,11 +561,9 @@ CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, insert(serializer, sensorId); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -606,7 +604,7 @@ void extract(Serializer& serializer, NedVel& self) } -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, NedVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -615,11 +613,9 @@ CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, c insert(serializer, sensorId); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -660,7 +656,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -669,11 +665,9 @@ CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, insert(serializer, sensorId); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); - assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index 7dd4cab61..3c207f0ab 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -113,6 +114,7 @@ mip_cmd_result mip_aiding_read_sensor_frame_mapping(struct mip_interface* device mip_cmd_result mip_aiding_save_sensor_frame_mapping(struct mip_interface* device); mip_cmd_result mip_aiding_load_sensor_frame_mapping(struct mip_interface* device); mip_cmd_result mip_aiding_default_sensor_frame_mapping(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -129,8 +131,8 @@ struct mip_aiding_reference_frame_command mip_function_selector function; uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_reference_frame_command_format format; ///< Format of the transformation. - float translation[3]; ///< Translation X, Y, and Z. - float rotation[4]; ///< Depends on the format parameter. Unused values are ignored. + mip_vector3f translation; ///< Translation X, Y, and Z. + mip_quatf rotation; ///< Depends on the format parameter. Unused values are ignored. }; typedef struct mip_aiding_reference_frame_command mip_aiding_reference_frame_command; @@ -144,19 +146,20 @@ struct mip_aiding_reference_frame_response { uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_reference_frame_command_format format; ///< Format of the transformation. - float translation[3]; ///< Translation X, Y, and Z. - float rotation[4]; ///< Depends on the format parameter. Unused values are ignored. + mip_vector3f translation; ///< Translation X, Y, and Z. + mip_quatf rotation; ///< Depends on the format parameter. Unused values are ignored. }; typedef struct mip_aiding_reference_frame_response mip_aiding_reference_frame_response; void insert_mip_aiding_reference_frame_response(struct mip_serializer* serializer, const mip_aiding_reference_frame_response* self); void extract_mip_aiding_reference_frame_response(struct mip_serializer* serializer, mip_aiding_reference_frame_response* self); -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation); -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t* frame_id_out, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out); -mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device); -mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device); -mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device); +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, mip_vector3f translation, mip_quatf rotation); +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, mip_vector3f translation_out, mip_quatf rotation_out); +mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id); +mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id); +mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -196,6 +199,7 @@ mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device); mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device); mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -206,17 +210,17 @@ mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* devi typedef uint16_t mip_aiding_ecef_pos_command_valid_flags; static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_X = 0x0002; ///< -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Y = 0x0004; ///< -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Z = 0x0008; ///< -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_ALL = 0x000E; +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_ALL = 0x0007; struct mip_aiding_ecef_pos_command { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Sensor ID. - double position[3]; ///< ECEF position [m]. - float uncertainty[3]; ///< ECEF position uncertainty [m]. + mip_vector3d position; ///< ECEF position [m]. + mip_vector3f uncertainty; ///< ECEF position uncertainty [m]. mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. }; @@ -227,7 +231,8 @@ void extract_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self); void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3d position, mip_vector3f uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -239,10 +244,10 @@ mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* typedef uint16_t mip_aiding_llh_pos_command_valid_flags; static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LATITUDE = 0x0002; ///< -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LONGITUDE = 0x0004; ///< -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_HEIGHT = 0x0008; ///< -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_ALL = 0x000E; +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LATITUDE = 0x0001; ///< +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LONGITUDE = 0x0002; ///< +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_HEIGHT = 0x0004; ///< +static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_ALL = 0x0007; struct mip_aiding_llh_pos_command { @@ -251,7 +256,7 @@ struct mip_aiding_llh_pos_command double latitude; ///< [deg] double longitude; ///< [deg] double height; ///< [m] - float uncertainty[3]; ///< NED position uncertainty. + mip_vector3f uncertainty; ///< NED position uncertainty. mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. }; @@ -262,7 +267,8 @@ void extract_mip_aiding_llh_pos_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self); void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self); -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, mip_vector3f uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -273,17 +279,17 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* typedef uint16_t mip_aiding_ecef_vel_command_valid_flags; static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_X = 0x0002; ///< -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Y = 0x0004; ///< -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Z = 0x0008; ///< -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_ALL = 0x000E; +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_ALL = 0x0007; struct mip_aiding_ecef_vel_command { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Sensor ID. - float velocity[3]; ///< ECEF velocity [m/s]. - float uncertainty[3]; ///< ECEF velocity uncertainty [m/s]. + mip_vector3f velocity; ///< ECEF velocity [m/s]. + mip_vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. }; @@ -294,7 +300,8 @@ void extract_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self); void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -305,17 +312,17 @@ mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* typedef uint16_t mip_aiding_ned_vel_command_valid_flags; static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_X = 0x0002; ///< -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Y = 0x0004; ///< -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Z = 0x0008; ///< -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_ALL = 0x000E; +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_ALL = 0x0007; struct mip_aiding_ned_vel_command { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Sensor ID. - float velocity[3]; ///< NED velocity [m/s]. - float uncertainty[3]; ///< NED velocity uncertainty [m/s]. + mip_vector3f velocity; ///< NED velocity [m/s]. + mip_vector3f uncertainty; ///< NED velocity uncertainty [m/s]. mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. }; @@ -326,7 +333,8 @@ void extract_mip_aiding_ned_vel_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self); void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self); -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -338,17 +346,17 @@ mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* typedef uint16_t mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags; static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_X = 0x0002; ///< -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Y = 0x0004; ///< -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Z = 0x0008; ///< -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_ALL = 0x000E; +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_ALL = 0x0007; struct mip_aiding_vehicle_fixed_frame_velocity_command { mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3]; ///< [m/s] - float uncertainty[3]; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + mip_vector3f velocity; ///< [m/s] + mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; }; @@ -359,7 +367,8 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializ void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self); void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self); -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -381,6 +390,7 @@ void insert_mip_aiding_true_heading_command(struct mip_serializer* serializer, c void extract_mip_aiding_true_heading_command(struct mip_serializer* serializer, mip_aiding_true_heading_command* self); mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags); + ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index e93c84e7a..4e0470c00 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -86,27 +87,51 @@ void extract(Serializer& serializer, Time& self); struct SensorFrameMapping { + FunctionSelector function = static_cast(0); + uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. + uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. - uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. + auto as_tuple() const + { + return std::make_tuple(sensor_id,frame_id); + } + + + static SensorFrameMapping create_sld_all(::mip::FunctionSelector function) + { + SensorFrameMapping cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. + + auto as_tuple() + { + return std::make_tuple(std::ref(sensor_id),std::ref(frame_id)); + } + }; }; void insert(Serializer& serializer, const SensorFrameMapping& self); @@ -120,6 +145,7 @@ CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, CmdResult saveSensorFrameMapping(C::mip_interface& device); CmdResult loadSensorFrameMapping(C::mip_interface& device); CmdResult defaultSensorFrameMapping(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -129,15 +155,6 @@ CmdResult defaultSensorFrameMapping(C::mip_interface& device); struct ReferenceFrame { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Format : uint8_t { EULER = 1, ///< Translation vector followed by euler angles (roll, pitch, yaw). @@ -147,18 +164,52 @@ struct ReferenceFrame FunctionSelector function = static_cast(0); uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. - float translation[3] = {0}; ///< Translation X, Y, and Z. - float rotation[4] = {0}; ///< Depends on the format parameter. Unused values are ignored. + Vector3f translation; ///< Translation X, Y, and Z. + Quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x800F; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(frame_id,format,translation,rotation); + } + + + static ReferenceFrame create_sld_all(::mip::FunctionSelector function) + { + ReferenceFrame cmd; + cmd.function = function; + cmd.frame_id = 0; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. - float translation[3] = {0}; ///< Translation X, Y, and Z. - float rotation[4] = {0}; ///< Depends on the format parameter. Unused values are ignored. + Vector3f translation; ///< Translation X, Y, and Z. + Quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + + + auto as_tuple() + { + return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation)); + } }; }; @@ -168,11 +219,12 @@ void extract(Serializer& serializer, ReferenceFrame& self); void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation); -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t* frameIdOut, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut); -CmdResult saveReferenceFrame(C::mip_interface& device); -CmdResult loadReferenceFrame(C::mip_interface& device); -CmdResult defaultReferenceFrame(C::mip_interface& device); +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, Vector3f translation, Quatf rotation); +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, Vector3f translationOut, Quatf rotationOut); +CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); +CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); +CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -182,15 +234,6 @@ CmdResult defaultReferenceFrame(C::mip_interface& device); struct AidingEchoControl { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Mode : uint8_t { SUPPRESS_ACK = 0, ///< Suppresses the usual command ack field for aiding messages. @@ -201,13 +244,46 @@ struct AidingEchoControl FunctionSelector function = static_cast(0); Mode mode = static_cast(0); ///< Controls data echoing. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(mode); + } + + + static AidingEchoControl create_sld_all(::mip::FunctionSelector function) + { + AidingEchoControl cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; Mode mode = static_cast(0); ///< Controls data echoing. + + auto as_tuple() + { + return std::make_tuple(std::ref(mode)); + } + }; }; void insert(Serializer& serializer, const AidingEchoControl& self); @@ -221,6 +297,7 @@ CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mod CmdResult saveAidingEchoControl(C::mip_interface& device); CmdResult loadAidingEchoControl(C::mip_interface& device); CmdResult defaultAidingEchoControl(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -231,20 +308,15 @@ CmdResult defaultAidingEchoControl(C::mip_interface& device); struct EcefPos { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct ValidFlags : Bitfield { enum _enumType : uint16_t { NONE = 0x0000, - X = 0x0002, ///< - Y = 0x0004, ///< - Z = 0x0008, ///< - ALL = 0x000E, + X = 0x0001, ///< + Y = 0x0002, ///< + Z = 0x0004, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -269,15 +341,28 @@ struct EcefPos Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Sensor ID. - double position[3] = {0}; ///< ECEF position [m]. - float uncertainty[3] = {0}; ///< ECEF position uncertainty [m]. + Vector3d position; ///< ECEF position [m]. + Vector3f uncertainty; ///< ECEF position uncertainty [m]. ValidFlags valid_flags; ///< Valid flags. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,position,uncertainty,valid_flags); + } + + typedef void Response; }; void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3d position, Vector3f uncertainty, EcefPos::ValidFlags validFlags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -289,20 +374,15 @@ CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, struct LlhPos { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct ValidFlags : Bitfield { enum _enumType : uint16_t { NONE = 0x0000, - LATITUDE = 0x0002, ///< - LONGITUDE = 0x0004, ///< - HEIGHT = 0x0008, ///< - ALL = 0x000E, + LATITUDE = 0x0001, ///< + LONGITUDE = 0x0002, ///< + HEIGHT = 0x0004, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -330,14 +410,27 @@ struct LlhPos double latitude = 0; ///< [deg] double longitude = 0; ///< [deg] double height = 0; ///< [m] - float uncertainty[3] = {0}; ///< NED position uncertainty. + Vector3f uncertainty; ///< NED position uncertainty. ValidFlags valid_flags; ///< Valid flags. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,latitude,longitude,height,uncertainty,valid_flags); + } + + typedef void Response; }; void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, Vector3f uncertainty, LlhPos::ValidFlags validFlags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -348,20 +441,15 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d struct EcefVel { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct ValidFlags : Bitfield { enum _enumType : uint16_t { NONE = 0x0000, - X = 0x0002, ///< - Y = 0x0004, ///< - Z = 0x0008, ///< - ALL = 0x000E, + X = 0x0001, ///< + Y = 0x0002, ///< + Z = 0x0004, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -386,15 +474,28 @@ struct EcefVel Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Sensor ID. - float velocity[3] = {0}; ///< ECEF velocity [m/s]. - float uncertainty[3] = {0}; ///< ECEF velocity uncertainty [m/s]. + Vector3f velocity; ///< ECEF velocity [m/s]. + Vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + } + + typedef void Response; }; void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, EcefVel::ValidFlags validFlags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -405,20 +506,15 @@ CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, struct NedVel { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct ValidFlags : Bitfield { enum _enumType : uint16_t { NONE = 0x0000, - X = 0x0002, ///< - Y = 0x0004, ///< - Z = 0x0008, ///< - ALL = 0x000E, + X = 0x0001, ///< + Y = 0x0002, ///< + Z = 0x0004, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -443,15 +539,28 @@ struct NedVel Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Sensor ID. - float velocity[3] = {0}; ///< NED velocity [m/s]. - float uncertainty[3] = {0}; ///< NED velocity uncertainty [m/s]. + Vector3f velocity; ///< NED velocity [m/s]. + Vector3f uncertainty; ///< NED velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + } + + typedef void Response; }; void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, NedVel::ValidFlags validFlags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -463,20 +572,15 @@ CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, c struct VehicleFixedFrameVelocity { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct ValidFlags : Bitfield { enum _enumType : uint16_t { NONE = 0x0000, - X = 0x0002, ///< - Y = 0x0004, ///< - Z = 0x0008, ///< - ALL = 0x000E, + X = 0x0001, ///< + Y = 0x0002, ///< + Z = 0x0004, ///< + ALL = 0x0007, }; uint16_t value = NONE; @@ -501,15 +605,28 @@ struct VehicleFixedFrameVelocity Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) - float velocity[3] = {0}; ///< [m/s] - float uncertainty[3] = {0}; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + Vector3f velocity; ///< [m/s] + Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + } + + typedef void Response; }; void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -519,22 +636,30 @@ CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, struct TrueHeading { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; - - static const bool HAS_FUNCTION_SELECTOR = false; - Time time; uint8_t sensor_id = 0; float heading = 0; ///< Heading in [radians] float uncertainty = 0; uint16_t valid_flags = 0; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,heading,uncertainty,valid_flags); + } + + typedef void Response; }; void insert(Serializer& serializer, const TrueHeading& self); void extract(Serializer& serializer, TrueHeading& self); CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); + ///@} /// diff --git a/src/mip/definitions/commands_base.c b/src/mip/definitions/commands_base.c index 3868f7619..5d5bbac6a 100644 --- a/src/mip/definitions/commands_base.c +++ b/src/mip/definitions/commands_base.c @@ -130,7 +130,6 @@ mip_cmd_result mip_base_get_device_descriptors(struct mip_interface* device, uin mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(descriptors_out && descriptors_out_count); for(*descriptors_out_count = 0; (*descriptors_out_count < descriptors_out_max) && (mip_serializer_remaining(&deserializer) > 0); (*descriptors_out_count)++) extract_u16(&deserializer, &descriptors_out[*descriptors_out_count]); @@ -175,7 +174,6 @@ mip_cmd_result mip_base_get_extended_descriptors(struct mip_interface* device, u mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(descriptors_out && descriptors_out_count); for(*descriptors_out_count = 0; (*descriptors_out_count < descriptors_out_max) && (mip_serializer_remaining(&deserializer) > 0); (*descriptors_out_count)++) extract_u16(&deserializer, &descriptors_out[*descriptors_out_count]); diff --git a/src/mip/definitions/commands_base.cpp b/src/mip/definitions/commands_base.cpp index e89ba54de..035d105f7 100644 --- a/src/mip/definitions/commands_base.cpp +++ b/src/mip/definitions/commands_base.cpp @@ -160,9 +160,7 @@ void insert(Serializer& serializer, const GetDeviceDescriptors::Response& self) } void extract(Serializer& serializer, GetDeviceDescriptors::Response& self) { - assert(self.descriptors || (self.descriptors_count == 0)); - uint8_t descriptorsCountMax = self.descriptors_count; - for(self.descriptors_count = 0; (self.descriptors_count < descriptorsCountMax) && (serializer.remaining() > 0); (self.descriptors_count)++) + for(self.descriptors_count = 0; (self.descriptors_count < sizeof(self.descriptors)/sizeof(self.descriptors[0])) && (serializer.remaining() > 0); (self.descriptors_count)++) extract(serializer, self.descriptors[self.descriptors_count]); } @@ -178,7 +176,6 @@ CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOu { Serializer deserializer(buffer, responseLength); - assert(descriptorsOut && descriptorsOutCount); for(*descriptorsOutCount = 0; (*descriptorsOutCount < descriptorsOutMax) && (deserializer.remaining() > 0); (*descriptorsOutCount)++) extract(deserializer, descriptorsOut[*descriptorsOutCount]); @@ -262,9 +259,7 @@ void insert(Serializer& serializer, const GetExtendedDescriptors::Response& self } void extract(Serializer& serializer, GetExtendedDescriptors::Response& self) { - assert(self.descriptors || (self.descriptors_count == 0)); - uint8_t descriptorsCountMax = self.descriptors_count; - for(self.descriptors_count = 0; (self.descriptors_count < descriptorsCountMax) && (serializer.remaining() > 0); (self.descriptors_count)++) + for(self.descriptors_count = 0; (self.descriptors_count < sizeof(self.descriptors)/sizeof(self.descriptors[0])) && (serializer.remaining() > 0); (self.descriptors_count)++) extract(serializer, self.descriptors[self.descriptors_count]); } @@ -280,7 +275,6 @@ CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptors { Serializer deserializer(buffer, responseLength); - assert(descriptorsOut && descriptorsOutCount); for(*descriptorsOutCount = 0; (*descriptorsOutCount < descriptorsOutMax) && (deserializer.remaining() > 0); (*descriptorsOutCount)++) extract(deserializer, descriptorsOut[*descriptorsOutCount]); diff --git a/src/mip/definitions/commands_base.h b/src/mip/definitions/commands_base.h index 073542861..07b1d7e6b 100644 --- a/src/mip/definitions/commands_base.h +++ b/src/mip/definitions/commands_base.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -130,6 +131,7 @@ void extract_mip_commanded_test_bits_gq7(struct mip_serializer* serializer, mip_ ///@{ mip_cmd_result mip_base_ping(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -143,6 +145,7 @@ mip_cmd_result mip_base_ping(struct mip_interface* device); ///@{ mip_cmd_result mip_base_set_idle(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -161,6 +164,7 @@ void insert_mip_base_get_device_info_response(struct mip_serializer* serializer, void extract_mip_base_get_device_info_response(struct mip_serializer* serializer, mip_base_get_device_info_response* self); mip_cmd_result mip_base_get_device_info(struct mip_interface* device, mip_base_device_info* device_info_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -174,7 +178,7 @@ mip_cmd_result mip_base_get_device_info(struct mip_interface* device, mip_base_d struct mip_base_get_device_descriptors_response { - uint16_t* descriptors; + uint16_t descriptors[253]; uint8_t descriptors_count; }; @@ -183,6 +187,7 @@ void insert_mip_base_get_device_descriptors_response(struct mip_serializer* seri void extract_mip_base_get_device_descriptors_response(struct mip_serializer* serializer, mip_base_get_device_descriptors_response* self); mip_cmd_result mip_base_get_device_descriptors(struct mip_interface* device, uint16_t* descriptors_out, size_t descriptors_out_max, uint8_t* descriptors_out_count); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -206,6 +211,7 @@ void insert_mip_base_built_in_test_response(struct mip_serializer* serializer, c void extract_mip_base_built_in_test_response(struct mip_serializer* serializer, mip_base_built_in_test_response* self); mip_cmd_result mip_base_built_in_test(struct mip_interface* device, uint32_t* result_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -217,6 +223,7 @@ mip_cmd_result mip_base_built_in_test(struct mip_interface* device, uint32_t* re ///@{ mip_cmd_result mip_base_resume(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -230,7 +237,7 @@ mip_cmd_result mip_base_resume(struct mip_interface* device); struct mip_base_get_extended_descriptors_response { - uint16_t* descriptors; + uint16_t descriptors[253]; uint8_t descriptors_count; }; @@ -239,6 +246,7 @@ void insert_mip_base_get_extended_descriptors_response(struct mip_serializer* se void extract_mip_base_get_extended_descriptors_response(struct mip_serializer* serializer, mip_base_get_extended_descriptors_response* self); mip_cmd_result mip_base_get_extended_descriptors(struct mip_interface* device, uint16_t* descriptors_out, size_t descriptors_out_max, uint8_t* descriptors_out_count); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -259,6 +267,7 @@ void insert_mip_base_continuous_bit_response(struct mip_serializer* serializer, void extract_mip_base_continuous_bit_response(struct mip_serializer* serializer, mip_base_continuous_bit_response* self); mip_cmd_result mip_base_continuous_bit(struct mip_interface* device, uint8_t* result_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -307,6 +316,7 @@ mip_cmd_result mip_base_read_comm_speed(struct mip_interface* device, uint8_t po mip_cmd_result mip_base_save_comm_speed(struct mip_interface* device, uint8_t port); mip_cmd_result mip_base_load_comm_speed(struct mip_interface* device, uint8_t port); mip_cmd_result mip_base_default_comm_speed(struct mip_interface* device, uint8_t port); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -337,6 +347,7 @@ void insert_mip_base_gps_time_update_command_field_id(struct mip_serializer* ser void extract_mip_base_gps_time_update_command_field_id(struct mip_serializer* serializer, mip_base_gps_time_update_command_field_id* self); mip_cmd_result mip_base_write_gps_time_update(struct mip_interface* device, mip_base_gps_time_update_command_field_id field_id, uint32_t value); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -348,6 +359,7 @@ mip_cmd_result mip_base_write_gps_time_update(struct mip_interface* device, mip_ ///@{ mip_cmd_result mip_base_soft_reset(struct mip_interface* device); + ///@} /// diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index c0f8abc41..82b724c20 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -197,17 +198,25 @@ struct CommandedTestBitsGq7 : Bitfield struct Ping { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const Ping& self); void extract(Serializer& serializer, Ping& self); CmdResult ping(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -222,17 +231,25 @@ CmdResult ping(C::mip_interface& device); struct SetIdle { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const SetIdle& self); void extract(Serializer& serializer, SetIdle& self); CmdResult setIdle(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -243,19 +260,34 @@ CmdResult setIdle(C::mip_interface& device); struct GetDeviceInfo { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; BaseDeviceInfo device_info; + + auto as_tuple() + { + return std::make_tuple(std::ref(device_info)); + } + }; }; void insert(Serializer& serializer, const GetDeviceInfo& self); @@ -265,6 +297,7 @@ void insert(Serializer& serializer, const GetDeviceInfo::Response& self); void extract(Serializer& serializer, GetDeviceInfo::Response& self); CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -278,20 +311,35 @@ CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) struct GetDeviceDescriptors { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; - uint16_t* descriptors = {nullptr}; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000001; + uint16_t descriptors[253] = {0}; uint8_t descriptors_count = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(descriptors),std::ref(descriptors_count)); + } + }; }; void insert(Serializer& serializer, const GetDeviceDescriptors& self); @@ -301,6 +349,7 @@ void insert(Serializer& serializer, const GetDeviceDescriptors::Response& self); void extract(Serializer& serializer, GetDeviceDescriptors::Response& self); CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -316,19 +365,34 @@ CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOu struct BuiltInTest { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint32_t result = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(result)); + } + }; }; void insert(Serializer& serializer, const BuiltInTest& self); @@ -338,6 +402,7 @@ void insert(Serializer& serializer, const BuiltInTest::Response& self); void extract(Serializer& serializer, BuiltInTest::Response& self); CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -350,17 +415,25 @@ CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut); struct Resume { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const Resume& self); void extract(Serializer& serializer, Resume& self); CmdResult resume(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -374,20 +447,35 @@ CmdResult resume(C::mip_interface& device); struct GetExtendedDescriptors { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; - uint16_t* descriptors = {nullptr}; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000001; + uint16_t descriptors[253] = {0}; uint8_t descriptors_count = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(descriptors),std::ref(descriptors_count)); + } + }; }; void insert(Serializer& serializer, const GetExtendedDescriptors& self); @@ -397,6 +485,7 @@ void insert(Serializer& serializer, const GetExtendedDescriptors::Response& self void extract(Serializer& serializer, GetExtendedDescriptors::Response& self); CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -409,19 +498,34 @@ CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptors struct ContinuousBit { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. + + auto as_tuple() + { + return std::make_tuple(std::ref(result)); + } + }; }; void insert(Serializer& serializer, const ContinuousBit& self); @@ -431,6 +535,7 @@ void insert(Serializer& serializer, const ContinuousBit::Response& self); void extract(Serializer& serializer, ContinuousBit::Response& self); CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -454,28 +559,53 @@ CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut); struct CommSpeed { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - static const uint32_t ALL_PORTS = 0; FunctionSelector function = static_cast(0); uint8_t port = 0; ///< Port ID number, starting with 1. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all ports. See the device user manual for details. uint32_t baud = 0; ///< Port baud rate. Must be a supported rate. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(port,baud); + } + + + static CommSpeed create_sld_all(::mip::FunctionSelector function) + { + CommSpeed cmd; + cmd.function = function; + cmd.port = 0; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t port = 0; ///< Port ID number, starting with 1. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all ports. See the device user manual for details. uint32_t baud = 0; ///< Port baud rate. Must be a supported rate. + + auto as_tuple() + { + return std::make_tuple(std::ref(port),std::ref(baud)); + } + }; }; void insert(Serializer& serializer, const CommSpeed& self); @@ -489,6 +619,7 @@ CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOu CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port); CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port); CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -502,15 +633,6 @@ CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port); struct GpsTimeUpdate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = false; - static const bool HAS_SAVE_FUNCTION = false; - static const bool HAS_LOAD_FUNCTION = false; - static const bool HAS_RESET_FUNCTION = false; - enum class FieldId : uint8_t { WEEK_NUMBER = 1, ///< Week number. @@ -521,11 +643,37 @@ struct GpsTimeUpdate FieldId field_id = static_cast(0); ///< Determines how to interpret value. uint32_t value = 0; ///< Week number or time of week, depending on the field_id. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x0000; + static const uint32_t SAVE_PARAMS = 0x0000; + static const uint32_t LOAD_PARAMS = 0x0000; + static const uint32_t DEFAULT_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(field_id,value); + } + + + static GpsTimeUpdate create_sld_all(::mip::FunctionSelector function) + { + GpsTimeUpdate cmd; + cmd.function = function; + return cmd; + } + + typedef void Response; }; void insert(Serializer& serializer, const GpsTimeUpdate& self); void extract(Serializer& serializer, GpsTimeUpdate& self); CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -538,17 +686,25 @@ CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fi struct SoftReset { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const SoftReset& self); void extract(Serializer& serializer, SoftReset& self); CmdResult softReset(C::mip_interface& device); + ///@} /// diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index bc7616a0e..8e0e06bff 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -263,7 +263,7 @@ void extract_mip_filter_external_gnss_update_command(mip_serializer* serializer, } -mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, const float* velocity, const float* pos_uncertainty, const float* vel_uncertainty) +mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, mip_vector3f velocity, mip_vector3f pos_uncertainty, mip_vector3f vel_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -279,17 +279,14 @@ mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, dou insert_double(&serializer, height); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, velocity[i]); + insert_mip_vector3f(&serializer, &velocity[i]); - assert(pos_uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, pos_uncertainty[i]); + insert_mip_vector3f(&serializer, &pos_uncertainty[i]); - assert(vel_uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, vel_uncertainty[i]); + insert_mip_vector3f(&serializer, &vel_uncertainty[i]); assert(mip_serializer_is_ok(&serializer)); @@ -785,7 +782,7 @@ void extract_mip_filter_sensor_to_vehicle_rotation_dcm_response(mip_serializer* } -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, const float* dcm) +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -793,15 +790,14 @@ mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interf insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) - insert_float(&serializer, dcm[i]); + insert_mip_matrix3f(&serializer, &dcm[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, float* dcm_out) +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -819,9 +815,8 @@ mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interfa mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(dcm_out || (9 == 0)); for(unsigned int i=0; i < 9; i++) - extract_float(&deserializer, &dcm_out[i]); + extract_mip_matrix3f(&deserializer, &dcm_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -900,7 +895,7 @@ void extract_mip_filter_sensor_to_vehicle_rotation_quaternion_response(mip_seria } -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, const float* quat) +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -908,15 +903,14 @@ mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(quat || (4 == 0)); for(unsigned int i=0; i < 4; i++) - insert_float(&serializer, quat[i]); + insert_mip_quatf(&serializer, &quat[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, float* quat_out) +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -934,9 +928,8 @@ mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_ mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(quat_out || (4 == 0)); for(unsigned int i=0; i < 4; i++) - extract_float(&deserializer, &quat_out[i]); + extract_mip_quatf(&deserializer, &quat_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1015,7 +1008,7 @@ void extract_mip_filter_sensor_to_vehicle_offset_response(mip_serializer* serial } -mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, const float* offset) +mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1023,15 +1016,14 @@ mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* d insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, offset[i]); + insert_mip_vector3f(&serializer, &offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, float* offset_out) +mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1049,9 +1041,8 @@ mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* de mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &offset_out[i]); + extract_mip_vector3f(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1130,7 +1121,7 @@ void extract_mip_filter_antenna_offset_response(mip_serializer* serializer, mip_ } -mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, const float* offset) +mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, mip_vector3f offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1138,15 +1129,14 @@ mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, con insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, offset[i]); + insert_mip_vector3f(&serializer, &offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, float* offset_out) +mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, mip_vector3f offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1164,9 +1154,8 @@ mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, floa mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &offset_out[i]); + extract_mip_vector3f(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1591,7 +1580,7 @@ void extract_mip_filter_accel_noise_response(mip_serializer* serializer, mip_fil } -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise) +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, mip_vector3f noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1599,15 +1588,14 @@ mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, noise[i]); + insert_mip_vector3f(&serializer, &noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out) +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, mip_vector3f noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1625,9 +1613,8 @@ mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &noise_out[i]); + extract_mip_vector3f(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1706,7 +1693,7 @@ void extract_mip_filter_gyro_noise_response(mip_serializer* serializer, mip_filt } -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise) +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, mip_vector3f noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1714,15 +1701,14 @@ mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const f insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, noise[i]); + insert_mip_vector3f(&serializer, &noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out) +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, mip_vector3f noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1740,9 +1726,8 @@ mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* n mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &noise_out[i]); + extract_mip_vector3f(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1833,7 +1818,7 @@ void extract_mip_filter_accel_bias_model_response(mip_serializer* serializer, mi } -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise) +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1841,19 +1826,17 @@ mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, c insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, beta[i]); + insert_mip_vector3f(&serializer, &beta[i]); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, noise[i]); + insert_mip_vector3f(&serializer, &noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1871,13 +1854,11 @@ mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, fl mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(beta_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &beta_out[i]); + extract_mip_vector3f(&deserializer, &beta_out[i]); - assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &noise_out[i]); + extract_mip_vector3f(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1968,7 +1949,7 @@ void extract_mip_filter_gyro_bias_model_response(mip_serializer* serializer, mip } -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise) +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1976,19 +1957,17 @@ mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, co insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, beta[i]); + insert_mip_vector3f(&serializer, &beta[i]); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, noise[i]); + insert_mip_vector3f(&serializer, &noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2006,13 +1985,11 @@ mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, flo mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); - assert(beta_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &beta_out[i]); + extract_mip_vector3f(&deserializer, &beta_out[i]); - assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &noise_out[i]); + extract_mip_vector3f(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2543,6 +2520,964 @@ mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device) { return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_COMMANDED_ANGULAR_ZUPT, NULL, 0); } +void insert_mip_filter_mag_capture_auto_cal_command(mip_serializer* serializer, const mip_filter_mag_capture_auto_cal_command* self) +{ + insert_mip_function_selector(serializer, self->function); + +} +void extract_mip_filter_mag_capture_auto_cal_command(mip_serializer* serializer, mip_filter_mag_capture_auto_cal_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + +} + +mip_cmd_result mip_filter_write_mag_capture_auto_cal(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_save_mag_capture_auto_cal(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_gravity_noise_command(mip_serializer* serializer, const mip_filter_gravity_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_gravity_noise_command(mip_serializer* serializer, mip_filter_gravity_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_gravity_noise_response(mip_serializer* serializer, const mip_filter_gravity_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_gravity_noise_response(mip_serializer* serializer, mip_filter_gravity_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, mip_vector3f noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + for(unsigned int i=0; i < 3; i++) + insert_mip_vector3f(&serializer, &noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, mip_vector3f noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_GRAVITY_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + for(unsigned int i=0; i < 3; i++) + extract_mip_vector3f(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_gravity_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_pressure_altitude_noise_command(mip_serializer* serializer, const mip_filter_pressure_altitude_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_float(serializer, self->noise); + + } +} +void extract_mip_filter_pressure_altitude_noise_command(mip_serializer* serializer, mip_filter_pressure_altitude_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_float(serializer, &self->noise); + + } +} + +void insert_mip_filter_pressure_altitude_noise_response(mip_serializer* serializer, const mip_filter_pressure_altitude_noise_response* self) +{ + insert_float(serializer, self->noise); + +} +void extract_mip_filter_pressure_altitude_noise_response(mip_serializer* serializer, mip_filter_pressure_altitude_noise_response* self) +{ + extract_float(serializer, &self->noise); + +} + +mip_cmd_result mip_filter_write_pressure_altitude_noise(struct mip_interface* device, float noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_float(&serializer, noise); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* device, float* noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(noise_out); + extract_float(&deserializer, noise_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_pressure_altitude_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_hard_iron_offset_noise_command(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + for(unsigned int i=0; i < 3; i++) + insert_mip_vector3f(&serializer, &noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + for(unsigned int i=0; i < 3; i++) + extract_mip_vector3f(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_soft_iron_matrix_noise_command(mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 9; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_soft_iron_matrix_noise_command(mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 9; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_soft_iron_matrix_noise_response(mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_response* self) +{ + for(unsigned int i=0; i < 9; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_soft_iron_matrix_noise_response(mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_response* self) +{ + for(unsigned int i=0; i < 9; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + for(unsigned int i=0; i < 9; i++) + insert_mip_matrix3f(&serializer, &noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + for(unsigned int i=0; i < 9; i++) + extract_mip_matrix3f(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_mag_noise_command(mip_serializer* serializer, const mip_filter_mag_noise_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + + } +} +void extract_mip_filter_mag_noise_command(mip_serializer* serializer, mip_filter_mag_noise_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + + } +} + +void insert_mip_filter_mag_noise_response(mip_serializer* serializer, const mip_filter_mag_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->noise[i]); + +} +void extract_mip_filter_mag_noise_response(mip_serializer* serializer, mip_filter_mag_noise_response* self) +{ + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->noise[i]); + +} + +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, mip_vector3f noise) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + for(unsigned int i=0; i < 3; i++) + insert_mip_vector3f(&serializer, &noise[i]); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, mip_vector3f noise_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAG_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + for(unsigned int i=0; i < 3; i++) + extract_mip_vector3f(&deserializer, &noise_out[i]); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_mag_noise(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_inclination_source_command(mip_serializer* serializer, const mip_filter_inclination_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->inclination); + + } +} +void extract_mip_filter_inclination_source_command(mip_serializer* serializer, mip_filter_inclination_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->inclination); + + } +} + +void insert_mip_filter_inclination_source_response(mip_serializer* serializer, const mip_filter_inclination_source_response* self) +{ + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->inclination); + +} +void extract_mip_filter_inclination_source_response(mip_serializer* serializer, mip_filter_inclination_source_response* self) +{ + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->inclination); + +} + +mip_cmd_result mip_filter_write_inclination_source(struct mip_interface* device, mip_filter_mag_param_source source, float inclination) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_mag_param_source(&serializer, source); + + insert_float(&serializer, inclination); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_inclination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* inclination_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_INCLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); + + assert(inclination_out); + extract_float(&deserializer, inclination_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_inclination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, const mip_filter_magnetic_declination_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->declination); + + } +} +void extract_mip_filter_magnetic_declination_source_command(mip_serializer* serializer, mip_filter_magnetic_declination_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->declination); + + } +} + +void insert_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, const mip_filter_magnetic_declination_source_response* self) +{ + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->declination); + +} +void extract_mip_filter_magnetic_declination_source_response(mip_serializer* serializer, mip_filter_magnetic_declination_source_response* self) +{ + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->declination); + +} + +mip_cmd_result mip_filter_write_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source source, float declination) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_mag_param_source(&serializer, source); + + insert_float(&serializer, declination); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* declination_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_DECLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); + + assert(declination_out); + extract_float(&deserializer, declination_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_filter_mag_field_magnitude_source_command(mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_command* self) +{ + insert_mip_function_selector(serializer, self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->magnitude); + + } +} +void extract_mip_filter_mag_field_magnitude_source_command(mip_serializer* serializer, mip_filter_mag_field_magnitude_source_command* self) +{ + extract_mip_function_selector(serializer, &self->function); + + if( self->function == MIP_FUNCTION_WRITE ) + { + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->magnitude); + + } +} + +void insert_mip_filter_mag_field_magnitude_source_response(mip_serializer* serializer, const mip_filter_mag_field_magnitude_source_response* self) +{ + insert_mip_filter_mag_param_source(serializer, self->source); + + insert_float(serializer, self->magnitude); + +} +void extract_mip_filter_mag_field_magnitude_source_response(mip_serializer* serializer, mip_filter_mag_field_magnitude_source_response* self) +{ + extract_mip_filter_mag_param_source(serializer, &self->source); + + extract_float(serializer, &self->magnitude); + +} + +mip_cmd_result mip_filter_write_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source source, float magnitude) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + + insert_mip_filter_mag_param_source(&serializer, source); + + insert_float(&serializer, magnitude); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_read_mag_field_magnitude_source(struct mip_interface* device, mip_filter_mag_param_source* source_out, float* magnitude_out) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); + + assert(mip_serializer_is_ok(&serializer)); + + uint8_t responseLength = sizeof(buffer); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + mip_serializer deserializer; + mip_serializer_init_insertion(&deserializer, buffer, responseLength); + + assert(source_out); + extract_mip_filter_mag_param_source(&deserializer, source_out); + + assert(magnitude_out); + extract_float(&deserializer, magnitude_out); + + if( mip_serializer_remaining(&deserializer) != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +mip_cmd_result mip_filter_save_mag_field_magnitude_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_load_mag_field_magnitude_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +mip_cmd_result mip_filter_default_mag_field_magnitude_source(struct mip_interface* device) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_filter_reference_position_command(mip_serializer* serializer, const mip_filter_reference_position_command* self) { insert_mip_function_selector(serializer, self->function); @@ -3625,7 +4560,7 @@ void extract_mip_filter_initialization_configuration_command_initial_condition_s *self = tmp; } -mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, const float* initial_position, const float* initial_velocity, mip_filter_reference_frame reference_frame_selector) +mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, mip_vector3f initial_position, mip_vector3f initial_velocity, mip_filter_reference_frame reference_frame_selector) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3645,13 +4580,11 @@ mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interfac insert_float(&serializer, initial_roll); - assert(initial_position || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, initial_position[i]); + insert_mip_vector3f(&serializer, &initial_position[i]); - assert(initial_velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, initial_velocity[i]); + insert_mip_vector3f(&serializer, &initial_velocity[i]); insert_mip_filter_reference_frame(&serializer, reference_frame_selector); @@ -3659,7 +4592,7 @@ mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interfac return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, float* initial_position_out, float* initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out) +mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, mip_vector3f initial_position_out, mip_vector3f initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3695,13 +4628,11 @@ mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface assert(initial_roll_out); extract_float(&deserializer, initial_roll_out); - assert(initial_position_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &initial_position_out[i]); + extract_mip_vector3f(&deserializer, &initial_position_out[i]); - assert(initial_velocity_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &initial_velocity_out[i]); + extract_mip_vector3f(&deserializer, &initial_velocity_out[i]); assert(reference_frame_selector_out); extract_mip_filter_reference_frame(&deserializer, reference_frame_selector_out); @@ -3912,7 +4843,7 @@ void extract_mip_filter_multi_antenna_offset_response(mip_serializer* serializer } -mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, const float* antenna_offset) +mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3922,15 +4853,14 @@ mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* devic insert_u8(&serializer, receiver_id); - assert(antenna_offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, antenna_offset[i]); + insert_mip_vector3f(&serializer, &antenna_offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, float* antenna_offset_out) +mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3952,9 +4882,8 @@ mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device extract_u8(&deserializer, &receiver_id); - assert(antenna_offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &antenna_offset_out[i]); + extract_mip_vector3f(&deserializer, &antenna_offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4055,7 +4984,7 @@ void extract_mip_filter_rel_pos_configuration_response(mip_serializer* serialize } -mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, const double* reference_coordinates) +mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, mip_vector3d reference_coordinates) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4067,15 +4996,14 @@ mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* devi insert_mip_filter_reference_frame(&serializer, reference_frame_selector); - assert(reference_coordinates || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_double(&serializer, reference_coordinates[i]); + insert_mip_vector3d(&serializer, &reference_coordinates[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, double* reference_coordinates_out) +mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, mip_vector3d reference_coordinates_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4099,9 +5027,8 @@ mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* devic assert(reference_frame_selector_out); extract_mip_filter_reference_frame(&deserializer, reference_frame_selector_out); - assert(reference_coordinates_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_double(&deserializer, &reference_coordinates_out[i]); + extract_mip_vector3d(&deserializer, &reference_coordinates_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4199,7 +5126,7 @@ void extract_mip_filter_ref_point_lever_arm_command_reference_point_selector(str *self = tmp; } -mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, const float* lever_arm_offset) +mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, mip_vector3f lever_arm_offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4209,15 +5136,14 @@ mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device insert_mip_filter_ref_point_lever_arm_command_reference_point_selector(&serializer, ref_point_sel); - assert(lever_arm_offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, lever_arm_offset[i]); + insert_mip_vector3f(&serializer, &lever_arm_offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, float* lever_arm_offset_out) +mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, mip_vector3f lever_arm_offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4238,9 +5164,8 @@ mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, assert(ref_point_sel_out); extract_mip_filter_ref_point_lever_arm_command_reference_point_selector(&deserializer, ref_point_sel_out); - assert(lever_arm_offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &lever_arm_offset_out[i]); + extract_mip_vector3f(&deserializer, &lever_arm_offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4368,7 +5293,7 @@ void extract_mip_filter_speed_lever_arm_response(mip_serializer* serializer, mip } -mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, const float* lever_arm_offset) +mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4378,15 +5303,14 @@ mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, ui insert_u8(&serializer, source); - assert(lever_arm_offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_float(&serializer, lever_arm_offset[i]); + insert_mip_vector3f(&serializer, &lever_arm_offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, float* lever_arm_offset_out) +mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4408,9 +5332,8 @@ mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uin extract_u8(&deserializer, &source); - assert(lever_arm_offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_float(&deserializer, &lever_arm_offset_out[i]); + extract_mip_vector3f(&deserializer, &lever_arm_offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index d4602c38b..04a70d0e9 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -220,7 +220,7 @@ void extract(Serializer& serializer, ExternalGnssUpdate& self) } -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) +CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, Vector3f velocity, Vector3f posUncertainty, Vector3f velUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -235,15 +235,12 @@ CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t insert(serializer, height); - assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); - assert(posUncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, posUncertainty[i]); - assert(velUncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velUncertainty[i]); @@ -684,13 +681,12 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self) } -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) +CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) insert(serializer, dcm[i]); @@ -698,7 +694,7 @@ CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) +CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -713,7 +709,6 @@ CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut { Serializer deserializer(buffer, responseLength); - assert(dcmOut || (9 == 0)); for(unsigned int i=0; i < 9; i++) extract(deserializer, dcmOut[i]); @@ -788,13 +783,12 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response } -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) +CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quat) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(quat || (4 == 0)); for(unsigned int i=0; i < 4; i++) insert(serializer, quat[i]); @@ -802,7 +796,7 @@ CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) +CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quatOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -817,7 +811,6 @@ CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* { Serializer deserializer(buffer, responseLength); - assert(quatOut || (4 == 0)); for(unsigned int i=0; i < 4; i++) extract(deserializer, quatOut[i]); @@ -892,13 +885,12 @@ void extract(Serializer& serializer, SensorToVehicleOffset::Response& self) } -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) +CmdResult writeSensorToVehicleOffset(C::mip_interface& device, Vector3f offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, offset[i]); @@ -906,7 +898,7 @@ CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offs return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) +CmdResult readSensorToVehicleOffset(C::mip_interface& device, Vector3f offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -921,7 +913,6 @@ CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) { Serializer deserializer(buffer, responseLength); - assert(offsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, offsetOut[i]); @@ -996,13 +987,12 @@ void extract(Serializer& serializer, AntennaOffset::Response& self) } -CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) +CmdResult writeAntennaOffset(C::mip_interface& device, Vector3f offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, offset[i]); @@ -1010,7 +1000,7 @@ CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) +CmdResult readAntennaOffset(C::mip_interface& device, Vector3f offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1025,7 +1015,6 @@ CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) { Serializer deserializer(buffer, responseLength); - assert(offsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, offsetOut[i]); @@ -1391,13 +1380,12 @@ void extract(Serializer& serializer, AccelNoise::Response& self) } -CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) +CmdResult writeAccelNoise(C::mip_interface& device, Vector3f noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1405,7 +1393,7 @@ CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) +CmdResult readAccelNoise(C::mip_interface& device, Vector3f noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1420,7 +1408,6 @@ CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) { Serializer deserializer(buffer, responseLength); - assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -1495,13 +1482,12 @@ void extract(Serializer& serializer, GyroNoise::Response& self) } -CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) +CmdResult writeGyroNoise(C::mip_interface& device, Vector3f noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1509,7 +1495,7 @@ CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) +CmdResult readGyroNoise(C::mip_interface& device, Vector3f noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1524,7 +1510,6 @@ CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) { Serializer deserializer(buffer, responseLength); - assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -1611,17 +1596,15 @@ void extract(Serializer& serializer, AccelBiasModel::Response& self) } -CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) +CmdResult writeAccelBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, beta[i]); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1629,7 +1612,7 @@ CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +CmdResult readAccelBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1644,11 +1627,9 @@ CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* no { Serializer deserializer(buffer, responseLength); - assert(betaOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, betaOut[i]); - assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -1735,17 +1716,15 @@ void extract(Serializer& serializer, GyroBiasModel::Response& self) } -CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) +CmdResult writeGyroBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); - assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, beta[i]); - assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1753,7 +1732,7 @@ CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +CmdResult readGyroBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1768,11 +1747,9 @@ CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noi { Serializer deserializer(buffer, responseLength); - assert(betaOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, betaOut[i]); - assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -2255,6 +2232,872 @@ CmdResult commandedAngularZupt(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); } +void insert(Serializer& serializer, const MagCaptureAutoCal& self) +{ + insert(serializer, self.function); + +} +void extract(Serializer& serializer, MagCaptureAutoCal& self) +{ + extract(serializer, self.function); + +} + +CmdResult writeMagCaptureAutoCal(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult saveMagCaptureAutoCal(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const GravityNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, GravityNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const GravityNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, GravityNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeGravityNoise(C::mip_interface& device, Vector3f noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readGravityNoise(C::mip_interface& device, Vector3f noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultGravityNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const PressureAltitudeNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.noise); + + } +} +void extract(Serializer& serializer, PressureAltitudeNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.noise); + + } +} + +void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self) +{ + insert(serializer, self.noise); + +} +void extract(Serializer& serializer, PressureAltitudeNoise::Response& self) +{ + extract(serializer, self.noise); + +} + +CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, noise); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), CMD_PRESSURE_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(noiseOut); + extract(deserializer, *noiseOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult savePressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadPressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultPressureAltitudeNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const HardIronOffsetNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, HardIronOffsetNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, Vector3f noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readHardIronOffsetNoise(C::mip_interface& device, Vector3f noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const SoftIronMatrixNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 9; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, SoftIronMatrixNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 9; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self) +{ + for(unsigned int i=0; i < 9; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self) +{ + for(unsigned int i=0; i < 9; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + for(unsigned int i=0; i < 9; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + for(unsigned int i=0; i < 9; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagNoise& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + + } +} +void extract(Serializer& serializer, MagNoise& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + + } +} + +void insert(Serializer& serializer, const MagNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.noise[i]); + +} +void extract(Serializer& serializer, MagNoise::Response& self) +{ + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.noise[i]); + +} + +CmdResult writeMagNoise(C::mip_interface& device, Vector3f noise) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + for(unsigned int i=0; i < 3; i++) + insert(serializer, noise[i]); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMagNoise(C::mip_interface& device, Vector3f noiseOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + for(unsigned int i=0; i < 3; i++) + extract(deserializer, noiseOut[i]); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMagNoise(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const InclinationSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.inclination); + + } +} +void extract(Serializer& serializer, InclinationSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.inclination); + + } +} + +void insert(Serializer& serializer, const InclinationSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.inclination); + +} +void extract(Serializer& serializer, InclinationSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.inclination); + +} + +CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + insert(serializer, inclination); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + assert(inclinationOut); + extract(deserializer, *inclinationOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultInclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagneticDeclinationSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.declination); + + } +} +void extract(Serializer& serializer, MagneticDeclinationSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.declination); + + } +} + +void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.declination); + +} +void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.declination); + +} + +CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + insert(serializer, declination); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + assert(declinationOut); + extract(deserializer, *declinationOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const MagFieldMagnitudeSource& self) +{ + insert(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + insert(serializer, self.source); + + insert(serializer, self.magnitude); + + } +} +void extract(Serializer& serializer, MagFieldMagnitudeSource& self) +{ + extract(serializer, self.function); + + if( self.function == FunctionSelector::WRITE ) + { + extract(serializer, self.source); + + extract(serializer, self.magnitude); + + } +} + +void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self) +{ + insert(serializer, self.source); + + insert(serializer, self.magnitude); + +} +void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self) +{ + extract(serializer, self.source); + + extract(serializer, self.magnitude); + +} + +CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::WRITE); + insert(serializer, source); + + insert(serializer, magnitude); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::READ); + assert(serializer.isOk()); + + uint8_t responseLength = sizeof(buffer); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); + + if( result == MIP_ACK_OK ) + { + Serializer deserializer(buffer, responseLength); + + assert(sourceOut); + extract(deserializer, *sourceOut); + + assert(magnitudeOut); + extract(deserializer, *magnitudeOut); + + if( deserializer.remaining() != 0 ) + result = MIP_STATUS_ERROR; + } + return result; +} +CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::SAVE); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::LOAD); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, FunctionSelector::RESET); + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const ReferencePosition& self) { insert(serializer, self.function); @@ -3249,7 +4092,7 @@ void extract(Serializer& serializer, InitializationConfiguration::Response& self } -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) +CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, Vector3f initialPosition, Vector3f initialVelocity, FilterReferenceFrame referenceFrameSelector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3267,11 +4110,9 @@ CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t wai insert(serializer, initialRoll); - assert(initialPosition || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, initialPosition[i]); - assert(initialVelocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, initialVelocity[i]); @@ -3281,7 +4122,7 @@ CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t wai return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) +CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, Vector3f initialPositionOut, Vector3f initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3314,11 +4155,9 @@ CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* wai assert(initialRollOut); extract(deserializer, *initialRollOut); - assert(initialPositionOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, initialPositionOut[i]); - assert(initialVelocityOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, initialVelocityOut[i]); @@ -3514,7 +4353,7 @@ void extract(Serializer& serializer, MultiAntennaOffset::Response& self) } -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) +CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3522,7 +4361,6 @@ CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, insert(serializer, FunctionSelector::WRITE); insert(serializer, receiverId); - assert(antennaOffset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, antennaOffset[i]); @@ -3530,7 +4368,7 @@ CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) +CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3549,7 +4387,6 @@ CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, f extract(deserializer, receiverId); - assert(antennaOffsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, antennaOffsetOut[i]); @@ -3646,7 +4483,7 @@ void extract(Serializer& serializer, RelPosConfiguration::Response& self) } -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) +CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, Vector3d referenceCoordinates) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3656,7 +4493,6 @@ CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, Fil insert(serializer, referenceFrameSelector); - assert(referenceCoordinates || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, referenceCoordinates[i]); @@ -3664,7 +4500,7 @@ CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, Fil return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) +CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, Vector3d referenceCoordinatesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3685,7 +4521,6 @@ CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, assert(referenceFrameSelectorOut); extract(deserializer, *referenceFrameSelectorOut); - assert(referenceCoordinatesOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, referenceCoordinatesOut[i]); @@ -3768,7 +4603,7 @@ void extract(Serializer& serializer, RefPointLeverArm::Response& self) } -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) +CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, Vector3f leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3776,7 +4611,6 @@ CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refe insert(serializer, FunctionSelector::WRITE); insert(serializer, refPointSel); - assert(leverArmOffset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, leverArmOffset[i]); @@ -3784,7 +4618,7 @@ CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refe return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) +CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, Vector3f leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3802,7 +4636,6 @@ CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refer assert(refPointSelOut); extract(deserializer, *refPointSelOut); - assert(leverArmOffsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, leverArmOffsetOut[i]); @@ -3925,7 +4758,7 @@ void extract(Serializer& serializer, SpeedLeverArm::Response& self) } -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) +CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3933,7 +4766,6 @@ CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const flo insert(serializer, FunctionSelector::WRITE); insert(serializer, source); - assert(leverArmOffset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, leverArmOffset[i]); @@ -3941,7 +4773,7 @@ CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const flo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) +CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3960,7 +4792,6 @@ CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* lev extract(deserializer, source); - assert(leverArmOffsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, leverArmOffsetOut[i]); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 43baa0bb4..2ca7ed16f 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -193,6 +194,7 @@ void extract_mip_filter_adaptive_measurement(struct mip_serializer* serializer, ///@{ mip_cmd_result mip_filter_reset(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -224,6 +226,7 @@ void insert_mip_filter_set_initial_attitude_command(struct mip_serializer* seria void extract_mip_filter_set_initial_attitude_command(struct mip_serializer* serializer, mip_filter_set_initial_attitude_command* self); mip_cmd_result mip_filter_set_initial_attitude(struct mip_interface* device, float roll, float pitch, float heading); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -280,6 +283,7 @@ mip_cmd_result mip_filter_read_estimation_control(struct mip_interface* device, mip_cmd_result mip_filter_save_estimation_control(struct mip_interface* device); mip_cmd_result mip_filter_load_estimation_control(struct mip_interface* device); mip_cmd_result mip_filter_default_estimation_control(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -299,16 +303,17 @@ struct mip_filter_external_gnss_update_command double latitude; ///< [degrees] double longitude; ///< [degrees] double height; ///< Above WGS84 ellipsoid [meters] - float velocity[3]; ///< NED Frame [meters/second] - float pos_uncertainty[3]; ///< NED Frame, 1-sigma [meters] - float vel_uncertainty[3]; ///< NED Frame, 1-sigma [meters/second] + mip_vector3f velocity; ///< NED Frame [meters/second] + mip_vector3f pos_uncertainty; ///< NED Frame, 1-sigma [meters] + mip_vector3f vel_uncertainty; ///< NED Frame, 1-sigma [meters/second] }; typedef struct mip_filter_external_gnss_update_command mip_filter_external_gnss_update_command; void insert_mip_filter_external_gnss_update_command(struct mip_serializer* serializer, const mip_filter_external_gnss_update_command* self); void extract_mip_filter_external_gnss_update_command(struct mip_serializer* serializer, mip_filter_external_gnss_update_command* self); -mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, const float* velocity, const float* pos_uncertainty, const float* vel_uncertainty); +mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, mip_vector3f velocity, mip_vector3f pos_uncertainty, mip_vector3f vel_uncertainty); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -341,6 +346,7 @@ void insert_mip_filter_external_heading_update_command(struct mip_serializer* se void extract_mip_filter_external_heading_update_command(struct mip_serializer* serializer, mip_filter_external_heading_update_command* self); mip_cmd_result mip_filter_external_heading_update(struct mip_interface* device, float heading, float heading_uncertainty, uint8_t type); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -379,6 +385,7 @@ void insert_mip_filter_external_heading_update_with_time_command(struct mip_seri void extract_mip_filter_external_heading_update_with_time_command(struct mip_serializer* serializer, mip_filter_external_heading_update_with_time_command* self); mip_cmd_result mip_filter_external_heading_update_with_time(struct mip_interface* device, double gps_time, uint16_t gps_week, float heading, float heading_uncertainty, uint8_t type); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -425,6 +432,7 @@ mip_cmd_result mip_filter_read_tare_orientation(struct mip_interface* device, mi mip_cmd_result mip_filter_save_tare_orientation(struct mip_interface* device); mip_cmd_result mip_filter_load_tare_orientation(struct mip_interface* device); mip_cmd_result mip_filter_default_tare_orientation(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -466,6 +474,7 @@ mip_cmd_result mip_filter_read_vehicle_dynamics_mode(struct mip_interface* devic mip_cmd_result mip_filter_save_vehicle_dynamics_mode(struct mip_interface* device); mip_cmd_result mip_filter_load_vehicle_dynamics_mode(struct mip_interface* device); mip_cmd_result mip_filter_default_vehicle_dynamics_mode(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -524,6 +533,7 @@ mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_euler(struct mip_inter mip_cmd_result mip_filter_save_sensor_to_vehicle_rotation_euler(struct mip_interface* device); mip_cmd_result mip_filter_load_sensor_to_vehicle_rotation_euler(struct mip_interface* device); mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_euler(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -563,7 +573,7 @@ mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_euler(struct mip_in struct mip_filter_sensor_to_vehicle_rotation_dcm_command { mip_function_selector function; - float dcm[9]; + mip_matrix3f dcm; }; typedef struct mip_filter_sensor_to_vehicle_rotation_dcm_command mip_filter_sensor_to_vehicle_rotation_dcm_command; @@ -572,18 +582,19 @@ void extract_mip_filter_sensor_to_vehicle_rotation_dcm_command(struct mip_serial struct mip_filter_sensor_to_vehicle_rotation_dcm_response { - float dcm[9]; + mip_matrix3f dcm; }; typedef struct mip_filter_sensor_to_vehicle_rotation_dcm_response mip_filter_sensor_to_vehicle_rotation_dcm_response; void insert_mip_filter_sensor_to_vehicle_rotation_dcm_response(struct mip_serializer* serializer, const mip_filter_sensor_to_vehicle_rotation_dcm_response* self); void extract_mip_filter_sensor_to_vehicle_rotation_dcm_response(struct mip_serializer* serializer, mip_filter_sensor_to_vehicle_rotation_dcm_response* self); -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, const float* dcm); -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, float* dcm_out); +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm); +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm_out); mip_cmd_result mip_filter_save_sensor_to_vehicle_rotation_dcm(struct mip_interface* device); mip_cmd_result mip_filter_load_sensor_to_vehicle_rotation_dcm(struct mip_interface* device); mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_dcm(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -622,7 +633,7 @@ mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_dcm(struct mip_inte struct mip_filter_sensor_to_vehicle_rotation_quaternion_command { mip_function_selector function; - float quat[4]; + mip_quatf quat; }; typedef struct mip_filter_sensor_to_vehicle_rotation_quaternion_command mip_filter_sensor_to_vehicle_rotation_quaternion_command; @@ -631,18 +642,19 @@ void extract_mip_filter_sensor_to_vehicle_rotation_quaternion_command(struct mip struct mip_filter_sensor_to_vehicle_rotation_quaternion_response { - float quat[4]; + mip_quatf quat; }; typedef struct mip_filter_sensor_to_vehicle_rotation_quaternion_response mip_filter_sensor_to_vehicle_rotation_quaternion_response; void insert_mip_filter_sensor_to_vehicle_rotation_quaternion_response(struct mip_serializer* serializer, const mip_filter_sensor_to_vehicle_rotation_quaternion_response* self); void extract_mip_filter_sensor_to_vehicle_rotation_quaternion_response(struct mip_serializer* serializer, mip_filter_sensor_to_vehicle_rotation_quaternion_response* self); -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, const float* quat); -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, float* quat_out); +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat); +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat_out); mip_cmd_result mip_filter_save_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device); mip_cmd_result mip_filter_load_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device); mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -662,7 +674,7 @@ mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_quaternion(struct m struct mip_filter_sensor_to_vehicle_offset_command { mip_function_selector function; - float offset[3]; ///< [meters] + mip_vector3f offset; ///< [meters] }; typedef struct mip_filter_sensor_to_vehicle_offset_command mip_filter_sensor_to_vehicle_offset_command; @@ -671,18 +683,19 @@ void extract_mip_filter_sensor_to_vehicle_offset_command(struct mip_serializer* struct mip_filter_sensor_to_vehicle_offset_response { - float offset[3]; ///< [meters] + mip_vector3f offset; ///< [meters] }; typedef struct mip_filter_sensor_to_vehicle_offset_response mip_filter_sensor_to_vehicle_offset_response; void insert_mip_filter_sensor_to_vehicle_offset_response(struct mip_serializer* serializer, const mip_filter_sensor_to_vehicle_offset_response* self); void extract_mip_filter_sensor_to_vehicle_offset_response(struct mip_serializer* serializer, mip_filter_sensor_to_vehicle_offset_response* self); -mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, const float* offset); -mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, float* offset_out); +mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset); +mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset_out); mip_cmd_result mip_filter_save_sensor_to_vehicle_offset(struct mip_interface* device); mip_cmd_result mip_filter_load_sensor_to_vehicle_offset(struct mip_interface* device); mip_cmd_result mip_filter_default_sensor_to_vehicle_offset(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -699,7 +712,7 @@ mip_cmd_result mip_filter_default_sensor_to_vehicle_offset(struct mip_interface* struct mip_filter_antenna_offset_command { mip_function_selector function; - float offset[3]; ///< [meters] + mip_vector3f offset; ///< [meters] }; typedef struct mip_filter_antenna_offset_command mip_filter_antenna_offset_command; @@ -708,18 +721,19 @@ void extract_mip_filter_antenna_offset_command(struct mip_serializer* serializer struct mip_filter_antenna_offset_response { - float offset[3]; ///< [meters] + mip_vector3f offset; ///< [meters] }; typedef struct mip_filter_antenna_offset_response mip_filter_antenna_offset_response; void insert_mip_filter_antenna_offset_response(struct mip_serializer* serializer, const mip_filter_antenna_offset_response* self); void extract_mip_filter_antenna_offset_response(struct mip_serializer* serializer, mip_filter_antenna_offset_response* self); -mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, const float* offset); -mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, float* offset_out); +mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, mip_vector3f offset); +mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, mip_vector3f offset_out); mip_cmd_result mip_filter_save_antenna_offset(struct mip_interface* device); mip_cmd_result mip_filter_load_antenna_offset(struct mip_interface* device); mip_cmd_result mip_filter_default_antenna_offset(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -765,6 +779,7 @@ mip_cmd_result mip_filter_read_gnss_source(struct mip_interface* device, mip_fil mip_cmd_result mip_filter_save_gnss_source(struct mip_interface* device); mip_cmd_result mip_filter_load_gnss_source(struct mip_interface* device); mip_cmd_result mip_filter_default_gnss_source(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -821,6 +836,7 @@ mip_cmd_result mip_filter_read_heading_source(struct mip_interface* device, mip_ mip_cmd_result mip_filter_save_heading_source(struct mip_interface* device); mip_cmd_result mip_filter_load_heading_source(struct mip_interface* device); mip_cmd_result mip_filter_default_heading_source(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -861,6 +877,7 @@ mip_cmd_result mip_filter_read_auto_init_control(struct mip_interface* device, u mip_cmd_result mip_filter_save_auto_init_control(struct mip_interface* device); mip_cmd_result mip_filter_load_auto_init_control(struct mip_interface* device); mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -878,7 +895,7 @@ mip_cmd_result mip_filter_default_auto_init_control(struct mip_interface* device struct mip_filter_accel_noise_command { mip_function_selector function; - float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] + mip_vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_noise_command mip_filter_accel_noise_command; @@ -887,18 +904,19 @@ void extract_mip_filter_accel_noise_command(struct mip_serializer* serializer, m struct mip_filter_accel_noise_response { - float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] + mip_vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_noise_response mip_filter_accel_noise_response; void insert_mip_filter_accel_noise_response(struct mip_serializer* serializer, const mip_filter_accel_noise_response* self); void extract_mip_filter_accel_noise_response(struct mip_serializer* serializer, mip_filter_accel_noise_response* self); -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise); -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, mip_vector3f noise); +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, mip_vector3f noise_out); mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -916,7 +934,7 @@ mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); struct mip_filter_gyro_noise_command { mip_function_selector function; - float noise[3]; ///< Gyro Noise 1-sigma [rad/second] + mip_vector3f noise; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_noise_command mip_filter_gyro_noise_command; @@ -925,18 +943,19 @@ void extract_mip_filter_gyro_noise_command(struct mip_serializer* serializer, mi struct mip_filter_gyro_noise_response { - float noise[3]; ///< Gyro Noise 1-sigma [rad/second] + mip_vector3f noise; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_noise_response mip_filter_gyro_noise_response; void insert_mip_filter_gyro_noise_response(struct mip_serializer* serializer, const mip_filter_gyro_noise_response* self); void extract_mip_filter_gyro_noise_response(struct mip_serializer* serializer, mip_filter_gyro_noise_response* self); -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise); -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, mip_vector3f noise); +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, mip_vector3f noise_out); mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -951,8 +970,8 @@ mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); struct mip_filter_accel_bias_model_command { mip_function_selector function; - float beta[3]; ///< Accel Bias Beta [1/second] - float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] + mip_vector3f beta; ///< Accel Bias Beta [1/second] + mip_vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_bias_model_command mip_filter_accel_bias_model_command; @@ -961,19 +980,20 @@ void extract_mip_filter_accel_bias_model_command(struct mip_serializer* serializ struct mip_filter_accel_bias_model_response { - float beta[3]; ///< Accel Bias Beta [1/second] - float noise[3]; ///< Accel Noise 1-sigma [meters/second^2] + mip_vector3f beta; ///< Accel Bias Beta [1/second] + mip_vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] }; typedef struct mip_filter_accel_bias_model_response mip_filter_accel_bias_model_response; void insert_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, const mip_filter_accel_bias_model_response* self); void extract_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, mip_filter_accel_bias_model_response* self); -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise); -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise); +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out); mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -988,8 +1008,8 @@ mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device) struct mip_filter_gyro_bias_model_command { mip_function_selector function; - float beta[3]; ///< Gyro Bias Beta [1/second] - float noise[3]; ///< Gyro Noise 1-sigma [rad/second] + mip_vector3f beta; ///< Gyro Bias Beta [1/second] + mip_vector3f noise; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_bias_model_command mip_filter_gyro_bias_model_command; @@ -998,19 +1018,20 @@ void extract_mip_filter_gyro_bias_model_command(struct mip_serializer* serialize struct mip_filter_gyro_bias_model_response { - float beta[3]; ///< Gyro Bias Beta [1/second] - float noise[3]; ///< Gyro Noise 1-sigma [rad/second] + mip_vector3f beta; ///< Gyro Bias Beta [1/second] + mip_vector3f noise; ///< Gyro Noise 1-sigma [rad/second] }; typedef struct mip_filter_gyro_bias_model_response mip_filter_gyro_bias_model_response; void insert_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self); void extract_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, mip_filter_gyro_bias_model_response* self); -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise); -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise); +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out); mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1054,6 +1075,7 @@ mip_cmd_result mip_filter_read_altitude_aiding(struct mip_interface* device, mip mip_cmd_result mip_filter_save_altitude_aiding(struct mip_interface* device); mip_cmd_result mip_filter_load_altitude_aiding(struct mip_interface* device); mip_cmd_result mip_filter_default_altitude_aiding(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1094,6 +1116,7 @@ mip_cmd_result mip_filter_read_pitch_roll_aiding(struct mip_interface* device, m mip_cmd_result mip_filter_save_pitch_roll_aiding(struct mip_interface* device); mip_cmd_result mip_filter_load_pitch_roll_aiding(struct mip_interface* device); mip_cmd_result mip_filter_default_pitch_roll_aiding(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1129,6 +1152,7 @@ mip_cmd_result mip_filter_read_auto_zupt(struct mip_interface* device, uint8_t* mip_cmd_result mip_filter_save_auto_zupt(struct mip_interface* device); mip_cmd_result mip_filter_load_auto_zupt(struct mip_interface* device); mip_cmd_result mip_filter_default_auto_zupt(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1165,6 +1189,7 @@ mip_cmd_result mip_filter_read_auto_angular_zupt(struct mip_interface* device, u mip_cmd_result mip_filter_save_auto_angular_zupt(struct mip_interface* device); mip_cmd_result mip_filter_load_auto_angular_zupt(struct mip_interface* device); mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1174,6 +1199,7 @@ mip_cmd_result mip_filter_default_auto_angular_zupt(struct mip_interface* device ///@{ mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1183,6 +1209,7 @@ mip_cmd_result mip_filter_commanded_zupt(struct mip_interface* device); ///@{ mip_cmd_result mip_filter_commanded_angular_zupt(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1204,6 +1231,7 @@ void extract_mip_filter_mag_capture_auto_cal_command(struct mip_serializer* seri mip_cmd_result mip_filter_write_mag_capture_auto_cal(struct mip_interface* device); mip_cmd_result mip_filter_save_mag_capture_auto_cal(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1220,7 +1248,7 @@ mip_cmd_result mip_filter_save_mag_capture_auto_cal(struct mip_interface* device struct mip_filter_gravity_noise_command { mip_function_selector function; - float noise[3]; ///< Gravity Noise 1-sigma [gauss] + mip_vector3f noise; ///< Gravity Noise 1-sigma [gauss] }; typedef struct mip_filter_gravity_noise_command mip_filter_gravity_noise_command; @@ -1229,18 +1257,19 @@ void extract_mip_filter_gravity_noise_command(struct mip_serializer* serializer, struct mip_filter_gravity_noise_response { - float noise[3]; ///< Gravity Noise 1-sigma [gauss] + mip_vector3f noise; ///< Gravity Noise 1-sigma [gauss] }; typedef struct mip_filter_gravity_noise_response mip_filter_gravity_noise_response; void insert_mip_filter_gravity_noise_response(struct mip_serializer* serializer, const mip_filter_gravity_noise_response* self); void extract_mip_filter_gravity_noise_response(struct mip_serializer* serializer, mip_filter_gravity_noise_response* self); -mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, const float* noise); -mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, mip_vector3f noise); +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, mip_vector3f noise_out); mip_cmd_result mip_filter_save_gravity_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_gravity_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_gravity_noise(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1278,6 +1307,7 @@ mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* dev mip_cmd_result mip_filter_save_pressure_altitude_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_pressure_altitude_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_pressure_altitude_noise(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1296,7 +1326,7 @@ mip_cmd_result mip_filter_default_pressure_altitude_noise(struct mip_interface* struct mip_filter_hard_iron_offset_noise_command { mip_function_selector function; - float noise[3]; ///< Hard Iron Offset Noise 1-sigma [gauss] + mip_vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] }; typedef struct mip_filter_hard_iron_offset_noise_command mip_filter_hard_iron_offset_noise_command; @@ -1305,18 +1335,19 @@ void extract_mip_filter_hard_iron_offset_noise_command(struct mip_serializer* se struct mip_filter_hard_iron_offset_noise_response { - float noise[3]; ///< Hard Iron Offset Noise 1-sigma [gauss] + mip_vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] }; typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_offset_noise_response; void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); -mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, const float* noise); -mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise); +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise_out); mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1334,7 +1365,7 @@ mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* d struct mip_filter_soft_iron_matrix_noise_command { mip_function_selector function; - float noise[9]; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + mip_matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] }; typedef struct mip_filter_soft_iron_matrix_noise_command mip_filter_soft_iron_matrix_noise_command; @@ -1343,18 +1374,19 @@ void extract_mip_filter_soft_iron_matrix_noise_command(struct mip_serializer* se struct mip_filter_soft_iron_matrix_noise_response { - float noise[9]; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + mip_matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] }; typedef struct mip_filter_soft_iron_matrix_noise_response mip_filter_soft_iron_matrix_noise_response; void insert_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_response* self); void extract_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_response* self); -mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, const float* noise); -mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise); +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise_out); mip_cmd_result mip_filter_save_soft_iron_matrix_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_soft_iron_matrix_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1372,7 +1404,7 @@ mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* d struct mip_filter_mag_noise_command { mip_function_selector function; - float noise[3]; ///< Mag Noise 1-sigma [gauss] + mip_vector3f noise; ///< Mag Noise 1-sigma [gauss] }; typedef struct mip_filter_mag_noise_command mip_filter_mag_noise_command; @@ -1381,18 +1413,19 @@ void extract_mip_filter_mag_noise_command(struct mip_serializer* serializer, mip struct mip_filter_mag_noise_response { - float noise[3]; ///< Mag Noise 1-sigma [gauss] + mip_vector3f noise; ///< Mag Noise 1-sigma [gauss] }; typedef struct mip_filter_mag_noise_response mip_filter_mag_noise_response; void insert_mip_filter_mag_noise_response(struct mip_serializer* serializer, const mip_filter_mag_noise_response* self); void extract_mip_filter_mag_noise_response(struct mip_serializer* serializer, mip_filter_mag_noise_response* self); -mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, const float* noise); -mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, float* noise_out); +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, mip_vector3f noise); +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, mip_vector3f noise_out); mip_cmd_result mip_filter_save_mag_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_mag_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_mag_noise(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1431,6 +1464,7 @@ mip_cmd_result mip_filter_read_inclination_source(struct mip_interface* device, mip_cmd_result mip_filter_save_inclination_source(struct mip_interface* device); mip_cmd_result mip_filter_load_inclination_source(struct mip_interface* device); mip_cmd_result mip_filter_default_inclination_source(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1469,6 +1503,7 @@ mip_cmd_result mip_filter_read_magnetic_declination_source(struct mip_interface* mip_cmd_result mip_filter_save_magnetic_declination_source(struct mip_interface* device); mip_cmd_result mip_filter_load_magnetic_declination_source(struct mip_interface* device); mip_cmd_result mip_filter_default_magnetic_declination_source(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1506,6 +1541,7 @@ mip_cmd_result mip_filter_read_mag_field_magnitude_source(struct mip_interface* mip_cmd_result mip_filter_save_mag_field_magnitude_source(struct mip_interface* device); mip_cmd_result mip_filter_load_mag_field_magnitude_source(struct mip_interface* device); mip_cmd_result mip_filter_default_mag_field_magnitude_source(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1547,6 +1583,7 @@ mip_cmd_result mip_filter_read_reference_position(struct mip_interface* device, mip_cmd_result mip_filter_save_reference_position(struct mip_interface* device); mip_cmd_result mip_filter_load_reference_position(struct mip_interface* device); mip_cmd_result mip_filter_default_reference_position(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1604,6 +1641,7 @@ mip_cmd_result mip_filter_read_accel_magnitude_error_adaptive_measurement(struct mip_cmd_result mip_filter_save_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_magnitude_error_adaptive_measurement(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1656,6 +1694,7 @@ mip_cmd_result mip_filter_read_mag_magnitude_error_adaptive_measurement(struct m mip_cmd_result mip_filter_save_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); mip_cmd_result mip_filter_load_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); mip_cmd_result mip_filter_default_mag_magnitude_error_adaptive_measurement(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1706,6 +1745,7 @@ mip_cmd_result mip_filter_read_mag_dip_angle_error_adaptive_measurement(struct m mip_cmd_result mip_filter_save_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); mip_cmd_result mip_filter_load_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); mip_cmd_result mip_filter_default_mag_dip_angle_error_adaptive_measurement(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1754,6 +1794,7 @@ mip_cmd_result mip_filter_read_aiding_measurement_enable(struct mip_interface* d mip_cmd_result mip_filter_save_aiding_measurement_enable(struct mip_interface* device, mip_filter_aiding_measurement_enable_command_aiding_source aiding_source); mip_cmd_result mip_filter_load_aiding_measurement_enable(struct mip_interface* device, mip_filter_aiding_measurement_enable_command_aiding_source aiding_source); mip_cmd_result mip_filter_default_aiding_measurement_enable(struct mip_interface* device, mip_filter_aiding_measurement_enable_command_aiding_source aiding_source); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1765,6 +1806,7 @@ mip_cmd_result mip_filter_default_aiding_measurement_enable(struct mip_interface ///@{ mip_cmd_result mip_filter_run(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1803,6 +1845,7 @@ mip_cmd_result mip_filter_read_kinematic_constraint(struct mip_interface* device mip_cmd_result mip_filter_save_kinematic_constraint(struct mip_interface* device); mip_cmd_result mip_filter_load_kinematic_constraint(struct mip_interface* device); mip_cmd_result mip_filter_default_kinematic_constraint(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1838,8 +1881,8 @@ struct mip_filter_initialization_configuration_command float initial_heading; ///< User-specified initial platform heading (degrees). float initial_pitch; ///< User-specified initial platform pitch (degrees) float initial_roll; ///< User-specified initial platform roll (degrees) - float initial_position[3]; ///< User-specified initial platform position (units determined by reference frame selector, see note.) - float initial_velocity[3]; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) + mip_vector3f initial_position; ///< User-specified initial platform position (units determined by reference frame selector, see note.) + mip_vector3f initial_velocity; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) mip_filter_reference_frame reference_frame_selector; ///< User-specified initial position/velocity reference frames }; @@ -1861,8 +1904,8 @@ struct mip_filter_initialization_configuration_response float initial_heading; ///< User-specified initial platform heading (degrees). float initial_pitch; ///< User-specified initial platform pitch (degrees) float initial_roll; ///< User-specified initial platform roll (degrees) - float initial_position[3]; ///< User-specified initial platform position (units determined by reference frame selector, see note.) - float initial_velocity[3]; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) + mip_vector3f initial_position; ///< User-specified initial platform position (units determined by reference frame selector, see note.) + mip_vector3f initial_velocity; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) mip_filter_reference_frame reference_frame_selector; ///< User-specified initial position/velocity reference frames }; @@ -1870,11 +1913,12 @@ typedef struct mip_filter_initialization_configuration_response mip_filter_initi void insert_mip_filter_initialization_configuration_response(struct mip_serializer* serializer, const mip_filter_initialization_configuration_response* self); void extract_mip_filter_initialization_configuration_response(struct mip_serializer* serializer, mip_filter_initialization_configuration_response* self); -mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, const float* initial_position, const float* initial_velocity, mip_filter_reference_frame reference_frame_selector); -mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, float* initial_position_out, float* initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out); +mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, mip_vector3f initial_position, mip_vector3f initial_velocity, mip_filter_reference_frame reference_frame_selector); +mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, mip_vector3f initial_position_out, mip_vector3f initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out); mip_cmd_result mip_filter_save_initialization_configuration(struct mip_interface* device); mip_cmd_result mip_filter_load_initialization_configuration(struct mip_interface* device); mip_cmd_result mip_filter_default_initialization_configuration(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1909,6 +1953,7 @@ mip_cmd_result mip_filter_read_adaptive_filter_options(struct mip_interface* dev mip_cmd_result mip_filter_save_adaptive_filter_options(struct mip_interface* device); mip_cmd_result mip_filter_load_adaptive_filter_options(struct mip_interface* device); mip_cmd_result mip_filter_default_adaptive_filter_options(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1924,7 +1969,7 @@ struct mip_filter_multi_antenna_offset_command { mip_function_selector function; uint8_t receiver_id; ///< Receiver: 1, 2, etc... - float antenna_offset[3]; ///< Antenna lever arm offset vector in the vehicle frame (m) + mip_vector3f antenna_offset; ///< Antenna lever arm offset vector in the vehicle frame (m) }; typedef struct mip_filter_multi_antenna_offset_command mip_filter_multi_antenna_offset_command; @@ -1934,18 +1979,19 @@ void extract_mip_filter_multi_antenna_offset_command(struct mip_serializer* seri struct mip_filter_multi_antenna_offset_response { uint8_t receiver_id; - float antenna_offset[3]; + mip_vector3f antenna_offset; }; typedef struct mip_filter_multi_antenna_offset_response mip_filter_multi_antenna_offset_response; void insert_mip_filter_multi_antenna_offset_response(struct mip_serializer* serializer, const mip_filter_multi_antenna_offset_response* self); void extract_mip_filter_multi_antenna_offset_response(struct mip_serializer* serializer, mip_filter_multi_antenna_offset_response* self); -mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, const float* antenna_offset); -mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, float* antenna_offset_out); +mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset); +mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset_out); mip_cmd_result mip_filter_save_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id); mip_cmd_result mip_filter_load_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id); mip_cmd_result mip_filter_default_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1959,7 +2005,7 @@ struct mip_filter_rel_pos_configuration_command mip_function_selector function; uint8_t source; ///< 0 - auto (RTK base station), 1 - manual mip_filter_reference_frame reference_frame_selector; ///< ECEF or LLH - double reference_coordinates[3]; ///< reference coordinates, units determined by source selection + mip_vector3d reference_coordinates; ///< reference coordinates, units determined by source selection }; typedef struct mip_filter_rel_pos_configuration_command mip_filter_rel_pos_configuration_command; @@ -1970,18 +2016,19 @@ struct mip_filter_rel_pos_configuration_response { uint8_t source; ///< 0 - auto (RTK base station), 1 - manual mip_filter_reference_frame reference_frame_selector; ///< ECEF or LLH - double reference_coordinates[3]; ///< reference coordinates, units determined by source selection + mip_vector3d reference_coordinates; ///< reference coordinates, units determined by source selection }; typedef struct mip_filter_rel_pos_configuration_response mip_filter_rel_pos_configuration_response; void insert_mip_filter_rel_pos_configuration_response(struct mip_serializer* serializer, const mip_filter_rel_pos_configuration_response* self); void extract_mip_filter_rel_pos_configuration_response(struct mip_serializer* serializer, mip_filter_rel_pos_configuration_response* self); -mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, const double* reference_coordinates); -mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, double* reference_coordinates_out); +mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, mip_vector3d reference_coordinates); +mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, mip_vector3d reference_coordinates_out); mip_cmd_result mip_filter_save_rel_pos_configuration(struct mip_interface* device); mip_cmd_result mip_filter_load_rel_pos_configuration(struct mip_interface* device); mip_cmd_result mip_filter_default_rel_pos_configuration(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2004,7 +2051,7 @@ struct mip_filter_ref_point_lever_arm_command { mip_function_selector function; mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel; ///< Reserved, must be 1 - float lever_arm_offset[3]; ///< [m] Lever arm offset vector in the vehicle's reference frame. + mip_vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. }; typedef struct mip_filter_ref_point_lever_arm_command mip_filter_ref_point_lever_arm_command; @@ -2017,18 +2064,19 @@ void extract_mip_filter_ref_point_lever_arm_command_reference_point_selector(str struct mip_filter_ref_point_lever_arm_response { mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel; ///< Reserved, must be 1 - float lever_arm_offset[3]; ///< [m] Lever arm offset vector in the vehicle's reference frame. + mip_vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. }; typedef struct mip_filter_ref_point_lever_arm_response mip_filter_ref_point_lever_arm_response; void insert_mip_filter_ref_point_lever_arm_response(struct mip_serializer* serializer, const mip_filter_ref_point_lever_arm_response* self); void extract_mip_filter_ref_point_lever_arm_response(struct mip_serializer* serializer, mip_filter_ref_point_lever_arm_response* self); -mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, const float* lever_arm_offset); -mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, float* lever_arm_offset_out); +mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, mip_vector3f lever_arm_offset); +mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, mip_vector3f lever_arm_offset_out); mip_cmd_result mip_filter_save_ref_point_lever_arm(struct mip_interface* device); mip_cmd_result mip_filter_load_ref_point_lever_arm(struct mip_interface* device); mip_cmd_result mip_filter_default_ref_point_lever_arm(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2052,6 +2100,7 @@ void insert_mip_filter_speed_measurement_command(struct mip_serializer* serializ void extract_mip_filter_speed_measurement_command(struct mip_serializer* serializer, mip_filter_speed_measurement_command* self); mip_cmd_result mip_filter_speed_measurement(struct mip_interface* device, uint8_t source, float time_of_week, float speed, float speed_uncertainty); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2070,7 +2119,7 @@ struct mip_filter_speed_lever_arm_command { mip_function_selector function; uint8_t source; ///< Reserved, must be 1. - float lever_arm_offset[3]; ///< [m] Lever arm offset vector in the vehicle's reference frame. + mip_vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. }; typedef struct mip_filter_speed_lever_arm_command mip_filter_speed_lever_arm_command; @@ -2080,18 +2129,19 @@ void extract_mip_filter_speed_lever_arm_command(struct mip_serializer* serialize struct mip_filter_speed_lever_arm_response { uint8_t source; ///< Reserved, must be 1. - float lever_arm_offset[3]; ///< [m] Lever arm offset vector in the vehicle's reference frame. + mip_vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. }; typedef struct mip_filter_speed_lever_arm_response mip_filter_speed_lever_arm_response; void insert_mip_filter_speed_lever_arm_response(struct mip_serializer* serializer, const mip_filter_speed_lever_arm_response* self); void extract_mip_filter_speed_lever_arm_response(struct mip_serializer* serializer, mip_filter_speed_lever_arm_response* self); -mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, const float* lever_arm_offset); -mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, float* lever_arm_offset_out); +mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset); +mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset_out); mip_cmd_result mip_filter_save_speed_lever_arm(struct mip_interface* device, uint8_t source); mip_cmd_result mip_filter_load_speed_lever_arm(struct mip_interface* device, uint8_t source); mip_cmd_result mip_filter_default_speed_lever_arm(struct mip_interface* device, uint8_t source); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2130,6 +2180,7 @@ mip_cmd_result mip_filter_read_wheeled_vehicle_constraint_control(struct mip_int mip_cmd_result mip_filter_save_wheeled_vehicle_constraint_control(struct mip_interface* device); mip_cmd_result mip_filter_load_wheeled_vehicle_constraint_control(struct mip_interface* device); mip_cmd_result mip_filter_default_wheeled_vehicle_constraint_control(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2166,6 +2217,7 @@ mip_cmd_result mip_filter_read_vertical_gyro_constraint_control(struct mip_inter mip_cmd_result mip_filter_save_vertical_gyro_constraint_control(struct mip_interface* device); mip_cmd_result mip_filter_load_vertical_gyro_constraint_control(struct mip_interface* device); mip_cmd_result mip_filter_default_vertical_gyro_constraint_control(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2202,6 +2254,7 @@ mip_cmd_result mip_filter_read_gnss_antenna_cal_control(struct mip_interface* de mip_cmd_result mip_filter_save_gnss_antenna_cal_control(struct mip_interface* device); mip_cmd_result mip_filter_load_gnss_antenna_cal_control(struct mip_interface* device); mip_cmd_result mip_filter_default_gnss_antenna_cal_control(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2223,6 +2276,7 @@ void insert_mip_filter_set_initial_heading_command(struct mip_serializer* serial void extract_mip_filter_set_initial_heading_command(struct mip_serializer* serializer, mip_filter_set_initial_heading_command* self); mip_cmd_result mip_filter_set_initial_heading(struct mip_interface* device, float heading); + ///@} /// diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 768222125..bdfde21b3 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -160,11 +161,11 @@ enum class FilterReferenceFrame : uint8_t LLH = 2, ///< WGS84 Latitude, longitude, and height above ellipsoid }; -enum class FilterMagDeclinationSource : uint8_t +enum class FilterMagParamSource : uint8_t { - NONE = 1, ///< Magnetic field is assumed to have an declination angle equal to zero. + NONE = 1, ///< No source. See command documentation for default behavior WMM = 2, ///< Magnetic field is assumed to conform to the World Magnetic Model, calculated using current location estimate as an input to the model. - MANUAL = 3, ///< Magnetic field is assumed to have the declination angle specified by the user. + MANUAL = 3, ///< Magnetic field is assumed to have the parameter specified by the user. }; enum class FilterAdaptiveMeasurement : uint8_t @@ -190,17 +191,25 @@ enum class FilterAdaptiveMeasurement : uint8_t struct Reset { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const Reset& self); void extract(Serializer& serializer, Reset& self); CmdResult reset(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -222,20 +231,28 @@ CmdResult reset(C::mip_interface& device); struct SetInitialAttitude { + float roll = 0; ///< [radians] + float pitch = 0; ///< [radians] + float heading = 0; ///< [radians] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; - float roll = 0; ///< [radians] - float pitch = 0; ///< [radians] - float heading = 0; ///< [radians] + auto as_tuple() const + { + return std::make_tuple(roll,pitch,heading); + } + typedef void Response; }; void insert(Serializer& serializer, const SetInitialAttitude& self); void extract(Serializer& serializer, SetInitialAttitude& self); CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -256,15 +273,6 @@ CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, struct EstimationControl { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - struct EnableFlags : Bitfield { enum _enumType : uint16_t @@ -311,13 +319,46 @@ struct EstimationControl FunctionSelector function = static_cast(0); EnableFlags enable; ///< See above + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(enable); + } + + + static EstimationControl create_sld_all(::mip::FunctionSelector function) + { + EstimationControl cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; EnableFlags enable; ///< See above + + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } + }; }; void insert(Serializer& serializer, const EstimationControl& self); @@ -331,6 +372,7 @@ CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::Ena CmdResult saveEstimationControl(C::mip_interface& device); CmdResult loadEstimationControl(C::mip_interface& device); CmdResult defaultEstimationControl(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -345,25 +387,33 @@ CmdResult defaultEstimationControl(C::mip_interface& device); struct ExternalGnssUpdate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; - - static const bool HAS_FUNCTION_SELECTOR = false; - double gps_time = 0; ///< [seconds] uint16_t gps_week = 0; ///< [GPS week number, not modulus 1024] double latitude = 0; ///< [degrees] double longitude = 0; ///< [degrees] double height = 0; ///< Above WGS84 ellipsoid [meters] - float velocity[3] = {0}; ///< NED Frame [meters/second] - float pos_uncertainty[3] = {0}; ///< NED Frame, 1-sigma [meters] - float vel_uncertainty[3] = {0}; ///< NED Frame, 1-sigma [meters/second] + Vector3f velocity; ///< NED Frame [meters/second] + Vector3f pos_uncertainty; ///< NED Frame, 1-sigma [meters] + Vector3f vel_uncertainty; ///< NED Frame, 1-sigma [meters/second] + + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(gps_time,gps_week,latitude,longitude,height,velocity,pos_uncertainty,vel_uncertainty); + } + typedef void Response; }; void insert(Serializer& serializer, const ExternalGnssUpdate& self); void extract(Serializer& serializer, ExternalGnssUpdate& self); -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); +CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, Vector3f velocity, Vector3f posUncertainty, Vector3f velUncertainty); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -386,20 +436,28 @@ CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t struct ExternalHeadingUpdate { + float heading = 0; ///< Bounded by +-PI [radians] + float heading_uncertainty = 0; ///< 1-sigma [radians] + uint8_t type = 0; ///< 1 - True, 2 - Magnetic + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; - float heading = 0; ///< Bounded by +-PI [radians] - float heading_uncertainty = 0; ///< 1-sigma [radians] - uint8_t type = 0; ///< 1 - True, 2 - Magnetic + auto as_tuple() const + { + return std::make_tuple(heading,heading_uncertainty,type); + } + typedef void Response; }; void insert(Serializer& serializer, const ExternalHeadingUpdate& self); void extract(Serializer& serializer, ExternalHeadingUpdate& self); CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -426,22 +484,30 @@ CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float h struct ExternalHeadingUpdateWithTime { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; - - static const bool HAS_FUNCTION_SELECTOR = false; - double gps_time = 0; ///< [seconds] uint16_t gps_week = 0; ///< [GPS week number, not modulus 1024] float heading = 0; ///< Relative to true north, bounded by +-PI [radians] float heading_uncertainty = 0; ///< 1-sigma [radians] uint8_t type = 0; ///< 1 - True, 2 - Magnetic + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(gps_time,gps_week,heading,heading_uncertainty,type); + } + + typedef void Response; }; void insert(Serializer& serializer, const ExternalHeadingUpdateWithTime& self); void extract(Serializer& serializer, ExternalHeadingUpdateWithTime& self); CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -456,15 +522,6 @@ CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime struct TareOrientation { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - struct MipTareAxes : Bitfield { enum _enumType : uint8_t @@ -499,13 +556,46 @@ struct TareOrientation FunctionSelector function = static_cast(0); MipTareAxes axes; ///< Axes to tare + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(axes); + } + + + static TareOrientation create_sld_all(::mip::FunctionSelector function) + { + TareOrientation cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; MipTareAxes axes; ///< Axes to tare + + auto as_tuple() + { + return std::make_tuple(std::ref(axes)); + } + }; }; void insert(Serializer& serializer, const TareOrientation& self); @@ -519,6 +609,7 @@ CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTare CmdResult saveTareOrientation(C::mip_interface& device); CmdResult loadTareOrientation(C::mip_interface& device); CmdResult defaultTareOrientation(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -529,15 +620,6 @@ CmdResult defaultTareOrientation(C::mip_interface& device); struct VehicleDynamicsMode { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class DynamicsMode : uint8_t { PORTABLE = 1, ///< @@ -549,13 +631,46 @@ struct VehicleDynamicsMode FunctionSelector function = static_cast(0); DynamicsMode mode = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(mode); + } + + + static VehicleDynamicsMode create_sld_all(::mip::FunctionSelector function) + { + VehicleDynamicsMode cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; DynamicsMode mode = static_cast(0); + + auto as_tuple() + { + return std::make_tuple(std::ref(mode)); + } + }; }; void insert(Serializer& serializer, const VehicleDynamicsMode& self); @@ -569,6 +684,7 @@ CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode: CmdResult saveVehicleDynamicsMode(C::mip_interface& device); CmdResult loadVehicleDynamicsMode(C::mip_interface& device); CmdResult defaultVehicleDynamicsMode(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -601,29 +717,53 @@ CmdResult defaultVehicleDynamicsMode(C::mip_interface& device); struct SensorToVehicleRotationEuler { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(roll,pitch,yaw); + } + + + static SensorToVehicleRotationEuler create_sld_all(::mip::FunctionSelector function) + { + SensorToVehicleRotationEuler cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] + + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + } + }; }; void insert(Serializer& serializer, const SensorToVehicleRotationEuler& self); @@ -637,6 +777,7 @@ CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* roll CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device); CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device); CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -675,24 +816,48 @@ CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); struct SensorToVehicleRotationDcm { + FunctionSelector function = static_cast(0); + Matrix3f dcm; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(dcm); + } - FunctionSelector function = static_cast(0); - float dcm[9] = {0}; + + static SensorToVehicleRotationDcm create_sld_all(::mip::FunctionSelector function) + { + SensorToVehicleRotationDcm cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; - float dcm[9] = {0}; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Matrix3f dcm; + + + auto as_tuple() + { + return std::make_tuple(std::ref(dcm)); + } }; }; @@ -702,11 +867,12 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm& self); void insert(Serializer& serializer, const SensorToVehicleRotationDcm::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self); -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); +CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcm); +CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcmOut); CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device); CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device); CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -744,24 +910,48 @@ CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); struct SensorToVehicleRotationQuaternion { + FunctionSelector function = static_cast(0); + Quatf quat; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float quat[4] = {0}; + auto as_tuple() const + { + return std::make_tuple(quat); + } + + + static SensorToVehicleRotationQuaternion create_sld_all(::mip::FunctionSelector function) + { + SensorToVehicleRotationQuaternion cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; - float quat[4] = {0}; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Quatf quat; + + + auto as_tuple() + { + return std::make_tuple(std::ref(quat)); + } }; }; @@ -771,11 +961,12 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion& self); void insert(Serializer& serializer, const SensorToVehicleRotationQuaternion::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response& self); -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); +CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quat); +CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quatOut); CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -794,24 +985,48 @@ CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); struct SensorToVehicleOffset { + FunctionSelector function = static_cast(0); + Vector3f offset; ///< [meters] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float offset[3] = {0}; ///< [meters] + auto as_tuple() const + { + return std::make_tuple(offset); + } + + + static SensorToVehicleOffset create_sld_all(::mip::FunctionSelector function) + { + SensorToVehicleOffset cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; - float offset[3] = {0}; ///< [meters] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f offset; ///< [meters] + + + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } }; }; @@ -821,11 +1036,12 @@ void extract(Serializer& serializer, SensorToVehicleOffset& self); void insert(Serializer& serializer, const SensorToVehicleOffset::Response& self); void extract(Serializer& serializer, SensorToVehicleOffset::Response& self); -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); -CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); +CmdResult writeSensorToVehicleOffset(C::mip_interface& device, Vector3f offset); +CmdResult readSensorToVehicleOffset(C::mip_interface& device, Vector3f offsetOut); CmdResult saveSensorToVehicleOffset(C::mip_interface& device); CmdResult loadSensorToVehicleOffset(C::mip_interface& device); CmdResult defaultSensorToVehicleOffset(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -841,24 +1057,48 @@ CmdResult defaultSensorToVehicleOffset(C::mip_interface& device); struct AntennaOffset { + FunctionSelector function = static_cast(0); + Vector3f offset; ///< [meters] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float offset[3] = {0}; ///< [meters] + auto as_tuple() const + { + return std::make_tuple(offset); + } + + + static AntennaOffset create_sld_all(::mip::FunctionSelector function) + { + AntennaOffset cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; - float offset[3] = {0}; ///< [meters] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f offset; ///< [meters] + + + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } }; }; @@ -868,11 +1108,12 @@ void extract(Serializer& serializer, AntennaOffset& self); void insert(Serializer& serializer, const AntennaOffset::Response& self); void extract(Serializer& serializer, AntennaOffset::Response& self); -CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset); -CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut); +CmdResult writeAntennaOffset(C::mip_interface& device, Vector3f offset); +CmdResult readAntennaOffset(C::mip_interface& device, Vector3f offsetOut); CmdResult saveAntennaOffset(C::mip_interface& device); CmdResult loadAntennaOffset(C::mip_interface& device); CmdResult defaultAntennaOffset(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -887,15 +1128,6 @@ CmdResult defaultAntennaOffset(C::mip_interface& device); struct GnssSource { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Source : uint8_t { ALL_INT = 1, ///< All internal receivers @@ -907,13 +1139,46 @@ struct GnssSource FunctionSelector function = static_cast(0); Source source = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(source); + } + + + static GnssSource create_sld_all(::mip::FunctionSelector function) + { + GnssSource cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); + + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } + }; }; void insert(Serializer& serializer, const GnssSource& self); @@ -927,6 +1192,7 @@ CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut CmdResult saveGnssSource(C::mip_interface& device); CmdResult loadGnssSource(C::mip_interface& device); CmdResult defaultGnssSource(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -948,15 +1214,6 @@ CmdResult defaultGnssSource(C::mip_interface& device); struct HeadingSource { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Source : uint8_t { NONE = 0, ///< See note 3 @@ -972,13 +1229,46 @@ struct HeadingSource FunctionSelector function = static_cast(0); Source source = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(source); + } + + + static HeadingSource create_sld_all(::mip::FunctionSelector function) + { + HeadingSource cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); + + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } + }; }; void insert(Serializer& serializer, const HeadingSource& self); @@ -992,6 +1282,7 @@ CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sou CmdResult saveHeadingSource(C::mip_interface& device); CmdResult loadHeadingSource(C::mip_interface& device); CmdResult defaultHeadingSource(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1010,25 +1301,49 @@ CmdResult defaultHeadingSource(C::mip_interface& device); struct AutoInitControl { + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< See above + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(enable); + } - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< See above + + static AutoInitControl create_sld_all(::mip::FunctionSelector function) + { + AutoInitControl cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< See above + + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } + }; }; void insert(Serializer& serializer, const AutoInitControl& self); @@ -1042,6 +1357,7 @@ CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut); CmdResult saveAutoInitControl(C::mip_interface& device); CmdResult loadAutoInitControl(C::mip_interface& device); CmdResult defaultAutoInitControl(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1050,37 +1366,56 @@ CmdResult defaultAutoInitControl(C::mip_interface& device); /// /// Each of the noise values must be greater than 0.0. /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ struct AccelNoise { + FunctionSelector function = static_cast(0); + Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + auto as_tuple() const + { + return std::make_tuple(noise); + } + + + static AccelNoise create_sld_all(::mip::FunctionSelector function) + { + AccelNoise cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] + + + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } }; }; @@ -1090,11 +1425,12 @@ void extract(Serializer& serializer, AccelNoise& self); void insert(Serializer& serializer, const AccelNoise::Response& self); void extract(Serializer& serializer, AccelNoise::Response& self); -CmdResult writeAccelNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readAccelNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult writeAccelNoise(C::mip_interface& device, Vector3f noise); +CmdResult readAccelNoise(C::mip_interface& device, Vector3f noiseOut); CmdResult saveAccelNoise(C::mip_interface& device); CmdResult loadAccelNoise(C::mip_interface& device); CmdResult defaultAccelNoise(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1103,37 +1439,56 @@ CmdResult defaultAccelNoise(C::mip_interface& device); /// /// Each of the noise values must be greater than 0.0 /// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. +/// The noise value represents process noise in the Estimation Filter. /// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. /// Default values provide good performance for most laboratory conditions. -/// /// ///@{ struct GyroNoise { + FunctionSelector function = static_cast(0); + Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + auto as_tuple() const + { + return std::make_tuple(noise); + } + + + static GyroNoise create_sld_all(::mip::FunctionSelector function) + { + GyroNoise cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] + + + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } }; }; @@ -1143,52 +1498,69 @@ void extract(Serializer& serializer, GyroNoise& self); void insert(Serializer& serializer, const GyroNoise::Response& self); void extract(Serializer& serializer, GyroNoise::Response& self); -CmdResult writeGyroNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readGyroNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); +CmdResult writeGyroNoise(C::mip_interface& device, Vector3f noise); +CmdResult readGyroNoise(C::mip_interface& device, Vector3f noiseOut); CmdResult saveGyroNoise(C::mip_interface& device); CmdResult loadGyroNoise(C::mip_interface& device); CmdResult defaultGyroNoise(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_accel_bias_model (0x0D,0x1C) Accel Bias Model [CPP] /// Accelerometer Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ struct AccelBiasModel { + FunctionSelector function = static_cast(0); + Vector3f beta; ///< Accel Bias Beta [1/second] + Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float x_beta = 0; ///< Accel Bias Beta [1/second] - float y_beta = 0; ///< Accel Bias Beta [1/second] - float z_beta = 0; ///< Accel Bias Beta [1/second] - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + auto as_tuple() const + { + return std::make_tuple(beta,noise); + } + + + static AccelBiasModel create_sld_all(::mip::FunctionSelector function) + { + AccelBiasModel cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; - float x_beta = 0; ///< Accel Bias Beta [1/second] - float y_beta = 0; ///< Accel Bias Beta [1/second] - float z_beta = 0; ///< Accel Bias Beta [1/second] - float x = 0; ///< Accel Noise 1-sigma [meters/second^2] - float y = 0; ///< Accel Noise 1-sigma [meters/second^2] - float z = 0; ///< Accel Noise 1-sigma [meters/second^2] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f beta; ///< Accel Bias Beta [1/second] + Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] + + + auto as_tuple() + { + return std::make_tuple(std::ref(beta),std::ref(noise)); + } }; }; @@ -1198,105 +1570,146 @@ void extract(Serializer& serializer, AccelBiasModel& self); void insert(Serializer& serializer, const AccelBiasModel::Response& self); void extract(Serializer& serializer, AccelBiasModel::Response& self); -CmdResult writeAccelBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); -CmdResult readAccelBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult writeAccelBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise); +CmdResult readAccelBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut); CmdResult saveAccelBiasModel(C::mip_interface& device); CmdResult loadAccelBiasModel(C::mip_interface& device); CmdResult defaultAccelBiasModel(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_gyro_bias_model (0x0D,0x1D) Gyro Bias Model [CPP] /// Gyroscope Bias Model Parameters /// -/// Each of the noise values must be greater than 0.0 +/// Noise values must be greater than 0.0 /// /// ///@{ struct GyroBiasModel { + FunctionSelector function = static_cast(0); + Vector3f beta; ///< Gyro Bias Beta [1/second] + Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(beta,noise); + } - FunctionSelector function = static_cast(0); - float x_beta = 0; ///< Gyro Bias Beta [1/second] - float y_beta = 0; ///< Gyro Bias Beta [1/second] - float z_beta = 0; ///< Gyro Bias Beta [1/second] - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + + static GyroBiasModel create_sld_all(::mip::FunctionSelector function) + { + GyroBiasModel cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; - float x_beta = 0; ///< Gyro Bias Beta [1/second] - float y_beta = 0; ///< Gyro Bias Beta [1/second] - float z_beta = 0; ///< Gyro Bias Beta [1/second] - float x = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float y = 0; ///< Gyro Noise 1-sigma [meters/second^2] - float z = 0; ///< Gyro Noise 1-sigma [meters/second^2] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f beta; ///< Gyro Bias Beta [1/second] + Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] - }; -}; + + auto as_tuple() + { + return std::make_tuple(std::ref(beta),std::ref(noise)); + } + + }; +}; void insert(Serializer& serializer, const GyroBiasModel& self); void extract(Serializer& serializer, GyroBiasModel& self); void insert(Serializer& serializer, const GyroBiasModel::Response& self); void extract(Serializer& serializer, GyroBiasModel::Response& self); -CmdResult writeGyroBiasModel(C::mip_interface& device, float xBeta, float yBeta, float zBeta, float x, float y, float z); -CmdResult readGyroBiasModel(C::mip_interface& device, float* xBetaOut, float* yBetaOut, float* zBetaOut, float* xOut, float* yOut, float* zOut); +CmdResult writeGyroBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise); +CmdResult readGyroBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut); CmdResult saveGyroBiasModel(C::mip_interface& device); CmdResult loadGyroBiasModel(C::mip_interface& device); CmdResult defaultGyroBiasModel(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_altitude_aiding (0x0D,0x47) Altitude Aiding [CPP] -/// Altitude Aiding Control -/// /// Select altitude input for absolute altitude and/or vertical velocity. The primary altitude reading is always GNSS. -/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during GNSS outages. -/// -/// Possible altitude aiding selector values: +/// Aiding inputs are used to improve GNSS altitude readings when GNSS is available and to backup GNSS during outages. /// -/// 0x00 - No altitude aiding (disable) -/// 0x01 - Enable pressure sensor aiding(1) -/// -/// 1. Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. +/// Pressure altitude is based on "instant sea level pressure" which is dependent on location and weather conditions and can vary by more than 40 meters. /// /// ///@{ struct AltitudeAiding { + enum class AidingSelector : uint8_t + { + NONE = 0, ///< No altitude aiding + PRESURE = 1, ///< Enable pressure sensor aiding + }; + + FunctionSelector function = static_cast(0); + AidingSelector selector = static_cast(0); ///< See above + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t aiding_selector = 0; ///< See above + auto as_tuple() const + { + return std::make_tuple(selector); + } + + + static AltitudeAiding create_sld_all(::mip::FunctionSelector function) + { + AltitudeAiding cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; - uint8_t aiding_selector = 0; ///< See above + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + AidingSelector selector = static_cast(0); ///< See above + + + auto as_tuple() + { + return std::make_tuple(std::ref(selector)); + } }; }; @@ -1306,16 +1719,90 @@ void extract(Serializer& serializer, AltitudeAiding& self); void insert(Serializer& serializer, const AltitudeAiding::Response& self); void extract(Serializer& serializer, AltitudeAiding::Response& self); -CmdResult writeAltitudeAiding(C::mip_interface& device, uint8_t aidingSelector); -CmdResult readAltitudeAiding(C::mip_interface& device, uint8_t* aidingSelectorOut); +CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); +CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); CmdResult saveAltitudeAiding(C::mip_interface& device); CmdResult loadAltitudeAiding(C::mip_interface& device); CmdResult defaultAltitudeAiding(C::mip_interface& device); + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_pitch_roll_aiding (0x0D,0x4B) Pitch Roll Aiding [CPP] +/// Select pitch/roll aiding input. Pitch/roll reading is always derived from GNSS corrected inertial solution. +/// Aiding inputs are used to improve that solution during periods of low dynamics and GNSS outages. +/// +///@{ + +struct PitchRollAiding +{ + enum class AidingSource : uint8_t + { + NONE = 0, ///< No pitch/roll aiding + GRAVITY_VEC = 1, ///< Enable gravity vector aiding + }; + + FunctionSelector function = static_cast(0); + AidingSource source = static_cast(0); ///< Controls the aiding source + + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(source); + } + + + static PitchRollAiding create_sld_all(::mip::FunctionSelector function) + { + PitchRollAiding cmd; + cmd.function = function; + return cmd; + } + + struct Response + { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + AidingSource source = static_cast(0); ///< Controls the aiding source + + + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } + + }; +}; +void insert(Serializer& serializer, const PitchRollAiding& self); +void extract(Serializer& serializer, PitchRollAiding& self); + +void insert(Serializer& serializer, const PitchRollAiding::Response& self); +void extract(Serializer& serializer, PitchRollAiding::Response& self); + +CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); +CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); +CmdResult savePitchRollAiding(C::mip_interface& device); +CmdResult loadPitchRollAiding(C::mip_interface& device); +CmdResult defaultPitchRollAiding(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_auto_zupt (0x0D,0x1E) Auto Zupt [CPP] -/// Zero Velocity Update /// The ZUPT is triggered when the scalar magnitude of the GNSS reported velocity vector is equal-to or less than the threshold value. /// The device will NACK threshold values that are less than zero (i.e.negative.) /// @@ -1323,27 +1810,51 @@ CmdResult defaultAltitudeAiding(C::mip_interface& device); struct AutoZupt { + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + float threshold = 0; ///< [meters/second] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< 0 - Disable, 1 - Enable - float threshold = 0; ///< [meters/second] + auto as_tuple() const + { + return std::make_tuple(enable,threshold); + } + + + static AutoZupt create_sld_all(::mip::FunctionSelector function) + { + AutoZupt cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [meters/second] + + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(threshold)); + } + }; }; void insert(Serializer& serializer, const AutoZupt& self); @@ -1357,6 +1868,7 @@ CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thre CmdResult saveAutoZupt(C::mip_interface& device); CmdResult loadAutoZupt(C::mip_interface& device); CmdResult defaultAutoZupt(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1369,27 +1881,51 @@ CmdResult defaultAutoZupt(C::mip_interface& device); struct AutoAngularZupt { + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + float threshold = 0; ///< [radians/second] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< 0 - Disable, 1 - Enable - float threshold = 0; ///< [radians/second] + auto as_tuple() const + { + return std::make_tuple(enable,threshold); + } + + + static AutoAngularZupt create_sld_all(::mip::FunctionSelector function) + { + AutoAngularZupt cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [radians/second] + + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(threshold)); + } + }; }; void insert(Serializer& serializer, const AutoAngularZupt& self); @@ -1403,50 +1939,65 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa CmdResult saveAutoAngularZupt(C::mip_interface& device); CmdResult loadAutoAngularZupt(C::mip_interface& device); CmdResult defaultAutoAngularZupt(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_commanded_zupt (0x0D,0x22) Commanded Zupt [CPP] -/// Commanded Zero Velocity Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ struct CommandedZupt { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const CommandedZupt& self); void extract(Serializer& serializer, CommandedZupt& self); CmdResult commandedZupt(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_filter_commanded_angular_zupt (0x0D,0x23) Commanded Angular Zupt [CPP] -/// Commanded Zero Angular Rate Update /// Please see the device user manual for the maximum rate of this message. /// ///@{ struct CommandedAngularZupt { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const CommandedAngularZupt& self); void extract(Serializer& serializer, CommandedAngularZupt& self); CmdResult commandedAngularZupt(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1459,23 +2010,40 @@ CmdResult commandedAngularZupt(C::mip_interface& device); struct MagCaptureAutoCal { + FunctionSelector function = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = false; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = false; - static const bool HAS_RESET_FUNCTION = false; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8000; + static const uint32_t READ_PARAMS = 0x0000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x0000; + static const uint32_t DEFAULT_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); + auto as_tuple() const + { + return std::make_tuple(); + } + + + static MagCaptureAutoCal create_sld_all(::mip::FunctionSelector function) + { + MagCaptureAutoCal cmd; + cmd.function = function; + return cmd; + } + typedef void Response; }; void insert(Serializer& serializer, const MagCaptureAutoCal& self); void extract(Serializer& serializer, MagCaptureAutoCal& self); CmdResult writeMagCaptureAutoCal(C::mip_interface& device); CmdResult saveMagCaptureAutoCal(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1491,24 +2059,48 @@ CmdResult saveMagCaptureAutoCal(C::mip_interface& device); struct GravityNoise { + FunctionSelector function = static_cast(0); + Vector3f noise; ///< Gravity Noise 1-sigma [gauss] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float noise[3] = {0}; ///< Gravity Noise 1-sigma [gauss] + auto as_tuple() const + { + return std::make_tuple(noise); + } + + + static GravityNoise create_sld_all(::mip::FunctionSelector function) + { + GravityNoise cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; - float noise[3] = {0}; ///< Gravity Noise 1-sigma [gauss] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f noise; ///< Gravity Noise 1-sigma [gauss] + + + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } }; }; @@ -1518,11 +2110,12 @@ void extract(Serializer& serializer, GravityNoise& self); void insert(Serializer& serializer, const GravityNoise::Response& self); void extract(Serializer& serializer, GravityNoise::Response& self); -CmdResult writeGravityNoise(C::mip_interface& device, const float* noise); -CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut); +CmdResult writeGravityNoise(C::mip_interface& device, Vector3f noise); +CmdResult readGravityNoise(C::mip_interface& device, Vector3f noiseOut); CmdResult saveGravityNoise(C::mip_interface& device); CmdResult loadGravityNoise(C::mip_interface& device); CmdResult defaultGravityNoise(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1538,25 +2131,49 @@ CmdResult defaultGravityNoise(C::mip_interface& device); struct PressureAltitudeNoise { + FunctionSelector function = static_cast(0); + float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(noise); + } - FunctionSelector function = static_cast(0); - float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] + + static PressureAltitudeNoise create_sld_all(::mip::FunctionSelector function) + { + PressureAltitudeNoise cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] + + auto as_tuple() const + { + return std::make_tuple(noise); + } + }; }; void insert(Serializer& serializer, const PressureAltitudeNoise& self); @@ -1570,6 +2187,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); CmdResult savePressureAltitudeNoise(C::mip_interface& device); CmdResult loadPressureAltitudeNoise(C::mip_interface& device); CmdResult defaultPressureAltitudeNoise(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1587,24 +2205,48 @@ CmdResult defaultPressureAltitudeNoise(C::mip_interface& device); struct HardIronOffsetNoise { + FunctionSelector function = static_cast(0); + Vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float noise[3] = {0}; ///< Hard Iron Offset Noise 1-sigma [gauss] + auto as_tuple() const + { + return std::make_tuple(noise); + } + + + static HardIronOffsetNoise create_sld_all(::mip::FunctionSelector function) + { + HardIronOffsetNoise cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; - float noise[3] = {0}; ///< Hard Iron Offset Noise 1-sigma [gauss] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] + + + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } }; }; @@ -1614,11 +2256,12 @@ void extract(Serializer& serializer, HardIronOffsetNoise& self); void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, Vector3f noise); +CmdResult readHardIronOffsetNoise(C::mip_interface& device, Vector3f noiseOut); CmdResult saveHardIronOffsetNoise(C::mip_interface& device); CmdResult loadHardIronOffsetNoise(C::mip_interface& device); CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1635,24 +2278,48 @@ CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); struct SoftIronMatrixNoise { + FunctionSelector function = static_cast(0); + Matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float noise[9] = {0}; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + auto as_tuple() const + { + return std::make_tuple(noise); + } + + + static SoftIronMatrixNoise create_sld_all(::mip::FunctionSelector function) + { + SoftIronMatrixNoise cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; - float noise[9] = {0}; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] + + + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } }; }; @@ -1662,11 +2329,12 @@ void extract(Serializer& serializer, SoftIronMatrixNoise& self); void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self); void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self); -CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); -CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noise); +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noiseOut); CmdResult saveSoftIronMatrixNoise(C::mip_interface& device); CmdResult loadSoftIronMatrixNoise(C::mip_interface& device); CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1683,24 +2351,48 @@ CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); struct MagNoise { + FunctionSelector function = static_cast(0); + Vector3f noise; ///< Mag Noise 1-sigma [gauss] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - float noise[3] = {0}; ///< Mag Noise 1-sigma [gauss] + auto as_tuple() const + { + return std::make_tuple(noise); + } + + + static MagNoise create_sld_all(::mip::FunctionSelector function) + { + MagNoise cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; - float noise[3] = {0}; ///< Mag Noise 1-sigma [gauss] + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + Vector3f noise; ///< Mag Noise 1-sigma [gauss] + + + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } }; }; @@ -1710,11 +2402,12 @@ void extract(Serializer& serializer, MagNoise& self); void insert(Serializer& serializer, const MagNoise::Response& self); void extract(Serializer& serializer, MagNoise::Response& self); -CmdResult writeMagNoise(C::mip_interface& device, const float* noise); -CmdResult readMagNoise(C::mip_interface& device, float* noiseOut); +CmdResult writeMagNoise(C::mip_interface& device, Vector3f noise); +CmdResult readMagNoise(C::mip_interface& device, Vector3f noiseOut); CmdResult saveMagNoise(C::mip_interface& device); CmdResult loadMagNoise(C::mip_interface& device); CmdResult defaultMagNoise(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1729,27 +2422,51 @@ CmdResult defaultMagNoise(C::mip_interface& device); struct InclinationSource { + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Inclination Source + float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(source,inclination); + } - FunctionSelector function = static_cast(0); - FilterMagParamSource source = static_cast(0); ///< Inclination Source - float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) + + static InclinationSource create_sld_all(::mip::FunctionSelector function) + { + InclinationSource cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Inclination Source float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) + + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(inclination)); + } + }; }; void insert(Serializer& serializer, const InclinationSource& self); @@ -1763,6 +2480,7 @@ CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* CmdResult saveInclinationSource(C::mip_interface& device); CmdResult loadInclinationSource(C::mip_interface& device); CmdResult defaultInclinationSource(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1777,27 +2495,51 @@ CmdResult defaultInclinationSource(C::mip_interface& device); struct MagneticDeclinationSource { + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source + float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source - float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) + auto as_tuple() const + { + return std::make_tuple(source,declination); + } + + + static MagneticDeclinationSource create_sld_all(::mip::FunctionSelector function) + { + MagneticDeclinationSource cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) + + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(declination)); + } + }; }; void insert(Serializer& serializer, const MagneticDeclinationSource& self); @@ -1811,6 +2553,7 @@ CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParam CmdResult saveMagneticDeclinationSource(C::mip_interface& device); CmdResult loadMagneticDeclinationSource(C::mip_interface& device); CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1824,27 +2567,51 @@ CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); struct MagFieldMagnitudeSource { + FunctionSelector function = static_cast(0); + FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source + float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source - float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) + auto as_tuple() const + { + return std::make_tuple(source,magnitude); + } + + + static MagFieldMagnitudeSource create_sld_all(::mip::FunctionSelector function) + { + MagFieldMagnitudeSource cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) + + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(magnitude)); + } + }; }; void insert(Serializer& serializer, const MagFieldMagnitudeSource& self); @@ -1858,6 +2625,7 @@ CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSo CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device); CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device); CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1871,31 +2639,55 @@ CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device); struct ReferencePosition { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); bool enable = 0; ///< enable/disable double latitude = 0; ///< [degrees] double longitude = 0; ///< [degrees] double altitude = 0; ///< [meters] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x800F; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(enable,latitude,longitude,altitude); + } + + + static ReferencePosition create_sld_all(::mip::FunctionSelector function) + { + ReferencePosition cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< enable/disable double latitude = 0; ///< [degrees] double longitude = 0; ///< [degrees] double altitude = 0; ///< [meters] + + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(latitude),std::ref(longitude),std::ref(altitude)); + } + }; }; void insert(Serializer& serializer, const ReferencePosition& self); @@ -1909,6 +2701,7 @@ CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, doubl CmdResult saveReferencePosition(C::mip_interface& device); CmdResult loadReferencePosition(C::mip_interface& device); CmdResult defaultReferencePosition(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1932,15 +2725,6 @@ CmdResult defaultReferencePosition(C::mip_interface& device); struct AccelMagnitudeErrorAdaptiveMeasurement { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] @@ -1950,11 +2734,38 @@ struct AccelMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x807F; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(adaptive_measurement,frequency,low_limit,high_limit,low_limit_uncertainty,high_limit_uncertainty,minimum_uncertainty); + } + + + static AccelMagnitudeErrorAdaptiveMeasurement create_sld_all(::mip::FunctionSelector function) + { + AccelMagnitudeErrorAdaptiveMeasurement cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float low_limit = 0; ///< [meters/second^2] @@ -1963,6 +2774,12 @@ struct AccelMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + auto as_tuple() + { + return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + } + }; }; void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement& self); @@ -1976,6 +2793,7 @@ CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, F CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1994,15 +2812,6 @@ CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device struct MagMagnitudeErrorAdaptiveMeasurement { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] @@ -2012,11 +2821,38 @@ struct MagMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x807F; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(adaptive_measurement,frequency,low_limit,high_limit,low_limit_uncertainty,high_limit_uncertainty,minimum_uncertainty); + } + + + static MagMagnitudeErrorAdaptiveMeasurement create_sld_all(::mip::FunctionSelector function) + { + MagMagnitudeErrorAdaptiveMeasurement cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float low_limit = 0; ///< [meters/second^2] @@ -2025,6 +2861,12 @@ struct MagMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + auto as_tuple() + { + return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + } + }; }; void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement& self); @@ -2038,6 +2880,7 @@ CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, Fil CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2058,15 +2901,6 @@ CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); struct MagDipAngleErrorAdaptiveMeasurement { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); bool enable = 0; ///< Enable/Disable float frequency = 0; ///< Low-pass filter curoff frequency [hertz] @@ -2074,17 +2908,50 @@ struct MagDipAngleErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] - struct Response + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x801F; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(enable,frequency,high_limit,high_limit_uncertainty,minimum_uncertainty); + } + + + static MagDipAngleErrorAdaptiveMeasurement create_sld_all(::mip::FunctionSelector function) + { + MagDipAngleErrorAdaptiveMeasurement cmd; + cmd.function = function; + return cmd; + } + + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< Enable/Disable float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float high_limit = 0; ///< [meters/second^2] float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] + + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(frequency),std::ref(high_limit),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + } + }; }; void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement& self); @@ -2098,6 +2965,7 @@ CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2110,15 +2978,6 @@ CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); struct AidingMeasurementEnable { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class AidingSource : uint16_t { GNSS_POS_VEL = 0, ///< GNSS Position and Velocity @@ -2134,14 +2993,48 @@ struct AidingMeasurementEnable AidingSource aiding_source = static_cast(0); ///< Aiding measurement source bool enable = 0; ///< Controls the aiding source + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(aiding_source,enable); + } + + + static AidingMeasurementEnable create_sld_all(::mip::FunctionSelector function) + { + AidingMeasurementEnable cmd; + cmd.function = function; + cmd.aiding_source = ::mip::commands_filter::AidingMeasurementEnable::AidingSource::ALL; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; AidingSource aiding_source = static_cast(0); ///< Aiding measurement source bool enable = 0; ///< Controls the aiding source + + auto as_tuple() + { + return std::make_tuple(std::ref(aiding_source),std::ref(enable)); + } + }; }; void insert(Serializer& serializer, const AidingMeasurementEnable& self); @@ -2155,6 +3048,7 @@ CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2167,17 +3061,25 @@ CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasure struct Run { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const Run& self); void extract(Serializer& serializer, Run& self); CmdResult run(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2190,29 +3092,53 @@ CmdResult run(C::mip_interface& device); struct KinematicConstraint { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); uint8_t acceleration_constraint_selection = 0; ///< Acceleration constraint:
0=None (default),
1=Zero-acceleration. uint8_t velocity_constraint_selection = 0; ///< 0=None (default),
1=Zero-velocity,
2=Wheeled-vehicle.
uint8_t angular_constraint_selection = 0; ///< 0=None (default), 1=Zero-angular rate (ZUPT). + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(acceleration_constraint_selection,velocity_constraint_selection,angular_constraint_selection); + } + + + static KinematicConstraint create_sld_all(::mip::FunctionSelector function) + { + KinematicConstraint cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t acceleration_constraint_selection = 0; ///< Acceleration constraint:
0=None (default),
1=Zero-acceleration. uint8_t velocity_constraint_selection = 0; ///< 0=None (default),
1=Zero-velocity,
2=Wheeled-vehicle.
uint8_t angular_constraint_selection = 0; ///< 0=None (default), 1=Zero-angular rate (ZUPT). + + auto as_tuple() + { + return std::make_tuple(std::ref(acceleration_constraint_selection),std::ref(velocity_constraint_selection),std::ref(angular_constraint_selection)); + } + }; }; void insert(Serializer& serializer, const KinematicConstraint& self); @@ -2226,6 +3152,7 @@ CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* acceleratio CmdResult saveKinematicConstraint(C::mip_interface& device); CmdResult loadKinematicConstraint(C::mip_interface& device); CmdResult defaultKinematicConstraint(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2240,15 +3167,6 @@ CmdResult defaultKinematicConstraint(C::mip_interface& device); struct InitializationConfiguration { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - struct AlignmentSelector : Bitfield { enum _enumType : uint8_t @@ -2256,8 +3174,9 @@ struct InitializationConfiguration NONE = 0x00, DUAL_ANTENNA = 0x01, ///< Dual-antenna GNSS alignment KINEMATIC = 0x02, ///< GNSS kinematic alignment (GNSS velocity determines initial heading) - MAGNETOMETER = 0x04, ///< Magnetometer heading alignment - ALL = 0x07, + MAGNETOMETER = 0x04, ///< Magnetometer heading alignment (Internal magnetometer determines initial heading) + EXTERNAL = 0x08, ///< External heading alignment (External heading input determines heading) + ALL = 0x0F, }; uint8_t value = NONE; @@ -2275,6 +3194,8 @@ struct InitializationConfiguration void kinematic(bool val) { if(val) value |= KINEMATIC; else value &= ~KINEMATIC; } bool magnetometer() const { return (value & MAGNETOMETER) > 0; } void magnetometer(bool val) { if(val) value |= MAGNETOMETER; else value &= ~MAGNETOMETER; } + bool external() const { return (value & EXTERNAL) > 0; } + void external(bool val) { if(val) value |= EXTERNAL; else value &= ~EXTERNAL; } bool allSet() const { return value == ALL; } void setAll() { value |= ALL; } @@ -2295,25 +3216,58 @@ struct InitializationConfiguration float initial_heading = 0; ///< User-specified initial platform heading (degrees). float initial_pitch = 0; ///< User-specified initial platform pitch (degrees) float initial_roll = 0; ///< User-specified initial platform roll (degrees) - float initial_position[3] = {0}; ///< User-specified initial platform position (units determined by reference frame selector, see note.) - float initial_velocity[3] = {0}; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) + Vector3f initial_position; ///< User-specified initial platform position (units determined by reference frame selector, see note.) + Vector3f initial_velocity; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) FilterReferenceFrame reference_frame_selector = static_cast(0); ///< User-specified initial position/velocity reference frames + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x81FF; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(wait_for_run_command,initial_cond_src,auto_heading_alignment_selector,initial_heading,initial_pitch,initial_roll,initial_position,initial_velocity,reference_frame_selector); + } + + + static InitializationConfiguration create_sld_all(::mip::FunctionSelector function) + { + InitializationConfiguration cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t wait_for_run_command = 0; ///< Initialize filter only after receiving "run" command InitialConditionSource initial_cond_src = static_cast(0); ///< Initial condition source: AlignmentSelector auto_heading_alignment_selector; ///< Bitfield specifying the allowed automatic heading alignment methods for automatic initial conditions. Bits are set to 1 to enable, and the correspond to the following:
float initial_heading = 0; ///< User-specified initial platform heading (degrees). float initial_pitch = 0; ///< User-specified initial platform pitch (degrees) float initial_roll = 0; ///< User-specified initial platform roll (degrees) - float initial_position[3] = {0}; ///< User-specified initial platform position (units determined by reference frame selector, see note.) - float initial_velocity[3] = {0}; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) + Vector3f initial_position; ///< User-specified initial platform position (units determined by reference frame selector, see note.) + Vector3f initial_velocity; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) FilterReferenceFrame reference_frame_selector = static_cast(0); ///< User-specified initial position/velocity reference frames + + auto as_tuple() + { + return std::make_tuple(std::ref(wait_for_run_command),std::ref(initial_cond_src),std::ref(auto_heading_alignment_selector),std::ref(initial_heading),std::ref(initial_pitch),std::ref(initial_roll),std::ref(initial_position),std::ref(initial_velocity),std::ref(reference_frame_selector)); + } + }; }; void insert(Serializer& serializer, const InitializationConfiguration& self); @@ -2322,11 +3276,12 @@ void extract(Serializer& serializer, InitializationConfiguration& self); void insert(Serializer& serializer, const InitializationConfiguration::Response& self); void extract(Serializer& serializer, InitializationConfiguration::Response& self); -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); +CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, Vector3f initialPosition, Vector3f initialVelocity, FilterReferenceFrame referenceFrameSelector); +CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, Vector3f initialPositionOut, Vector3f initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); CmdResult saveInitializationConfiguration(C::mip_interface& device); CmdResult loadInitializationConfiguration(C::mip_interface& device); CmdResult defaultInitializationConfiguration(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2337,27 +3292,51 @@ CmdResult defaultInitializationConfiguration(C::mip_interface& device); struct AdaptiveFilterOptions { + FunctionSelector function = static_cast(0); + uint8_t level = 0; ///< Auto-adaptive operating level:
0=Off,
1=Conservative,
2=Moderate (default),
3=Aggressive. + uint16_t time_limit = 0; ///< Maximum duration of measurement rejection before entering recovery mode (ms) + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t level = 0; ///< Auto-adaptive operating level:
0=Off,
1=Conservative,
2=Moderate (default),
3=Aggressive. - uint16_t time_limit = 0; ///< Maximum duration of measurement rejection before entering recovery mode (ms) + auto as_tuple() const + { + return std::make_tuple(level,time_limit); + } + + + static AdaptiveFilterOptions create_sld_all(::mip::FunctionSelector function) + { + AdaptiveFilterOptions cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t level = 0; ///< Auto-adaptive operating level:
0=Off,
1=Conservative,
2=Moderate (default),
3=Aggressive. uint16_t time_limit = 0; ///< Maximum duration of measurement rejection before entering recovery mode (ms) + + auto as_tuple() + { + return std::make_tuple(std::ref(level),std::ref(time_limit)); + } + }; }; void insert(Serializer& serializer, const AdaptiveFilterOptions& self); @@ -2371,6 +3350,7 @@ CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, CmdResult saveAdaptiveFilterOptions(C::mip_interface& device); CmdResult loadAdaptiveFilterOptions(C::mip_interface& device); CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2384,26 +3364,51 @@ CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device); struct MultiAntennaOffset { + FunctionSelector function = static_cast(0); + uint8_t receiver_id = 0; ///< Receiver: 1, 2, etc... + Vector3f antenna_offset; ///< Antenna lever arm offset vector in the vehicle frame (m) + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t receiver_id = 0; ///< Receiver: 1, 2, etc... - float antenna_offset[3] = {0}; ///< Antenna lever arm offset vector in the vehicle frame (m) + auto as_tuple() const + { + return std::make_tuple(receiver_id,antenna_offset); + } + + + static MultiAntennaOffset create_sld_all(::mip::FunctionSelector function) + { + MultiAntennaOffset cmd; + cmd.function = function; + cmd.receiver_id = 0; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t receiver_id = 0; - float antenna_offset[3] = {0}; + Vector3f antenna_offset; + + + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_id),std::ref(antenna_offset)); + } }; }; @@ -2413,11 +3418,12 @@ void extract(Serializer& serializer, MultiAntennaOffset& self); void insert(Serializer& serializer, const MultiAntennaOffset::Response& self); void extract(Serializer& serializer, MultiAntennaOffset::Response& self); -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); +CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffset); +CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffsetOut); CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2428,28 +3434,52 @@ CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId struct RelPosConfiguration { + FunctionSelector function = static_cast(0); + uint8_t source = 0; ///< 0 - auto (RTK base station), 1 - manual + FilterReferenceFrame reference_frame_selector = static_cast(0); ///< ECEF or LLH + Vector3d reference_coordinates; ///< reference coordinates, units determined by source selection + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8007; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t source = 0; ///< 0 - auto (RTK base station), 1 - manual - FilterReferenceFrame reference_frame_selector = static_cast(0); ///< ECEF or LLH - double reference_coordinates[3] = {0}; ///< reference coordinates, units determined by source selection + auto as_tuple() const + { + return std::make_tuple(source,reference_frame_selector,reference_coordinates); + } + + + static RelPosConfiguration create_sld_all(::mip::FunctionSelector function) + { + RelPosConfiguration cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t source = 0; ///< 0 - auto (RTK base station), 1 - manual FilterReferenceFrame reference_frame_selector = static_cast(0); ///< ECEF or LLH - double reference_coordinates[3] = {0}; ///< reference coordinates, units determined by source selection + Vector3d reference_coordinates; ///< reference coordinates, units determined by source selection + + + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(reference_frame_selector),std::ref(reference_coordinates)); + } }; }; @@ -2459,11 +3489,12 @@ void extract(Serializer& serializer, RelPosConfiguration& self); void insert(Serializer& serializer, const RelPosConfiguration::Response& self); void extract(Serializer& serializer, RelPosConfiguration::Response& self); -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); +CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, Vector3d referenceCoordinates); +CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, Vector3d referenceCoordinatesOut); CmdResult saveRelPosConfiguration(C::mip_interface& device); CmdResult loadRelPosConfiguration(C::mip_interface& device); CmdResult defaultRelPosConfiguration(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2481,15 +3512,6 @@ CmdResult defaultRelPosConfiguration(C::mip_interface& device); struct RefPointLeverArm { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class ReferencePointSelector : uint8_t { VEH = 1, ///< Defines the origin of the vehicle @@ -2497,15 +3519,48 @@ struct RefPointLeverArm FunctionSelector function = static_cast(0); ReferencePointSelector ref_point_sel = static_cast(0); ///< Reserved, must be 1 - float lever_arm_offset[3] = {0}; ///< [m] Lever arm offset vector in the vehicle's reference frame. + Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. + + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(ref_point_sel,lever_arm_offset); + } + + + static RefPointLeverArm create_sld_all(::mip::FunctionSelector function) + { + RefPointLeverArm cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; ReferencePointSelector ref_point_sel = static_cast(0); ///< Reserved, must be 1 - float lever_arm_offset[3] = {0}; ///< [m] Lever arm offset vector in the vehicle's reference frame. + Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. + + + auto as_tuple() + { + return std::make_tuple(std::ref(ref_point_sel),std::ref(lever_arm_offset)); + } }; }; @@ -2515,11 +3570,12 @@ void extract(Serializer& serializer, RefPointLeverArm& self); void insert(Serializer& serializer, const RefPointLeverArm::Response& self); void extract(Serializer& serializer, RefPointLeverArm::Response& self); -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); +CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, Vector3f leverArmOffset); +CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, Vector3f leverArmOffsetOut); CmdResult saveRefPointLeverArm(C::mip_interface& device); CmdResult loadRefPointLeverArm(C::mip_interface& device); CmdResult defaultRefPointLeverArm(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2532,21 +3588,29 @@ CmdResult defaultRefPointLeverArm(C::mip_interface& device); struct SpeedMeasurement { + uint8_t source = 0; ///< Reserved, must be 1. + float time_of_week = 0; ///< GPS time of week when speed was sampled + float speed = 0; ///< Estimated speed along vehicle's x-axis (may be positive or negative) [meters/second] + float speed_uncertainty = 0; ///< Estimated uncertainty in the speed measurement (1-sigma value) [meters/second] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; - uint8_t source = 0; ///< Reserved, must be 1. - float time_of_week = 0; ///< GPS time of week when speed was sampled - float speed = 0; ///< Estimated speed along vehicle's x-axis (may be positive or negative) [meters/second] - float speed_uncertainty = 0; ///< Estimated uncertainty in the speed measurement (1-sigma value) [meters/second] + auto as_tuple() const + { + return std::make_tuple(source,time_of_week,speed,speed_uncertainty); + } + typedef void Response; }; void insert(Serializer& serializer, const SpeedMeasurement& self); void extract(Serializer& serializer, SpeedMeasurement& self); CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2563,26 +3627,51 @@ CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeO struct SpeedLeverArm { + FunctionSelector function = static_cast(0); + uint8_t source = 0; ///< Reserved, must be 1. + Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8001; + static const uint32_t SAVE_PARAMS = 0x8001; + static const uint32_t LOAD_PARAMS = 0x8001; + static const uint32_t DEFAULT_PARAMS = 0x8001; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t source = 0; ///< Reserved, must be 1. - float lever_arm_offset[3] = {0}; ///< [m] Lever arm offset vector in the vehicle's reference frame. + auto as_tuple() const + { + return std::make_tuple(source,lever_arm_offset); + } + + + static SpeedLeverArm create_sld_all(::mip::FunctionSelector function) + { + SpeedLeverArm cmd; + cmd.function = function; + cmd.source = 0; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; + static const uint32_t ECHOED_PARAMS = 0x0001; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t source = 0; ///< Reserved, must be 1. - float lever_arm_offset[3] = {0}; ///< [m] Lever arm offset vector in the vehicle's reference frame. + Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. + + + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(lever_arm_offset)); + } }; }; @@ -2592,11 +3681,12 @@ void extract(Serializer& serializer, SpeedLeverArm& self); void insert(Serializer& serializer, const SpeedLeverArm::Response& self); void extract(Serializer& serializer, SpeedLeverArm::Response& self); -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); +CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffset); +CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffsetOut); CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2613,25 +3703,49 @@ CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); struct WheeledVehicleConstraintControl { + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + auto as_tuple() const + { + return std::make_tuple(enable); + } + + + static WheeledVehicleConstraintControl create_sld_all(::mip::FunctionSelector function) + { + WheeledVehicleConstraintControl cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } + }; }; void insert(Serializer& serializer, const WheeledVehicleConstraintControl& self); @@ -2645,6 +3759,7 @@ CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device); CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device); CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2659,25 +3774,49 @@ CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); struct VerticalGyroConstraintControl { + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + auto as_tuple() const + { + return std::make_tuple(enable); + } + + + static VerticalGyroConstraintControl create_sld_all(::mip::FunctionSelector function) + { + VerticalGyroConstraintControl cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } + }; }; void insert(Serializer& serializer, const VerticalGyroConstraintControl& self); @@ -2691,6 +3830,7 @@ CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* e CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device); CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device); CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2703,27 +3843,51 @@ CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device); struct GnssAntennaCalControl { + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< 0 - Disable, 1 - Enable + float max_offset = 0; ///< Maximum absolute value of lever arm offset error in the vehicle frame [meters]. See device user manual for the valid range of this parameter. + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< 0 - Disable, 1 - Enable - float max_offset = 0; ///< Maximum absolute value of lever arm offset error in the vehicle frame [meters]. See device user manual for the valid range of this parameter. + auto as_tuple() const + { + return std::make_tuple(enable,max_offset); + } + + + static GnssAntennaCalControl create_sld_all(::mip::FunctionSelector function) + { + GnssAntennaCalControl cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float max_offset = 0; ///< Maximum absolute value of lever arm offset error in the vehicle frame [meters]. See device user manual for the valid range of this parameter. + + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(max_offset)); + } + }; }; void insert(Serializer& serializer, const GnssAntennaCalControl& self); @@ -2737,105 +3901,7 @@ CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut CmdResult saveGnssAntennaCalControl(C::mip_interface& device); CmdResult loadGnssAntennaCalControl(C::mip_interface& device); CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_hard_iron_offset_noise (0x0D,0x2B) Hard Iron Offset Noise [CPP] -/// Set the expected hard iron offset noise 1-sigma values -/// -/// This function can be used to tune the filter performance in the target application. -/// -/// Each of the noise values must be greater than 0.0 -/// -/// The noise value represents process noise in the 3DM-GX5-45 NAV Estimation Filter. -/// Changing this value modifies how the filter responds to dynamic input and can be used to tune the performance of the filter. -/// Default values provide good performance for most laboratory conditions. -/// -/// -///@{ - -struct HardIronOffsetNoise -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - FunctionSelector function = static_cast(0); - float x = 0; ///< HI Offset Noise 1-sima [gauss] - float y = 0; ///< HI Offset Noise 1-sima [gauss] - float z = 0; ///< HI Offset Noise 1-sima [gauss] - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; - - float x = 0; ///< HI Offset Noise 1-sima [gauss] - float y = 0; ///< HI Offset Noise 1-sima [gauss] - float z = 0; ///< HI Offset Noise 1-sima [gauss] - - }; -}; -void insert(Serializer& serializer, const HardIronOffsetNoise& self); -void extract(Serializer& serializer, HardIronOffsetNoise& self); -void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); -void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); - -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, float x, float y, float z); -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* xOut, float* yOut, float* zOut); -CmdResult saveHardIronOffsetNoise(C::mip_interface& device); -CmdResult loadHardIronOffsetNoise(C::mip_interface& device); -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_filter_magnetic_declination_source (0x0D,0x43) Magnetic Declination Source [CPP] -/// Source for magnetic declination angle, and user supplied value for manual selection. -/// -///@{ - -struct MagneticDeclinationSource -{ - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - - FunctionSelector function = static_cast(0); - FilterMagDeclinationSource source = static_cast(0); ///< Magnetic field declination angle source - float declination = 0; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - - struct Response - { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; - - FilterMagDeclinationSource source = static_cast(0); ///< Magnetic field declination angle source - float declination = 0; ///< Declination angle used when 'source' is set to 'MANUAL' (radians) - - }; -}; -void insert(Serializer& serializer, const MagneticDeclinationSource& self); -void extract(Serializer& serializer, MagneticDeclinationSource& self); - -void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); -void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); - -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource source, float declination); -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagDeclinationSource* sourceOut, float* declinationOut); -CmdResult saveMagneticDeclinationSource(C::mip_interface& device); -CmdResult loadMagneticDeclinationSource(C::mip_interface& device); -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2849,18 +3915,26 @@ CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); struct SetInitialHeading { + float heading = 0; ///< Initial heading in radians [-pi, pi] + static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; - float heading = 0; ///< Initial heading in radians [-pi, pi] + auto as_tuple() const + { + return std::make_tuple(heading); + } + typedef void Response; }; void insert(Serializer& serializer, const SetInitialHeading& self); void extract(Serializer& serializer, SetInitialHeading& self); CmdResult setInitialHeading(C::mip_interface& device, float heading); + ///@} /// diff --git a/src/mip/definitions/commands_gnss.cpp b/src/mip/definitions/commands_gnss.cpp index 00bb9e59c..d099323d0 100644 --- a/src/mip/definitions/commands_gnss.cpp +++ b/src/mip/definitions/commands_gnss.cpp @@ -50,7 +50,7 @@ void insert(Serializer& serializer, const ReceiverInfo::Response& self) } void extract(Serializer& serializer, ReceiverInfo::Response& self) { - C::extract_count(&serializer, &self.num_receivers, self.num_receivers); + C::extract_count(&serializer, &self.num_receivers, sizeof(self.receiver_info)/sizeof(self.receiver_info[0])); for(unsigned int i=0; i < self.num_receivers; i++) extract(serializer, self.receiver_info[i]); diff --git a/src/mip/definitions/commands_gnss.h b/src/mip/definitions/commands_gnss.h index 28959ca40..2f024f2cc 100644 --- a/src/mip/definitions/commands_gnss.h +++ b/src/mip/definitions/commands_gnss.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -79,7 +80,7 @@ void extract_mip_gnss_receiver_info_command_info(struct mip_serializer* serializ struct mip_gnss_receiver_info_response { uint8_t num_receivers; ///< Number of physical receivers in the device - mip_gnss_receiver_info_command_info* receiver_info; + mip_gnss_receiver_info_command_info receiver_info[5]; }; typedef struct mip_gnss_receiver_info_response mip_gnss_receiver_info_response; @@ -87,6 +88,7 @@ void insert_mip_gnss_receiver_info_response(struct mip_serializer* serializer, c void extract_mip_gnss_receiver_info_response(struct mip_serializer* serializer, mip_gnss_receiver_info_response* self); mip_cmd_result mip_gnss_receiver_info(struct mip_interface* device, uint8_t* num_receivers_out, uint8_t num_receivers_out_max, mip_gnss_receiver_info_command_info* receiver_info_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -128,6 +130,7 @@ mip_cmd_result mip_gnss_read_signal_configuration(struct mip_interface* device, mip_cmd_result mip_gnss_save_signal_configuration(struct mip_interface* device); mip_cmd_result mip_gnss_load_signal_configuration(struct mip_interface* device); mip_cmd_result mip_gnss_default_signal_configuration(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -163,6 +166,7 @@ mip_cmd_result mip_gnss_read_rtk_dongle_configuration(struct mip_interface* devi mip_cmd_result mip_gnss_save_rtk_dongle_configuration(struct mip_interface* device); mip_cmd_result mip_gnss_load_rtk_dongle_configuration(struct mip_interface* device); mip_cmd_result mip_gnss_default_rtk_dongle_configuration(struct mip_interface* device); + ///@} /// diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 1ecefbd15..605fec5ee 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -66,11 +67,6 @@ static const uint16_t GNSS_BEIDOU_ENABLE_B2 = 0x0002; struct ReceiverInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct Info { uint8_t receiver_id = 0; ///< Receiver id: e.g. 1, 2, etc. @@ -79,13 +75,33 @@ struct ReceiverInfo }; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(); + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_receivers = 0; ///< Number of physical receivers in the device - Info* receiver_info = {nullptr}; + Info receiver_info[5]; + + + auto as_tuple() + { + return std::make_tuple(std::ref(num_receivers),std::ref(receiver_info)); + } }; }; @@ -99,6 +115,7 @@ void insert(Serializer& serializer, const ReceiverInfo::Response& self); void extract(Serializer& serializer, ReceiverInfo::Response& self); CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -110,15 +127,6 @@ CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8 struct SignalConfiguration { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - FunctionSelector function = static_cast(0); uint8_t gps_enable = 0; ///< Bitfield 0: Enable L1CA, 1: Enable L2C uint8_t glonass_enable = 0; ///< Bitfield 0: Enable L1OF, 1: Enable L2OF @@ -126,17 +134,50 @@ struct SignalConfiguration uint8_t beidou_enable = 0; ///< Bitfield 0: Enable B1, 1: Enable B2 uint8_t reserved[4] = {0}; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x801F; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(gps_enable,glonass_enable,galileo_enable,beidou_enable,reserved); + } + + + static SignalConfiguration create_sld_all(::mip::FunctionSelector function) + { + SignalConfiguration cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t gps_enable = 0; ///< Bitfield 0: Enable L1CA, 1: Enable L2C uint8_t glonass_enable = 0; ///< Bitfield 0: Enable L1OF, 1: Enable L2OF uint8_t galileo_enable = 0; ///< Bitfield 0: Enable E1, 1: Enable E5B uint8_t beidou_enable = 0; ///< Bitfield 0: Enable B1, 1: Enable B2 uint8_t reserved[4] = {0}; + + auto as_tuple() + { + return std::make_tuple(std::ref(gps_enable),std::ref(glonass_enable),std::ref(galileo_enable),std::ref(beidou_enable),std::ref(reserved)); + } + }; }; void insert(Serializer& serializer, const SignalConfiguration& self); @@ -150,6 +191,7 @@ CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOu CmdResult saveSignalConfiguration(C::mip_interface& device); CmdResult loadSignalConfiguration(C::mip_interface& device); CmdResult defaultSignalConfiguration(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -161,27 +203,51 @@ CmdResult defaultSignalConfiguration(C::mip_interface& device); struct RtkDongleConfiguration { + FunctionSelector function = static_cast(0); + uint8_t enable = 0; ///< 0 - Disabled, 1- Enabled + uint8_t reserved[3] = {0}; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8003; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(enable,reserved); + } - FunctionSelector function = static_cast(0); - uint8_t enable = 0; ///< 0 - Disabled, 1- Enabled - uint8_t reserved[3] = {0}; + + static RtkDongleConfiguration create_sld_all(::mip::FunctionSelector function) + { + RtkDongleConfiguration cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; uint8_t reserved[3] = {0}; + + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(reserved)); + } + }; }; void insert(Serializer& serializer, const RtkDongleConfiguration& self); @@ -195,6 +261,7 @@ CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOu CmdResult saveRtkDongleConfiguration(C::mip_interface& device); CmdResult loadRtkDongleConfiguration(C::mip_interface& device); CmdResult defaultRtkDongleConfiguration(C::mip_interface& device); + ///@} /// diff --git a/src/mip/definitions/commands_rtk.h b/src/mip/definitions/commands_rtk.h index 31f6cb07d..6271716eb 100644 --- a/src/mip/definitions/commands_rtk.h +++ b/src/mip/definitions/commands_rtk.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -132,6 +133,7 @@ void insert_mip_rtk_get_status_flags_response(struct mip_serializer* serializer, void extract_mip_rtk_get_status_flags_response(struct mip_serializer* serializer, mip_rtk_get_status_flags_response* self); mip_cmd_result mip_rtk_get_status_flags(struct mip_interface* device, mip_rtk_get_status_flags_command_status_flags* flags_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -149,6 +151,7 @@ void insert_mip_rtk_get_imei_response(struct mip_serializer* serializer, const m void extract_mip_rtk_get_imei_response(struct mip_serializer* serializer, mip_rtk_get_imei_response* self); mip_cmd_result mip_rtk_get_imei(struct mip_interface* device, char* imei_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -166,6 +169,7 @@ void insert_mip_rtk_get_imsi_response(struct mip_serializer* serializer, const m void extract_mip_rtk_get_imsi_response(struct mip_serializer* serializer, mip_rtk_get_imsi_response* self); mip_cmd_result mip_rtk_get_imsi(struct mip_interface* device, char* imsi_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -183,6 +187,7 @@ void insert_mip_rtk_get_iccid_response(struct mip_serializer* serializer, const void extract_mip_rtk_get_iccid_response(struct mip_serializer* serializer, mip_rtk_get_iccid_response* self); mip_cmd_result mip_rtk_get_iccid(struct mip_interface* device, char* iccid_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -221,6 +226,7 @@ mip_cmd_result mip_rtk_read_connected_device_type(struct mip_interface* device, mip_cmd_result mip_rtk_save_connected_device_type(struct mip_interface* device); mip_cmd_result mip_rtk_load_connected_device_type(struct mip_interface* device); mip_cmd_result mip_rtk_default_connected_device_type(struct mip_interface* device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -238,6 +244,7 @@ void insert_mip_rtk_get_act_code_response(struct mip_serializer* serializer, con void extract_mip_rtk_get_act_code_response(struct mip_serializer* serializer, mip_rtk_get_act_code_response* self); mip_cmd_result mip_rtk_get_act_code(struct mip_interface* device, char* activation_code_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -255,6 +262,7 @@ void insert_mip_rtk_get_modem_firmware_version_response(struct mip_serializer* s void extract_mip_rtk_get_modem_firmware_version_response(struct mip_serializer* serializer, mip_rtk_get_modem_firmware_version_response* self); mip_cmd_result mip_rtk_get_modem_firmware_version(struct mip_interface* device, char* modem_firmware_version_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -275,6 +283,7 @@ void insert_mip_rtk_get_rssi_response(struct mip_serializer* serializer, const m void extract_mip_rtk_get_rssi_response(struct mip_serializer* serializer, mip_rtk_get_rssi_response* self); mip_cmd_result mip_rtk_get_rssi(struct mip_interface* device, bool* valid_out, int32_t* rssi_out, int32_t* signal_quality_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -316,6 +325,7 @@ void insert_mip_rtk_service_status_response(struct mip_serializer* serializer, c void extract_mip_rtk_service_status_response(struct mip_serializer* serializer, mip_rtk_service_status_response* self); mip_cmd_result mip_rtk_service_status(struct mip_interface* device, uint32_t reserved1, uint32_t reserved2, mip_rtk_service_status_command_service_flags* flags_out, uint32_t* received_bytes_out, uint32_t* last_bytes_out, uint64_t* last_bytes_time_out); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -335,6 +345,7 @@ void insert_mip_rtk_prod_erase_storage_command(struct mip_serializer* serializer void extract_mip_rtk_prod_erase_storage_command(struct mip_serializer* serializer, mip_rtk_prod_erase_storage_command* self); mip_cmd_result mip_rtk_prod_erase_storage(struct mip_interface* device, mip_media_selector media); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -356,6 +367,7 @@ void insert_mip_rtk_led_control_command(struct mip_serializer* serializer, const void extract_mip_rtk_led_control_command(struct mip_serializer* serializer, mip_rtk_led_control_command* self); mip_cmd_result mip_rtk_led_control(struct mip_interface* device, const uint8_t* primary_color, const uint8_t* alt_color, mip_led_action act, uint32_t period); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -366,6 +378,7 @@ mip_cmd_result mip_rtk_led_control(struct mip_interface* device, const uint8_t* ///@{ mip_cmd_result mip_rtk_modem_hard_reset(struct mip_interface* device); + ///@} /// diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 4611979f1..3ad5ccafd 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -84,11 +85,6 @@ enum class LedAction : uint8_t struct GetStatusFlags { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct StatusFlagsLegacy : Bitfield { enum _enumType : uint32_t @@ -203,13 +199,33 @@ struct GetStatusFlags }; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(); + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; StatusFlags flags; ///< Model number dependent. See above structures. + + auto as_tuple() + { + return std::make_tuple(std::ref(flags)); + } + }; }; void insert(Serializer& serializer, const GetStatusFlags& self); @@ -219,6 +235,7 @@ void insert(Serializer& serializer, const GetStatusFlags::Response& self); void extract(Serializer& serializer, GetStatusFlags::Response& self); CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -228,19 +245,34 @@ CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* struct GetImei { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; char IMEI[32] = {0}; + + auto as_tuple() + { + return std::make_tuple(std::ref(IMEI)); + } + }; }; void insert(Serializer& serializer, const GetImei& self); @@ -250,6 +282,7 @@ void insert(Serializer& serializer, const GetImei::Response& self); void extract(Serializer& serializer, GetImei::Response& self); CmdResult getImei(C::mip_interface& device, char* imeiOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -259,19 +292,34 @@ CmdResult getImei(C::mip_interface& device, char* imeiOut); struct GetImsi { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; char IMSI[32] = {0}; + + auto as_tuple() + { + return std::make_tuple(std::ref(IMSI)); + } + }; }; void insert(Serializer& serializer, const GetImsi& self); @@ -281,6 +329,7 @@ void insert(Serializer& serializer, const GetImsi::Response& self); void extract(Serializer& serializer, GetImsi::Response& self); CmdResult getImsi(C::mip_interface& device, char* imsiOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -290,19 +339,34 @@ CmdResult getImsi(C::mip_interface& device, char* imsiOut); struct GetIccid { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; char ICCID[32] = {0}; + + auto as_tuple() + { + return std::make_tuple(std::ref(ICCID)); + } + }; }; void insert(Serializer& serializer, const GetIccid& self); @@ -312,6 +376,7 @@ void insert(Serializer& serializer, const GetIccid::Response& self); void extract(Serializer& serializer, GetIccid::Response& self); CmdResult getIccid(C::mip_interface& device, char* iccidOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -321,15 +386,6 @@ CmdResult getIccid(C::mip_interface& device, char* iccidOut); struct ConnectedDeviceType { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; - - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = true; - static const bool HAS_LOAD_FUNCTION = true; - static const bool HAS_RESET_FUNCTION = true; - enum class Type : uint8_t { GENERIC = 0, ///< @@ -339,13 +395,46 @@ struct ConnectedDeviceType FunctionSelector function = static_cast(0); Type devType = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; + + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x8000; + static const uint32_t LOAD_PARAMS = 0x8000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(devType); + } + + + static ConnectedDeviceType create_sld_all(::mip::FunctionSelector function) + { + ConnectedDeviceType cmd; + cmd.function = function; + return cmd; + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; Type devType = static_cast(0); + + auto as_tuple() + { + return std::make_tuple(std::ref(devType)); + } + }; }; void insert(Serializer& serializer, const ConnectedDeviceType& self); @@ -359,6 +448,7 @@ CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType: CmdResult saveConnectedDeviceType(C::mip_interface& device); CmdResult loadConnectedDeviceType(C::mip_interface& device); CmdResult defaultConnectedDeviceType(C::mip_interface& device); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -368,19 +458,34 @@ CmdResult defaultConnectedDeviceType(C::mip_interface& device); struct GetActCode { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; char ActivationCode[32] = {0}; + + auto as_tuple() + { + return std::make_tuple(std::ref(ActivationCode)); + } + }; }; void insert(Serializer& serializer, const GetActCode& self); @@ -390,6 +495,7 @@ void insert(Serializer& serializer, const GetActCode::Response& self); void extract(Serializer& serializer, GetActCode::Response& self); CmdResult getActCode(C::mip_interface& device, char* activationcodeOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -399,19 +505,34 @@ CmdResult getActCode(C::mip_interface& device, char* activationcodeOut); struct GetModemFirmwareVersion { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; char ModemFirmwareVersion[32] = {0}; + + auto as_tuple() + { + return std::make_tuple(std::ref(ModemFirmwareVersion)); + } + }; }; void insert(Serializer& serializer, const GetModemFirmwareVersion& self); @@ -421,6 +542,7 @@ void insert(Serializer& serializer, const GetModemFirmwareVersion::Response& sel void extract(Serializer& serializer, GetModemFirmwareVersion::Response& self); CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -431,21 +553,36 @@ CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwarev struct GetRssi { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; bool valid = 0; int32_t rssi = 0; int32_t signalQuality = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(valid),std::ref(rssi),std::ref(signalQuality)); + } + }; }; void insert(Serializer& serializer, const GetRssi& self); @@ -455,6 +592,7 @@ void insert(Serializer& serializer, const GetRssi::Response& self); void extract(Serializer& serializer, GetRssi::Response& self); CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -465,11 +603,6 @@ CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, in struct ServiceStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - struct ServiceFlags : Bitfield { enum _enumType : uint8_t @@ -504,16 +637,36 @@ struct ServiceStatus uint32_t reserved1 = 0; uint32_t reserved2 = 0; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; + + static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(reserved1,reserved2); + } + struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; ServiceFlags flags; uint32_t receivedBytes = 0; uint32_t lastBytes = 0; uint64_t lastBytesTime = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(flags),std::ref(receivedBytes),std::ref(lastBytes),std::ref(lastBytesTime)); + } + }; }; void insert(Serializer& serializer, const ServiceStatus& self); @@ -523,6 +676,7 @@ void insert(Serializer& serializer, const ServiceStatus::Response& self); void extract(Serializer& serializer, ServiceStatus::Response& self); CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -534,18 +688,26 @@ CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t r struct ProdEraseStorage { + MediaSelector media = static_cast(0); + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; - MediaSelector media = static_cast(0); + auto as_tuple() const + { + return std::make_tuple(media); + } + typedef void Response; }; void insert(Serializer& serializer, const ProdEraseStorage& self); void extract(Serializer& serializer, ProdEraseStorage& self); CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -556,21 +718,29 @@ CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media); struct LedControl { + uint8_t primaryColor[3] = {0}; + uint8_t altColor[3] = {0}; + LedAction act = static_cast(0); + uint32_t period = 0; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; - uint8_t primaryColor[3] = {0}; - uint8_t altColor[3] = {0}; - LedAction act = static_cast(0); - uint32_t period = 0; + auto as_tuple() const + { + return std::make_tuple(primaryColor,altColor,act,period); + } + typedef void Response; }; void insert(Serializer& serializer, const LedControl& self); void extract(Serializer& serializer, LedControl& self); CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -582,17 +752,25 @@ CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, cons struct ModemHardReset { + static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; static const bool HAS_FUNCTION_SELECTOR = false; + static const uint32_t COUNTER_PARAMS = 0x00000000; + auto as_tuple() const + { + return std::make_tuple(); + } + typedef void Response; }; void insert(Serializer& serializer, const ModemHardReset& self); void extract(Serializer& serializer, ModemHardReset& self); CmdResult modemHardReset(C::mip_interface& device); + ///@} /// diff --git a/src/mip/definitions/commands_system.h b/src/mip/definitions/commands_system.h index f890553c2..ebdd56585 100644 --- a/src/mip/definitions/commands_system.h +++ b/src/mip/definitions/commands_system.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -91,6 +92,7 @@ void extract_mip_system_comm_mode_response(struct mip_serializer* serializer, mi mip_cmd_result mip_system_write_comm_mode(struct mip_interface* device, uint8_t mode); mip_cmd_result mip_system_read_comm_mode(struct mip_interface* device, uint8_t* mode_out); mip_cmd_result mip_system_default_comm_mode(struct mip_interface* device); + ///@} /// diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index 812075097..01e4b1e6c 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -70,25 +71,49 @@ static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_GPS = 0x03; struct CommMode { + FunctionSelector function = static_cast(0); + uint8_t mode = 0; + static const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; - static const bool HAS_WRITE_FUNCTION = true; - static const bool HAS_READ_FUNCTION = true; - static const bool HAS_SAVE_FUNCTION = false; - static const bool HAS_LOAD_FUNCTION = false; - static const bool HAS_RESET_FUNCTION = true; + static const bool HAS_FUNCTION_SELECTOR = true; + static const uint32_t WRITE_PARAMS = 0x8001; + static const uint32_t READ_PARAMS = 0x8000; + static const uint32_t SAVE_PARAMS = 0x0000; + static const uint32_t LOAD_PARAMS = 0x0000; + static const uint32_t DEFAULT_PARAMS = 0x8000; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; - FunctionSelector function = static_cast(0); - uint8_t mode = 0; + auto as_tuple() const + { + return std::make_tuple(mode); + } + + + static CommMode create_sld_all(::mip::FunctionSelector function) + { + CommMode cmd; + cmd.function = function; + return cmd; + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; + static const uint32_t ECHOED_PARAMS = 0x0000; + static const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t mode = 0; + + auto as_tuple() + { + return std::make_tuple(std::ref(mode)); + } + }; }; void insert(Serializer& serializer, const CommMode& self); @@ -100,6 +125,7 @@ void extract(Serializer& serializer, CommMode::Response& self); CmdResult writeCommMode(C::mip_interface& device, uint8_t mode); CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut); CmdResult defaultCommMode(C::mip_interface& device); + ///@} /// diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index 9dfe6eedd..b9aea9892 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -227,6 +228,7 @@ void insert_mip_filter_position_llh_data(struct mip_serializer* serializer, cons void extract_mip_filter_position_llh_data(struct mip_serializer* serializer, mip_filter_position_llh_data* self); bool extract_mip_filter_position_llh_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -248,6 +250,7 @@ void insert_mip_filter_velocity_ned_data(struct mip_serializer* serializer, cons void extract_mip_filter_velocity_ned_data(struct mip_serializer* serializer, mip_filter_velocity_ned_data* self); bool extract_mip_filter_velocity_ned_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -266,7 +269,7 @@ bool extract_mip_filter_velocity_ned_data_from_field(const struct mip_field* fie struct mip_filter_attitude_quaternion_data { - float q[4]; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND + mip_quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -275,6 +278,7 @@ void insert_mip_filter_attitude_quaternion_data(struct mip_serializer* serialize void extract_mip_filter_attitude_quaternion_data(struct mip_serializer* serializer, mip_filter_attitude_quaternion_data* self); bool extract_mip_filter_attitude_quaternion_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -295,7 +299,7 @@ bool extract_mip_filter_attitude_quaternion_data_from_field(const struct mip_fie struct mip_filter_attitude_dcm_data { - float dcm[9]; ///< Matrix elements in row-major order. + mip_matrix3f dcm; ///< Matrix elements in row-major order. uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -304,6 +308,7 @@ void insert_mip_filter_attitude_dcm_data(struct mip_serializer* serializer, cons void extract_mip_filter_attitude_dcm_data(struct mip_serializer* serializer, mip_filter_attitude_dcm_data* self); bool extract_mip_filter_attitude_dcm_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -326,6 +331,7 @@ void insert_mip_filter_euler_angles_data(struct mip_serializer* serializer, cons void extract_mip_filter_euler_angles_data(struct mip_serializer* serializer, mip_filter_euler_angles_data* self); bool extract_mip_filter_euler_angles_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -336,7 +342,7 @@ bool extract_mip_filter_euler_angles_data_from_field(const struct mip_field* fie struct mip_filter_gyro_bias_data { - float bias[3]; ///< (x, y, z) [radians/second] + mip_vector3f bias; ///< (x, y, z) [radians/second] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -345,6 +351,7 @@ void insert_mip_filter_gyro_bias_data(struct mip_serializer* serializer, const m void extract_mip_filter_gyro_bias_data(struct mip_serializer* serializer, mip_filter_gyro_bias_data* self); bool extract_mip_filter_gyro_bias_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -355,7 +362,7 @@ bool extract_mip_filter_gyro_bias_data_from_field(const struct mip_field* field, struct mip_filter_accel_bias_data { - float bias[3]; ///< (x, y, z) [meters/second^2] + mip_vector3f bias; ///< (x, y, z) [meters/second^2] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -364,6 +371,7 @@ void insert_mip_filter_accel_bias_data(struct mip_serializer* serializer, const void extract_mip_filter_accel_bias_data(struct mip_serializer* serializer, mip_filter_accel_bias_data* self); bool extract_mip_filter_accel_bias_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -385,6 +393,7 @@ void insert_mip_filter_position_llh_uncertainty_data(struct mip_serializer* seri void extract_mip_filter_position_llh_uncertainty_data(struct mip_serializer* serializer, mip_filter_position_llh_uncertainty_data* self); bool extract_mip_filter_position_llh_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -406,6 +415,7 @@ void insert_mip_filter_velocity_ned_uncertainty_data(struct mip_serializer* seri void extract_mip_filter_velocity_ned_uncertainty_data(struct mip_serializer* serializer, mip_filter_velocity_ned_uncertainty_data* self); bool extract_mip_filter_velocity_ned_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -428,6 +438,7 @@ void insert_mip_filter_euler_angles_uncertainty_data(struct mip_serializer* seri void extract_mip_filter_euler_angles_uncertainty_data(struct mip_serializer* serializer, mip_filter_euler_angles_uncertainty_data* self); bool extract_mip_filter_euler_angles_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -438,7 +449,7 @@ bool extract_mip_filter_euler_angles_uncertainty_data_from_field(const struct mi struct mip_filter_gyro_bias_uncertainty_data { - float bias_uncert[3]; ///< (x,y,z) [radians/sec] + mip_vector3f bias_uncert; ///< (x,y,z) [radians/sec] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -447,6 +458,7 @@ void insert_mip_filter_gyro_bias_uncertainty_data(struct mip_serializer* seriali void extract_mip_filter_gyro_bias_uncertainty_data(struct mip_serializer* serializer, mip_filter_gyro_bias_uncertainty_data* self); bool extract_mip_filter_gyro_bias_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -457,7 +469,7 @@ bool extract_mip_filter_gyro_bias_uncertainty_data_from_field(const struct mip_f struct mip_filter_accel_bias_uncertainty_data { - float bias_uncert[3]; ///< (x,y,z) [meters/second^2] + mip_vector3f bias_uncert; ///< (x,y,z) [meters/second^2] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -466,6 +478,7 @@ void insert_mip_filter_accel_bias_uncertainty_data(struct mip_serializer* serial void extract_mip_filter_accel_bias_uncertainty_data(struct mip_serializer* serializer, mip_filter_accel_bias_uncertainty_data* self); bool extract_mip_filter_accel_bias_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -492,6 +505,7 @@ void insert_mip_filter_timestamp_data(struct mip_serializer* serializer, const m void extract_mip_filter_timestamp_data(struct mip_serializer* serializer, mip_filter_timestamp_data* self); bool extract_mip_filter_timestamp_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -512,6 +526,7 @@ void insert_mip_filter_status_data(struct mip_serializer* serializer, const mip_ void extract_mip_filter_status_data(struct mip_serializer* serializer, mip_filter_status_data* self); bool extract_mip_filter_status_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -523,7 +538,7 @@ bool extract_mip_filter_status_data_from_field(const struct mip_field* field, vo struct mip_filter_linear_accel_data { - float accel[3]; ///< (x,y,z) [meters/second^2] + mip_vector3f accel; ///< (x,y,z) [meters/second^2] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -532,6 +547,7 @@ void insert_mip_filter_linear_accel_data(struct mip_serializer* serializer, cons void extract_mip_filter_linear_accel_data(struct mip_serializer* serializer, mip_filter_linear_accel_data* self); bool extract_mip_filter_linear_accel_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -542,7 +558,7 @@ bool extract_mip_filter_linear_accel_data_from_field(const struct mip_field* fie struct mip_filter_gravity_vector_data { - float gravity[3]; ///< (x, y, z) [meters/second^2] + mip_vector3f gravity; ///< (x, y, z) [meters/second^2] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -551,6 +567,7 @@ void insert_mip_filter_gravity_vector_data(struct mip_serializer* serializer, co void extract_mip_filter_gravity_vector_data(struct mip_serializer* serializer, mip_filter_gravity_vector_data* self); bool extract_mip_filter_gravity_vector_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -561,7 +578,7 @@ bool extract_mip_filter_gravity_vector_data_from_field(const struct mip_field* f struct mip_filter_comp_accel_data { - float accel[3]; ///< (x,y,z) [meters/second^2] + mip_vector3f accel; ///< (x,y,z) [meters/second^2] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -570,6 +587,7 @@ void insert_mip_filter_comp_accel_data(struct mip_serializer* serializer, const void extract_mip_filter_comp_accel_data(struct mip_serializer* serializer, mip_filter_comp_accel_data* self); bool extract_mip_filter_comp_accel_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -580,7 +598,7 @@ bool extract_mip_filter_comp_accel_data_from_field(const struct mip_field* field struct mip_filter_comp_angular_rate_data { - float gyro[3]; ///< (x, y, z) [radians/second] + mip_vector3f gyro; ///< (x, y, z) [radians/second] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -589,6 +607,7 @@ void insert_mip_filter_comp_angular_rate_data(struct mip_serializer* serializer, void extract_mip_filter_comp_angular_rate_data(struct mip_serializer* serializer, mip_filter_comp_angular_rate_data* self); bool extract_mip_filter_comp_angular_rate_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -599,7 +618,7 @@ bool extract_mip_filter_comp_angular_rate_data_from_field(const struct mip_field struct mip_filter_quaternion_attitude_uncertainty_data { - float q[4]; ///< [dimensionless] + mip_quatf q; ///< [dimensionless] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -608,6 +627,7 @@ void insert_mip_filter_quaternion_attitude_uncertainty_data(struct mip_serialize void extract_mip_filter_quaternion_attitude_uncertainty_data(struct mip_serializer* serializer, mip_filter_quaternion_attitude_uncertainty_data* self); bool extract_mip_filter_quaternion_attitude_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -627,6 +647,7 @@ void insert_mip_filter_wgs84_gravity_mag_data(struct mip_serializer* serializer, void extract_mip_filter_wgs84_gravity_mag_data(struct mip_serializer* serializer, mip_filter_wgs84_gravity_mag_data* self); bool extract_mip_filter_wgs84_gravity_mag_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -661,6 +682,7 @@ bool extract_mip_filter_heading_update_state_data_from_field(const struct mip_fi void insert_mip_filter_heading_update_state_data_heading_source(struct mip_serializer* serializer, const mip_filter_heading_update_state_data_heading_source self); void extract_mip_filter_heading_update_state_data_heading_source(struct mip_serializer* serializer, mip_filter_heading_update_state_data_heading_source* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -685,6 +707,7 @@ void insert_mip_filter_magnetic_model_data(struct mip_serializer* serializer, co void extract_mip_filter_magnetic_model_data(struct mip_serializer* serializer, mip_filter_magnetic_model_data* self); bool extract_mip_filter_magnetic_model_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -695,7 +718,7 @@ bool extract_mip_filter_magnetic_model_data_from_field(const struct mip_field* f struct mip_filter_accel_scale_factor_data { - float scale_factor[3]; ///< (x,y,z) [dimensionless] + mip_vector3f scale_factor; ///< (x,y,z) [dimensionless] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -704,6 +727,7 @@ void insert_mip_filter_accel_scale_factor_data(struct mip_serializer* serializer void extract_mip_filter_accel_scale_factor_data(struct mip_serializer* serializer, mip_filter_accel_scale_factor_data* self); bool extract_mip_filter_accel_scale_factor_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -714,7 +738,7 @@ bool extract_mip_filter_accel_scale_factor_data_from_field(const struct mip_fiel struct mip_filter_accel_scale_factor_uncertainty_data { - float scale_factor_uncert[3]; ///< (x,y,z) [dimensionless] + mip_vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -723,6 +747,7 @@ void insert_mip_filter_accel_scale_factor_uncertainty_data(struct mip_serializer void extract_mip_filter_accel_scale_factor_uncertainty_data(struct mip_serializer* serializer, mip_filter_accel_scale_factor_uncertainty_data* self); bool extract_mip_filter_accel_scale_factor_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -733,7 +758,7 @@ bool extract_mip_filter_accel_scale_factor_uncertainty_data_from_field(const str struct mip_filter_gyro_scale_factor_data { - float scale_factor[3]; ///< (x,y,z) [dimensionless] + mip_vector3f scale_factor; ///< (x,y,z) [dimensionless] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -742,6 +767,7 @@ void insert_mip_filter_gyro_scale_factor_data(struct mip_serializer* serializer, void extract_mip_filter_gyro_scale_factor_data(struct mip_serializer* serializer, mip_filter_gyro_scale_factor_data* self); bool extract_mip_filter_gyro_scale_factor_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -752,7 +778,7 @@ bool extract_mip_filter_gyro_scale_factor_data_from_field(const struct mip_field struct mip_filter_gyro_scale_factor_uncertainty_data { - float scale_factor_uncert[3]; ///< (x,y,z) [dimensionless] + mip_vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -761,6 +787,7 @@ void insert_mip_filter_gyro_scale_factor_uncertainty_data(struct mip_serializer* void extract_mip_filter_gyro_scale_factor_uncertainty_data(struct mip_serializer* serializer, mip_filter_gyro_scale_factor_uncertainty_data* self); bool extract_mip_filter_gyro_scale_factor_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -771,7 +798,7 @@ bool extract_mip_filter_gyro_scale_factor_uncertainty_data_from_field(const stru struct mip_filter_mag_bias_data { - float bias[3]; ///< (x,y,z) [Gauss] + mip_vector3f bias; ///< (x,y,z) [Gauss] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -780,6 +807,7 @@ void insert_mip_filter_mag_bias_data(struct mip_serializer* serializer, const mi void extract_mip_filter_mag_bias_data(struct mip_serializer* serializer, mip_filter_mag_bias_data* self); bool extract_mip_filter_mag_bias_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -790,7 +818,7 @@ bool extract_mip_filter_mag_bias_data_from_field(const struct mip_field* field, struct mip_filter_mag_bias_uncertainty_data { - float bias_uncert[3]; ///< (x,y,z) [Gauss] + mip_vector3f bias_uncert; ///< (x,y,z) [Gauss] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -799,6 +827,7 @@ void insert_mip_filter_mag_bias_uncertainty_data(struct mip_serializer* serializ void extract_mip_filter_mag_bias_uncertainty_data(struct mip_serializer* serializer, mip_filter_mag_bias_uncertainty_data* self); bool extract_mip_filter_mag_bias_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -824,6 +853,7 @@ void insert_mip_filter_standard_atmosphere_data(struct mip_serializer* serialize void extract_mip_filter_standard_atmosphere_data(struct mip_serializer* serializer, mip_filter_standard_atmosphere_data* self); bool extract_mip_filter_standard_atmosphere_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -847,6 +877,7 @@ void insert_mip_filter_pressure_altitude_data(struct mip_serializer* serializer, void extract_mip_filter_pressure_altitude_data(struct mip_serializer* serializer, mip_filter_pressure_altitude_data* self); bool extract_mip_filter_pressure_altitude_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -865,6 +896,7 @@ void insert_mip_filter_density_altitude_data(struct mip_serializer* serializer, void extract_mip_filter_density_altitude_data(struct mip_serializer* serializer, mip_filter_density_altitude_data* self); bool extract_mip_filter_density_altitude_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -877,7 +909,7 @@ bool extract_mip_filter_density_altitude_data_from_field(const struct mip_field* struct mip_filter_antenna_offset_correction_data { - float offset[3]; ///< (x,y,z) [meters] + mip_vector3f offset; ///< (x,y,z) [meters] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -886,6 +918,7 @@ void insert_mip_filter_antenna_offset_correction_data(struct mip_serializer* ser void extract_mip_filter_antenna_offset_correction_data(struct mip_serializer* serializer, mip_filter_antenna_offset_correction_data* self); bool extract_mip_filter_antenna_offset_correction_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -896,7 +929,7 @@ bool extract_mip_filter_antenna_offset_correction_data_from_field(const struct m struct mip_filter_antenna_offset_correction_uncertainty_data { - float offset_uncert[3]; ///< (x,y,z) [meters] + mip_vector3f offset_uncert; ///< (x,y,z) [meters] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -905,6 +938,7 @@ void insert_mip_filter_antenna_offset_correction_uncertainty_data(struct mip_ser void extract_mip_filter_antenna_offset_correction_uncertainty_data(struct mip_serializer* serializer, mip_filter_antenna_offset_correction_uncertainty_data* self); bool extract_mip_filter_antenna_offset_correction_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -918,7 +952,7 @@ bool extract_mip_filter_antenna_offset_correction_uncertainty_data_from_field(co struct mip_filter_multi_antenna_offset_correction_data { uint8_t receiver_id; ///< Receiver ID for the receiver to which the antenna is attached - float offset[3]; ///< (x,y,z) [meters] + mip_vector3f offset; ///< (x,y,z) [meters] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -927,6 +961,7 @@ void insert_mip_filter_multi_antenna_offset_correction_data(struct mip_serialize void extract_mip_filter_multi_antenna_offset_correction_data(struct mip_serializer* serializer, mip_filter_multi_antenna_offset_correction_data* self); bool extract_mip_filter_multi_antenna_offset_correction_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -938,7 +973,7 @@ bool extract_mip_filter_multi_antenna_offset_correction_data_from_field(const st struct mip_filter_multi_antenna_offset_correction_uncertainty_data { uint8_t receiver_id; ///< Receiver ID for the receiver to which the antenna is attached - float offset_uncert[3]; ///< (x,y,z) [meters] + mip_vector3f offset_uncert; ///< (x,y,z) [meters] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -947,6 +982,7 @@ void insert_mip_filter_multi_antenna_offset_correction_uncertainty_data(struct m void extract_mip_filter_multi_antenna_offset_correction_uncertainty_data(struct mip_serializer* serializer, mip_filter_multi_antenna_offset_correction_uncertainty_data* self); bool extract_mip_filter_multi_antenna_offset_correction_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -959,7 +995,7 @@ bool extract_mip_filter_multi_antenna_offset_correction_uncertainty_data_from_fi struct mip_filter_magnetometer_offset_data { - float hard_iron[3]; ///< (x,y,z) [Gauss] + mip_vector3f hard_iron; ///< (x,y,z) [Gauss] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -968,6 +1004,7 @@ void insert_mip_filter_magnetometer_offset_data(struct mip_serializer* serialize void extract_mip_filter_magnetometer_offset_data(struct mip_serializer* serializer, mip_filter_magnetometer_offset_data* self); bool extract_mip_filter_magnetometer_offset_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -980,7 +1017,7 @@ bool extract_mip_filter_magnetometer_offset_data_from_field(const struct mip_fie struct mip_filter_magnetometer_matrix_data { - float soft_iron[9]; ///< Row-major [dimensionless] + mip_matrix3f soft_iron; ///< Row-major [dimensionless] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -989,6 +1026,7 @@ void insert_mip_filter_magnetometer_matrix_data(struct mip_serializer* serialize void extract_mip_filter_magnetometer_matrix_data(struct mip_serializer* serializer, mip_filter_magnetometer_matrix_data* self); bool extract_mip_filter_magnetometer_matrix_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -999,7 +1037,7 @@ bool extract_mip_filter_magnetometer_matrix_data_from_field(const struct mip_fie struct mip_filter_magnetometer_offset_uncertainty_data { - float hard_iron_uncertainty[3]; ///< (x,y,z) [Gauss] + mip_vector3f hard_iron_uncertainty; ///< (x,y,z) [Gauss] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -1008,6 +1046,7 @@ void insert_mip_filter_magnetometer_offset_uncertainty_data(struct mip_serialize void extract_mip_filter_magnetometer_offset_uncertainty_data(struct mip_serializer* serializer, mip_filter_magnetometer_offset_uncertainty_data* self); bool extract_mip_filter_magnetometer_offset_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1018,7 +1057,7 @@ bool extract_mip_filter_magnetometer_offset_uncertainty_data_from_field(const st struct mip_filter_magnetometer_matrix_uncertainty_data { - float soft_iron_uncertainty[9]; ///< Row-major [dimensionless] + mip_matrix3f soft_iron_uncertainty; ///< Row-major [dimensionless] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -1027,6 +1066,7 @@ void insert_mip_filter_magnetometer_matrix_uncertainty_data(struct mip_serialize void extract_mip_filter_magnetometer_matrix_uncertainty_data(struct mip_serializer* serializer, mip_filter_magnetometer_matrix_uncertainty_data* self); bool extract_mip_filter_magnetometer_matrix_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1036,7 +1076,7 @@ bool extract_mip_filter_magnetometer_matrix_uncertainty_data_from_field(const st struct mip_filter_magnetometer_covariance_matrix_data { - float covariance[9]; + mip_matrix3f covariance; uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -1045,6 +1085,7 @@ void insert_mip_filter_magnetometer_covariance_matrix_data(struct mip_serializer void extract_mip_filter_magnetometer_covariance_matrix_data(struct mip_serializer* serializer, mip_filter_magnetometer_covariance_matrix_data* self); bool extract_mip_filter_magnetometer_covariance_matrix_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1055,7 +1096,7 @@ bool extract_mip_filter_magnetometer_covariance_matrix_data_from_field(const str struct mip_filter_magnetometer_residual_vector_data { - float residual[3]; ///< (x,y,z) [Gauss] + mip_vector3f residual; ///< (x,y,z) [Gauss] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -1064,6 +1105,7 @@ void insert_mip_filter_magnetometer_residual_vector_data(struct mip_serializer* void extract_mip_filter_magnetometer_residual_vector_data(struct mip_serializer* serializer, mip_filter_magnetometer_residual_vector_data* self); bool extract_mip_filter_magnetometer_residual_vector_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1085,6 +1127,7 @@ void insert_mip_filter_clock_correction_data(struct mip_serializer* serializer, void extract_mip_filter_clock_correction_data(struct mip_serializer* serializer, mip_filter_clock_correction_data* self); bool extract_mip_filter_clock_correction_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1106,6 +1149,7 @@ void insert_mip_filter_clock_correction_uncertainty_data(struct mip_serializer* void extract_mip_filter_clock_correction_uncertainty_data(struct mip_serializer* serializer, mip_filter_clock_correction_uncertainty_data* self); bool extract_mip_filter_clock_correction_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1127,6 +1171,7 @@ void insert_mip_filter_gnss_pos_aid_status_data(struct mip_serializer* serialize void extract_mip_filter_gnss_pos_aid_status_data(struct mip_serializer* serializer, mip_filter_gnss_pos_aid_status_data* self); bool extract_mip_filter_gnss_pos_aid_status_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1147,6 +1192,7 @@ void insert_mip_filter_gnss_att_aid_status_data(struct mip_serializer* serialize void extract_mip_filter_gnss_att_aid_status_data(struct mip_serializer* serializer, mip_filter_gnss_att_aid_status_data* self); bool extract_mip_filter_gnss_att_aid_status_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1174,6 +1220,7 @@ bool extract_mip_filter_head_aid_status_data_from_field(const struct mip_field* void insert_mip_filter_head_aid_status_data_heading_aid_type(struct mip_serializer* serializer, const mip_filter_head_aid_status_data_heading_aid_type self); void extract_mip_filter_head_aid_status_data_heading_aid_type(struct mip_serializer* serializer, mip_filter_head_aid_status_data_heading_aid_type* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1184,7 +1231,7 @@ void extract_mip_filter_head_aid_status_data_heading_aid_type(struct mip_seriali struct mip_filter_rel_pos_ned_data { - double relative_position[3]; ///< [meters, NED] + mip_vector3d relative_position; ///< [meters, NED] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -1193,6 +1240,7 @@ void insert_mip_filter_rel_pos_ned_data(struct mip_serializer* serializer, const void extract_mip_filter_rel_pos_ned_data(struct mip_serializer* serializer, mip_filter_rel_pos_ned_data* self); bool extract_mip_filter_rel_pos_ned_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1203,7 +1251,7 @@ bool extract_mip_filter_rel_pos_ned_data_from_field(const struct mip_field* fiel struct mip_filter_ecef_pos_data { - double position_ecef[3]; ///< [meters, ECEF] + mip_vector3d position_ecef; ///< [meters, ECEF] uint16_t valid_flags; ///< 0 - invalid, 1 valid }; @@ -1212,6 +1260,7 @@ void insert_mip_filter_ecef_pos_data(struct mip_serializer* serializer, const mi void extract_mip_filter_ecef_pos_data(struct mip_serializer* serializer, mip_filter_ecef_pos_data* self); bool extract_mip_filter_ecef_pos_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1222,7 +1271,7 @@ bool extract_mip_filter_ecef_pos_data_from_field(const struct mip_field* field, struct mip_filter_ecef_vel_data { - float velocity_ecef[3]; ///< [meters/second, ECEF] + mip_vector3f velocity_ecef; ///< [meters/second, ECEF] uint16_t valid_flags; ///< 0 - invalid, 1 valid }; @@ -1231,6 +1280,7 @@ void insert_mip_filter_ecef_vel_data(struct mip_serializer* serializer, const mi void extract_mip_filter_ecef_vel_data(struct mip_serializer* serializer, mip_filter_ecef_vel_data* self); bool extract_mip_filter_ecef_vel_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1241,7 +1291,7 @@ bool extract_mip_filter_ecef_vel_data_from_field(const struct mip_field* field, struct mip_filter_ecef_pos_uncertainty_data { - float pos_uncertainty[3]; ///< [meters] + mip_vector3f pos_uncertainty; ///< [meters] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -1250,6 +1300,7 @@ void insert_mip_filter_ecef_pos_uncertainty_data(struct mip_serializer* serializ void extract_mip_filter_ecef_pos_uncertainty_data(struct mip_serializer* serializer, mip_filter_ecef_pos_uncertainty_data* self); bool extract_mip_filter_ecef_pos_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1260,7 +1311,7 @@ bool extract_mip_filter_ecef_pos_uncertainty_data_from_field(const struct mip_fi struct mip_filter_ecef_vel_uncertainty_data { - float vel_uncertainty[3]; ///< [meters/second] + mip_vector3f vel_uncertainty; ///< [meters/second] uint16_t valid_flags; ///< 0 - invalid, 1 - valid }; @@ -1269,6 +1320,7 @@ void insert_mip_filter_ecef_vel_uncertainty_data(struct mip_serializer* serializ void extract_mip_filter_ecef_vel_uncertainty_data(struct mip_serializer* serializer, mip_filter_ecef_vel_uncertainty_data* self); bool extract_mip_filter_ecef_vel_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1290,6 +1342,7 @@ void insert_mip_filter_aiding_measurement_summary_data(struct mip_serializer* se void extract_mip_filter_aiding_measurement_summary_data(struct mip_serializer* serializer, mip_filter_aiding_measurement_summary_data* self); bool extract_mip_filter_aiding_measurement_summary_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1309,6 +1362,7 @@ void insert_mip_filter_odometer_scale_factor_error_data(struct mip_serializer* s void extract_mip_filter_odometer_scale_factor_error_data(struct mip_serializer* serializer, mip_filter_odometer_scale_factor_error_data* self); bool extract_mip_filter_odometer_scale_factor_error_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1328,6 +1382,7 @@ void insert_mip_filter_odometer_scale_factor_error_uncertainty_data(struct mip_s void extract_mip_filter_odometer_scale_factor_error_uncertainty_data(struct mip_serializer* serializer, mip_filter_odometer_scale_factor_error_uncertainty_data* self); bool extract_mip_filter_odometer_scale_factor_error_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1369,6 +1424,7 @@ void extract_mip_filter_gnss_dual_antenna_status_data_fix_type(struct mip_serial void insert_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, const mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags self); void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags* self); + ///@} /// diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 78bf3fd2b..347a04dc4 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -357,25 +358,25 @@ struct GnssAidStatusFlags : Bitfield struct PositionLlh { + double latitude = 0; ///< [degrees] + double longitude = 0; ///< [degrees] + double ellipsoid_height = 0; ///< [meters] + uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(latitude,longitude,ellipsoid_height,valid_flags); } - double latitude = 0; ///< [degrees] - double longitude = 0; ///< [degrees] - double ellipsoid_height = 0; ///< [meters] - uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid - }; void insert(Serializer& serializer, const PositionLlh& self); void extract(Serializer& serializer, PositionLlh& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -386,25 +387,25 @@ void extract(Serializer& serializer, PositionLlh& self); struct VelocityNed { + float north = 0; ///< [meters/second] + float east = 0; ///< [meters/second] + float down = 0; ///< [meters/second] + uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(north,east,down,valid_flags); } - float north = 0; ///< [meters/second] - float east = 0; ///< [meters/second] - float down = 0; ///< [meters/second] - uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid - }; void insert(Serializer& serializer, const VelocityNed& self); void extract(Serializer& serializer, VelocityNed& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -423,23 +424,23 @@ void extract(Serializer& serializer, VelocityNed& self); struct AttitudeQuaternion { + Quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } - float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AttitudeQuaternion& self); void extract(Serializer& serializer, AttitudeQuaternion& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -460,23 +461,23 @@ void extract(Serializer& serializer, AttitudeQuaternion& self); struct AttitudeDcm { + Matrix3f dcm; ///< Matrix elements in row-major order. + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(dcm[0],dcm[1],dcm[2],dcm[3],dcm[4],dcm[5],dcm[6],dcm[7],dcm[8],valid_flags); } - float dcm[9] = {0}; ///< Matrix elements in row-major order. - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AttitudeDcm& self); void extract(Serializer& serializer, AttitudeDcm& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -488,25 +489,25 @@ void extract(Serializer& serializer, AttitudeDcm& self); struct EulerAngles { + float roll = 0; ///< [radians] + float pitch = 0; ///< [radians] + float yaw = 0; ///< [radians] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(roll,pitch,yaw,valid_flags); } - float roll = 0; ///< [radians] - float pitch = 0; ///< [radians] - float yaw = 0; ///< [radians] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const EulerAngles& self); void extract(Serializer& serializer, EulerAngles& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -517,23 +518,23 @@ void extract(Serializer& serializer, EulerAngles& self); struct GyroBias { + Vector3f bias; ///< (x, y, z) [radians/second] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } - float bias[3] = {0}; ///< (x, y, z) [radians/second] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const GyroBias& self); void extract(Serializer& serializer, GyroBias& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -544,23 +545,23 @@ void extract(Serializer& serializer, GyroBias& self); struct AccelBias { + Vector3f bias; ///< (x, y, z) [meters/second^2] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } - float bias[3] = {0}; ///< (x, y, z) [meters/second^2] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AccelBias& self); void extract(Serializer& serializer, AccelBias& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -571,25 +572,25 @@ void extract(Serializer& serializer, AccelBias& self); struct PositionLlhUncertainty { + float north = 0; ///< [meters] + float east = 0; ///< [meters] + float down = 0; ///< [meters] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(north,east,down,valid_flags); } - float north = 0; ///< [meters] - float east = 0; ///< [meters] - float down = 0; ///< [meters] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const PositionLlhUncertainty& self); void extract(Serializer& serializer, PositionLlhUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -600,25 +601,25 @@ void extract(Serializer& serializer, PositionLlhUncertainty& self); struct VelocityNedUncertainty { + float north = 0; ///< [meters/second] + float east = 0; ///< [meters/second] + float down = 0; ///< [meters/second] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(north,east,down,valid_flags); } - float north = 0; ///< [meters/second] - float east = 0; ///< [meters/second] - float down = 0; ///< [meters/second] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const VelocityNedUncertainty& self); void extract(Serializer& serializer, VelocityNedUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -630,25 +631,25 @@ void extract(Serializer& serializer, VelocityNedUncertainty& self); struct EulerAnglesUncertainty { + float roll = 0; ///< [radians] + float pitch = 0; ///< [radians] + float yaw = 0; ///< [radians] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(roll,pitch,yaw,valid_flags); } - float roll = 0; ///< [radians] - float pitch = 0; ///< [radians] - float yaw = 0; ///< [radians] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const EulerAnglesUncertainty& self); void extract(Serializer& serializer, EulerAnglesUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -659,23 +660,23 @@ void extract(Serializer& serializer, EulerAnglesUncertainty& self); struct GyroBiasUncertainty { + Vector3f bias_uncert; ///< (x,y,z) [radians/sec] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } - float bias_uncert[3] = {0}; ///< (x,y,z) [radians/sec] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const GyroBiasUncertainty& self); void extract(Serializer& serializer, GyroBiasUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -686,23 +687,23 @@ void extract(Serializer& serializer, GyroBiasUncertainty& self); struct AccelBiasUncertainty { + Vector3f bias_uncert; ///< (x,y,z) [meters/second^2] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } - float bias_uncert[3] = {0}; ///< (x,y,z) [meters/second^2] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AccelBiasUncertainty& self); void extract(Serializer& serializer, AccelBiasUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -719,24 +720,24 @@ void extract(Serializer& serializer, AccelBiasUncertainty& self); struct Timestamp { + double tow = 0; ///< GPS Time of Week [seconds] + uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(tow,week_number,valid_flags); } - double tow = 0; ///< GPS Time of Week [seconds] - uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const Timestamp& self); void extract(Serializer& serializer, Timestamp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -747,24 +748,24 @@ void extract(Serializer& serializer, Timestamp& self); struct Status { + FilterMode filter_state = static_cast(0); ///< Device-specific filter state. Please consult the user manual for definition. + FilterDynamicsMode dynamics_mode = static_cast(0); ///< Device-specific dynamics mode. Please consult the user manual for definition. + FilterStatusFlags status_flags; ///< Device-specific status flags. Please consult the user manual for definition. + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(filter_state,dynamics_mode,status_flags); } - FilterMode filter_state = static_cast(0); ///< Device-specific filter state. Please consult the user manual for definition. - FilterDynamicsMode dynamics_mode = static_cast(0); ///< Device-specific dynamics mode. Please consult the user manual for definition. - FilterStatusFlags status_flags; ///< Device-specific status flags. Please consult the user manual for definition. - }; void insert(Serializer& serializer, const Status& self); void extract(Serializer& serializer, Status& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -776,23 +777,23 @@ void extract(Serializer& serializer, Status& self); struct LinearAccel { + Vector3f accel; ///< (x,y,z) [meters/second^2] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } - float accel[3] = {0}; ///< (x,y,z) [meters/second^2] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const LinearAccel& self); void extract(Serializer& serializer, LinearAccel& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -803,23 +804,23 @@ void extract(Serializer& serializer, LinearAccel& self); struct GravityVector { + Vector3f gravity; ///< (x, y, z) [meters/second^2] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(gravity[0],gravity[1],gravity[2],valid_flags); } - float gravity[3] = {0}; ///< (x, y, z) [meters/second^2] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const GravityVector& self); void extract(Serializer& serializer, GravityVector& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -830,23 +831,23 @@ void extract(Serializer& serializer, GravityVector& self); struct CompAccel { + Vector3f accel; ///< (x,y,z) [meters/second^2] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } - float accel[3] = {0}; ///< (x,y,z) [meters/second^2] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const CompAccel& self); void extract(Serializer& serializer, CompAccel& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -857,23 +858,23 @@ void extract(Serializer& serializer, CompAccel& self); struct CompAngularRate { + Vector3f gyro; ///< (x, y, z) [radians/second] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(gyro[0],gyro[1],gyro[2],valid_flags); } - float gyro[3] = {0}; ///< (x, y, z) [radians/second] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const CompAngularRate& self); void extract(Serializer& serializer, CompAngularRate& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -884,23 +885,23 @@ void extract(Serializer& serializer, CompAngularRate& self); struct QuaternionAttitudeUncertainty { + Quatf q; ///< [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } - float q[4] = {0}; ///< [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const QuaternionAttitudeUncertainty& self); void extract(Serializer& serializer, QuaternionAttitudeUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -911,23 +912,23 @@ void extract(Serializer& serializer, QuaternionAttitudeUncertainty& self); struct Wgs84GravityMag { + float magnitude = 0; ///< [meters/second^2] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(magnitude,valid_flags); } - float magnitude = 0; ///< [meters/second^2] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const Wgs84GravityMag& self); void extract(Serializer& serializer, Wgs84GravityMag& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -941,16 +942,6 @@ void extract(Serializer& serializer, Wgs84GravityMag& self); struct HeadingUpdateState { - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(heading,heading_1sigma,source,valid_flags); - } - enum class HeadingSource : uint16_t { NONE = 0, ///< @@ -965,10 +956,20 @@ struct HeadingUpdateState HeadingSource source = static_cast(0); uint16_t valid_flags = 0; ///< 1 if a valid heading update was received in 2 seconds, 0 otherwise. + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; + + + auto as_tuple() const + { + return std::make_tuple(heading,heading_1sigma,source,valid_flags); + } + }; void insert(Serializer& serializer, const HeadingUpdateState& self); void extract(Serializer& serializer, HeadingUpdateState& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -980,27 +981,27 @@ void extract(Serializer& serializer, HeadingUpdateState& self); struct MagneticModel { + float intensity_north = 0; ///< [Gauss] + float intensity_east = 0; ///< [Gauss] + float intensity_down = 0; ///< [Gauss] + float inclination = 0; ///< [radians] + float declination = 0; ///< [radians] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(intensity_north,intensity_east,intensity_down,inclination,declination,valid_flags); } - float intensity_north = 0; ///< [Gauss] - float intensity_east = 0; ///< [Gauss] - float intensity_down = 0; ///< [Gauss] - float inclination = 0; ///< [radians] - float declination = 0; ///< [radians] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagneticModel& self); void extract(Serializer& serializer, MagneticModel& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1011,23 +1012,23 @@ void extract(Serializer& serializer, MagneticModel& self); struct AccelScaleFactor { + Vector3f scale_factor; ///< (x,y,z) [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } - float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AccelScaleFactor& self); void extract(Serializer& serializer, AccelScaleFactor& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1038,23 +1039,23 @@ void extract(Serializer& serializer, AccelScaleFactor& self); struct AccelScaleFactorUncertainty { + Vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } - float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AccelScaleFactorUncertainty& self); void extract(Serializer& serializer, AccelScaleFactorUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1065,23 +1066,23 @@ void extract(Serializer& serializer, AccelScaleFactorUncertainty& self); struct GyroScaleFactor { + Vector3f scale_factor; ///< (x,y,z) [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } - float scale_factor[3] = {0}; ///< (x,y,z) [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const GyroScaleFactor& self); void extract(Serializer& serializer, GyroScaleFactor& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1092,23 +1093,23 @@ void extract(Serializer& serializer, GyroScaleFactor& self); struct GyroScaleFactorUncertainty { + Vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } - float scale_factor_uncert[3] = {0}; ///< (x,y,z) [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const GyroScaleFactorUncertainty& self); void extract(Serializer& serializer, GyroScaleFactorUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1119,23 +1120,23 @@ void extract(Serializer& serializer, GyroScaleFactorUncertainty& self); struct MagBias { + Vector3f bias; ///< (x,y,z) [Gauss] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } - float bias[3] = {0}; ///< (x,y,z) [Gauss] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagBias& self); void extract(Serializer& serializer, MagBias& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1146,23 +1147,23 @@ void extract(Serializer& serializer, MagBias& self); struct MagBiasUncertainty { + Vector3f bias_uncert; ///< (x,y,z) [Gauss] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } - float bias_uncert[3] = {0}; ///< (x,y,z) [Gauss] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagBiasUncertainty& self); void extract(Serializer& serializer, MagBiasUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1175,27 +1176,27 @@ void extract(Serializer& serializer, MagBiasUncertainty& self); struct StandardAtmosphere { + float geometric_altitude = 0; ///< Input into calculation [meters] + float geopotential_altitude = 0; ///< [meters] + float standard_temperature = 0; ///< [degC] + float standard_pressure = 0; ///< [milliBar] + float standard_density = 0; ///< [kilogram/meter^3] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(geometric_altitude,geopotential_altitude,standard_temperature,standard_pressure,standard_density,valid_flags); } - float geometric_altitude = 0; ///< Input into calculation [meters] - float geopotential_altitude = 0; ///< [meters] - float standard_temperature = 0; ///< [degC] - float standard_pressure = 0; ///< [milliBar] - float standard_density = 0; ///< [kilogram/meter^3] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const StandardAtmosphere& self); void extract(Serializer& serializer, StandardAtmosphere& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1210,23 +1211,23 @@ void extract(Serializer& serializer, StandardAtmosphere& self); struct PressureAltitude { + float pressure_altitude = 0; ///< [meters] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(pressure_altitude,valid_flags); } - float pressure_altitude = 0; ///< [meters] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const PressureAltitude& self); void extract(Serializer& serializer, PressureAltitude& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1236,23 +1237,23 @@ void extract(Serializer& serializer, PressureAltitude& self); struct DensityAltitude { + float density_altitude = 0; ///< m + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(density_altitude,valid_flags); } - float density_altitude = 0; ///< m - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const DensityAltitude& self); void extract(Serializer& serializer, DensityAltitude& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1265,23 +1266,23 @@ void extract(Serializer& serializer, DensityAltitude& self); struct AntennaOffsetCorrection { + Vector3f offset; ///< (x,y,z) [meters] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(offset[0],offset[1],offset[2],valid_flags); } - float offset[3] = {0}; ///< (x,y,z) [meters] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AntennaOffsetCorrection& self); void extract(Serializer& serializer, AntennaOffsetCorrection& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1292,23 +1293,23 @@ void extract(Serializer& serializer, AntennaOffsetCorrection& self); struct AntennaOffsetCorrectionUncertainty { + Vector3f offset_uncert; ///< (x,y,z) [meters] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } - float offset_uncert[3] = {0}; ///< (x,y,z) [meters] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const AntennaOffsetCorrectionUncertainty& self); void extract(Serializer& serializer, AntennaOffsetCorrectionUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1321,24 +1322,24 @@ void extract(Serializer& serializer, AntennaOffsetCorrectionUncertainty& self); struct MultiAntennaOffsetCorrection { + uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached + Vector3f offset; ///< (x,y,z) [meters] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(receiver_id,offset[0],offset[1],offset[2],valid_flags); } - uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached - float offset[3] = {0}; ///< (x,y,z) [meters] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MultiAntennaOffsetCorrection& self); void extract(Serializer& serializer, MultiAntennaOffsetCorrection& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1349,24 +1350,24 @@ void extract(Serializer& serializer, MultiAntennaOffsetCorrection& self); struct MultiAntennaOffsetCorrectionUncertainty { + uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached + Vector3f offset_uncert; ///< (x,y,z) [meters] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(receiver_id,offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } - uint8_t receiver_id = 0; ///< Receiver ID for the receiver to which the antenna is attached - float offset_uncert[3] = {0}; ///< (x,y,z) [meters] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MultiAntennaOffsetCorrectionUncertainty& self); void extract(Serializer& serializer, MultiAntennaOffsetCorrectionUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1379,23 +1380,23 @@ void extract(Serializer& serializer, MultiAntennaOffsetCorrectionUncertainty& se struct MagnetometerOffset { + Vector3f hard_iron; ///< (x,y,z) [Gauss] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(hard_iron[0],hard_iron[1],hard_iron[2],valid_flags); } - float hard_iron[3] = {0}; ///< (x,y,z) [Gauss] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagnetometerOffset& self); void extract(Serializer& serializer, MagnetometerOffset& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1408,23 +1409,23 @@ void extract(Serializer& serializer, MagnetometerOffset& self); struct MagnetometerMatrix { + Matrix3f soft_iron; ///< Row-major [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(soft_iron[0],soft_iron[1],soft_iron[2],soft_iron[3],soft_iron[4],soft_iron[5],soft_iron[6],soft_iron[7],soft_iron[8],valid_flags); } - float soft_iron[9] = {0}; ///< Row-major [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagnetometerMatrix& self); void extract(Serializer& serializer, MagnetometerMatrix& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1435,23 +1436,23 @@ void extract(Serializer& serializer, MagnetometerMatrix& self); struct MagnetometerOffsetUncertainty { + Vector3f hard_iron_uncertainty; ///< (x,y,z) [Gauss] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(hard_iron_uncertainty[0],hard_iron_uncertainty[1],hard_iron_uncertainty[2],valid_flags); } - float hard_iron_uncertainty[3] = {0}; ///< (x,y,z) [Gauss] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagnetometerOffsetUncertainty& self); void extract(Serializer& serializer, MagnetometerOffsetUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1462,23 +1463,23 @@ void extract(Serializer& serializer, MagnetometerOffsetUncertainty& self); struct MagnetometerMatrixUncertainty { + Matrix3f soft_iron_uncertainty; ///< Row-major [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(soft_iron_uncertainty[0],soft_iron_uncertainty[1],soft_iron_uncertainty[2],soft_iron_uncertainty[3],soft_iron_uncertainty[4],soft_iron_uncertainty[5],soft_iron_uncertainty[6],soft_iron_uncertainty[7],soft_iron_uncertainty[8],valid_flags); } - float soft_iron_uncertainty[9] = {0}; ///< Row-major [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagnetometerMatrixUncertainty& self); void extract(Serializer& serializer, MagnetometerMatrixUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1488,23 +1489,23 @@ void extract(Serializer& serializer, MagnetometerMatrixUncertainty& self); struct MagnetometerCovarianceMatrix { + Matrix3f covariance; + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(covariance[0],covariance[1],covariance[2],covariance[3],covariance[4],covariance[5],covariance[6],covariance[7],covariance[8],valid_flags); } - float covariance[9] = {0}; - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagnetometerCovarianceMatrix& self); void extract(Serializer& serializer, MagnetometerCovarianceMatrix& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1515,23 +1516,23 @@ void extract(Serializer& serializer, MagnetometerCovarianceMatrix& self); struct MagnetometerResidualVector { + Vector3f residual; ///< (x,y,z) [Gauss] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(residual[0],residual[1],residual[2],valid_flags); } - float residual[3] = {0}; ///< (x,y,z) [Gauss] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const MagnetometerResidualVector& self); void extract(Serializer& serializer, MagnetometerResidualVector& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1542,25 +1543,25 @@ void extract(Serializer& serializer, MagnetometerResidualVector& self); struct ClockCorrection { + uint8_t receiver_id = 0; ///< 1, 2, etc. + float bias = 0; ///< [seconds] + float bias_drift = 0; ///< [seconds/second] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(receiver_id,bias,bias_drift,valid_flags); } - uint8_t receiver_id = 0; ///< 1, 2, etc. - float bias = 0; ///< [seconds] - float bias_drift = 0; ///< [seconds/second] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const ClockCorrection& self); void extract(Serializer& serializer, ClockCorrection& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1571,25 +1572,25 @@ void extract(Serializer& serializer, ClockCorrection& self); struct ClockCorrectionUncertainty { + uint8_t receiver_id = 0; ///< 1, 2, etc. + float bias_uncertainty = 0; ///< [seconds] + float bias_drift_uncertainty = 0; ///< [seconds/second] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(receiver_id,bias_uncertainty,bias_drift_uncertainty,valid_flags); } - uint8_t receiver_id = 0; ///< 1, 2, etc. - float bias_uncertainty = 0; ///< [seconds] - float bias_drift_uncertainty = 0; ///< [seconds/second] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const ClockCorrectionUncertainty& self); void extract(Serializer& serializer, ClockCorrectionUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1600,25 +1601,25 @@ void extract(Serializer& serializer, ClockCorrectionUncertainty& self); struct GnssPosAidStatus { + uint8_t receiver_id = 0; + float time_of_week = 0; ///< Last GNSS aiding measurement time of week [seconds] + GnssAidStatusFlags status; ///< Aiding measurement status bitfield + uint8_t reserved[8] = {0}; + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(receiver_id,time_of_week,status,reserved); } - uint8_t receiver_id = 0; - float time_of_week = 0; ///< Last GNSS aiding measurement time of week [seconds] - GnssAidStatusFlags status; ///< Aiding measurement status bitfield - uint8_t reserved[8] = {0}; - }; void insert(Serializer& serializer, const GnssPosAidStatus& self); void extract(Serializer& serializer, GnssPosAidStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1629,24 +1630,24 @@ void extract(Serializer& serializer, GnssPosAidStatus& self); struct GnssAttAidStatus { + float time_of_week = 0; ///< Last valid aiding measurement time of week [seconds] [processed instead of measured?] + GnssAidStatusFlags status; ///< Last valid aiding measurement status bitfield + uint8_t reserved[8] = {0}; + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(time_of_week,status,reserved); } - float time_of_week = 0; ///< Last valid aiding measurement time of week [seconds] [processed instead of measured?] - GnssAidStatusFlags status; ///< Last valid aiding measurement status bitfield - uint8_t reserved[8] = {0}; - }; void insert(Serializer& serializer, const GnssAttAidStatus& self); void extract(Serializer& serializer, GnssAttAidStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1657,16 +1658,6 @@ void extract(Serializer& serializer, GnssAttAidStatus& self); struct HeadAidStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(time_of_week,type,reserved[0],reserved[1]); - } - enum class HeadingAidType : uint8_t { DUAL_ANTENNA = 1, ///< @@ -1677,10 +1668,20 @@ struct HeadAidStatus HeadingAidType type = static_cast(0); ///< 1 - Dual antenna, 2 - External heading message (user supplied) float reserved[2] = {0}; + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; + + + auto as_tuple() const + { + return std::make_tuple(time_of_week,type,reserved); + } + }; void insert(Serializer& serializer, const HeadAidStatus& self); void extract(Serializer& serializer, HeadAidStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1691,23 +1692,23 @@ void extract(Serializer& serializer, HeadAidStatus& self); struct RelPosNed { + Vector3d relative_position; ///< [meters, NED] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(relative_position[0],relative_position[1],relative_position[2],valid_flags); } - double relative_position[3] = {0}; ///< [meters, NED] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const RelPosNed& self); void extract(Serializer& serializer, RelPosNed& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1718,23 +1719,23 @@ void extract(Serializer& serializer, RelPosNed& self); struct EcefPos { + Vector3d position_ecef; ///< [meters, ECEF] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(position_ecef[0],position_ecef[1],position_ecef[2],valid_flags); } - double position_ecef[3] = {0}; ///< [meters, ECEF] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid - }; void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1745,23 +1746,23 @@ void extract(Serializer& serializer, EcefPos& self); struct EcefVel { + Vector3f velocity_ecef; ///< [meters/second, ECEF] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(velocity_ecef[0],velocity_ecef[1],velocity_ecef[2],valid_flags); } - float velocity_ecef[3] = {0}; ///< [meters/second, ECEF] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid - }; void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1772,23 +1773,23 @@ void extract(Serializer& serializer, EcefVel& self); struct EcefPosUncertainty { + Vector3f pos_uncertainty; ///< [meters] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(pos_uncertainty[0],pos_uncertainty[1],pos_uncertainty[2],valid_flags); } - float pos_uncertainty[3] = {0}; ///< [meters] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const EcefPosUncertainty& self); void extract(Serializer& serializer, EcefPosUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1799,23 +1800,23 @@ void extract(Serializer& serializer, EcefPosUncertainty& self); struct EcefVelUncertainty { + Vector3f vel_uncertainty; ///< [meters/second] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(vel_uncertainty[0],vel_uncertainty[1],vel_uncertainty[2],valid_flags); } - float vel_uncertainty[3] = {0}; ///< [meters/second] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const EcefVelUncertainty& self); void extract(Serializer& serializer, EcefVelUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1826,25 +1827,25 @@ void extract(Serializer& serializer, EcefVelUncertainty& self); struct AidingMeasurementSummary { + float time_of_week = 0; ///< [seconds] + uint8_t source = 0; + FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) + FilterMeasurementIndicator indicator; + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(time_of_week,source,type,indicator); } - float time_of_week = 0; ///< [seconds] - uint8_t source = 0; - FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) - FilterMeasurementIndicator indicator; - }; void insert(Serializer& serializer, const AidingMeasurementSummary& self); void extract(Serializer& serializer, AidingMeasurementSummary& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1855,23 +1856,23 @@ void extract(Serializer& serializer, AidingMeasurementSummary& self); struct OdometerScaleFactorError { + float scale_factor_error = 0; ///< [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scale_factor_error,valid_flags); } - float scale_factor_error = 0; ///< [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const OdometerScaleFactorError& self); void extract(Serializer& serializer, OdometerScaleFactorError& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1882,23 +1883,23 @@ void extract(Serializer& serializer, OdometerScaleFactorError& self); struct OdometerScaleFactorErrorUncertainty { + float scale_factor_error_uncertainty = 0; ///< [dimensionless] + uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scale_factor_error_uncertainty,valid_flags); } - float scale_factor_error_uncertainty = 0; ///< [dimensionless] - uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - }; void insert(Serializer& serializer, const OdometerScaleFactorErrorUncertainty& self); void extract(Serializer& serializer, OdometerScaleFactorErrorUncertainty& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1909,16 +1910,6 @@ void extract(Serializer& serializer, OdometerScaleFactorErrorUncertainty& self); struct GnssDualAntennaStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(time_of_week,heading,heading_unc,fix_type,status_flags,valid_flags); - } - enum class FixType : uint8_t { FIX_NONE = 0, ///< @@ -1964,10 +1955,20 @@ struct GnssDualAntennaStatus DualAntennaStatusFlags status_flags; uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid + static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; + + + auto as_tuple() const + { + return std::make_tuple(time_of_week,heading,heading_unc,fix_type,status_flags,valid_flags); + } + }; void insert(Serializer& serializer, const GnssDualAntennaStatus& self); void extract(Serializer& serializer, GnssDualAntennaStatus& self); + ///@} /// diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index 9bab81253..9417718d7 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -206,6 +207,7 @@ bool extract_mip_gnss_pos_llh_data_from_field(const struct mip_field* field, voi void insert_mip_gnss_pos_llh_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_pos_llh_data_valid_flags self); void extract_mip_gnss_pos_llh_data_valid_flags(struct mip_serializer* serializer, mip_gnss_pos_llh_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -223,7 +225,7 @@ static const mip_gnss_pos_ecef_data_valid_flags MIP_GNSS_POS_ECEF_DATA_VALID_FLA struct mip_gnss_pos_ecef_data { - double x[3]; ///< [meters] + mip_vector3d x; ///< [meters] float x_accuracy; ///< [meters] mip_gnss_pos_ecef_data_valid_flags valid_flags; @@ -236,6 +238,7 @@ bool extract_mip_gnss_pos_ecef_data_from_field(const struct mip_field* field, vo void insert_mip_gnss_pos_ecef_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_pos_ecef_data_valid_flags self); void extract_mip_gnss_pos_ecef_data_valid_flags(struct mip_serializer* serializer, mip_gnss_pos_ecef_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -257,7 +260,7 @@ static const mip_gnss_vel_ned_data_valid_flags MIP_GNSS_VEL_NED_DATA_VALID_FLAGS struct mip_gnss_vel_ned_data { - float v[3]; ///< [meters/second] + mip_vector3f v; ///< [meters/second] float speed; ///< [meters/second] float ground_speed; ///< [meters/second] float heading; ///< [degrees] @@ -274,6 +277,7 @@ bool extract_mip_gnss_vel_ned_data_from_field(const struct mip_field* field, voi void insert_mip_gnss_vel_ned_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_vel_ned_data_valid_flags self); void extract_mip_gnss_vel_ned_data_valid_flags(struct mip_serializer* serializer, mip_gnss_vel_ned_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -291,7 +295,7 @@ static const mip_gnss_vel_ecef_data_valid_flags MIP_GNSS_VEL_ECEF_DATA_VALID_FLA struct mip_gnss_vel_ecef_data { - float v[3]; ///< [meters/second] + mip_vector3f v; ///< [meters/second] float v_accuracy; ///< [meters/second] mip_gnss_vel_ecef_data_valid_flags valid_flags; @@ -304,6 +308,7 @@ bool extract_mip_gnss_vel_ecef_data_from_field(const struct mip_field* field, vo void insert_mip_gnss_vel_ecef_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_vel_ecef_data_valid_flags self); void extract_mip_gnss_vel_ecef_data_valid_flags(struct mip_serializer* serializer, mip_gnss_vel_ecef_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -344,6 +349,7 @@ bool extract_mip_gnss_dop_data_from_field(const struct mip_field* field, void* p void insert_mip_gnss_dop_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_dop_data_valid_flags self); void extract_mip_gnss_dop_data_valid_flags(struct mip_serializer* serializer, mip_gnss_dop_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -379,6 +385,7 @@ bool extract_mip_gnss_utc_time_data_from_field(const struct mip_field* field, vo void insert_mip_gnss_utc_time_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_utc_time_data_valid_flags self); void extract_mip_gnss_utc_time_data_valid_flags(struct mip_serializer* serializer, mip_gnss_utc_time_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -409,6 +416,7 @@ bool extract_mip_gnss_gps_time_data_from_field(const struct mip_field* field, vo void insert_mip_gnss_gps_time_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_gps_time_data_valid_flags self); void extract_mip_gnss_gps_time_data_valid_flags(struct mip_serializer* serializer, mip_gnss_gps_time_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -441,6 +449,7 @@ bool extract_mip_gnss_clock_info_data_from_field(const struct mip_field* field, void insert_mip_gnss_clock_info_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_clock_info_data_valid_flags self); void extract_mip_gnss_clock_info_data_valid_flags(struct mip_serializer* serializer, mip_gnss_clock_info_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -494,6 +503,7 @@ void extract_mip_gnss_fix_info_data_fix_flags(struct mip_serializer* serializer, void insert_mip_gnss_fix_info_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_fix_info_data_valid_flags self); void extract_mip_gnss_fix_info_data_valid_flags(struct mip_serializer* serializer, mip_gnss_fix_info_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -543,6 +553,7 @@ void extract_mip_gnss_sv_info_data_svflags(struct mip_serializer* serializer, mi void insert_mip_gnss_sv_info_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_sv_info_data_valid_flags self); void extract_mip_gnss_sv_info_data_valid_flags(struct mip_serializer* serializer, mip_gnss_sv_info_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -601,6 +612,7 @@ void extract_mip_gnss_hw_status_data_antenna_power(struct mip_serializer* serial void insert_mip_gnss_hw_status_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_hw_status_data_valid_flags self); void extract_mip_gnss_hw_status_data_valid_flags(struct mip_serializer* serializer, mip_gnss_hw_status_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -647,6 +659,7 @@ bool extract_mip_gnss_dgps_info_data_from_field(const struct mip_field* field, v void insert_mip_gnss_dgps_info_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_dgps_info_data_valid_flags self); void extract_mip_gnss_dgps_info_data_valid_flags(struct mip_serializer* serializer, mip_gnss_dgps_info_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -683,6 +696,7 @@ bool extract_mip_gnss_dgps_channel_data_from_field(const struct mip_field* field void insert_mip_gnss_dgps_channel_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_dgps_channel_data_valid_flags self); void extract_mip_gnss_dgps_channel_data_valid_flags(struct mip_serializer* serializer, mip_gnss_dgps_channel_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -719,6 +733,7 @@ bool extract_mip_gnss_clock_info_2_data_from_field(const struct mip_field* field void insert_mip_gnss_clock_info_2_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_clock_info_2_data_valid_flags self); void extract_mip_gnss_clock_info_2_data_valid_flags(struct mip_serializer* serializer, mip_gnss_clock_info_2_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -746,6 +761,7 @@ bool extract_mip_gnss_gps_leap_seconds_data_from_field(const struct mip_field* f void insert_mip_gnss_gps_leap_seconds_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_gps_leap_seconds_data_valid_flags self); void extract_mip_gnss_gps_leap_seconds_data_valid_flags(struct mip_serializer* serializer, mip_gnss_gps_leap_seconds_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -795,6 +811,7 @@ void extract_mip_gnss_sbas_info_data_sbas_status(struct mip_serializer* serializ void insert_mip_gnss_sbas_info_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_sbas_info_data_valid_flags self); void extract_mip_gnss_sbas_info_data_valid_flags(struct mip_serializer* serializer, mip_gnss_sbas_info_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -855,6 +872,7 @@ bool extract_mip_gnss_sbas_correction_data_from_field(const struct mip_field* fi void insert_mip_gnss_sbas_correction_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_sbas_correction_data_valid_flags self); void extract_mip_gnss_sbas_correction_data_valid_flags(struct mip_serializer* serializer, mip_gnss_sbas_correction_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -915,6 +933,7 @@ void extract_mip_gnss_rf_error_detection_data_spoofing_state(struct mip_serializ void insert_mip_gnss_rf_error_detection_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_rf_error_detection_data_valid_flags self); void extract_mip_gnss_rf_error_detection_data_valid_flags(struct mip_serializer* serializer, mip_gnss_rf_error_detection_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -953,7 +972,7 @@ struct mip_gnss_base_station_info_data { double time_of_week; ///< GPS Time of week the message was received [seconds] uint16_t week_number; ///< GPS Week since 1980 [weeks] - double ecef_pos[3]; ///< Earth-centered, Earth-fixed [m] + mip_vector3d ecef_pos; ///< Earth-centered, Earth-fixed [m] float height; ///< Antenna Height above the marker used in the survey [m] uint16_t station_id; ///< Range: 0-4095 mip_gnss_base_station_info_data_indicator_flags indicators; ///< Bitfield @@ -971,6 +990,7 @@ void extract_mip_gnss_base_station_info_data_indicator_flags(struct mip_serializ void insert_mip_gnss_base_station_info_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_base_station_info_data_valid_flags self); void extract_mip_gnss_base_station_info_data_valid_flags(struct mip_serializer* serializer, mip_gnss_base_station_info_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1029,6 +1049,7 @@ void extract_mip_gnss_rtk_corrections_status_data_valid_flags(struct mip_seriali void insert_mip_gnss_rtk_corrections_status_data_epoch_status(struct mip_serializer* serializer, const mip_gnss_rtk_corrections_status_data_epoch_status self); void extract_mip_gnss_rtk_corrections_status_data_epoch_status(struct mip_serializer* serializer, mip_gnss_rtk_corrections_status_data_epoch_status* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1071,6 +1092,7 @@ bool extract_mip_gnss_satellite_status_data_from_field(const struct mip_field* f void insert_mip_gnss_satellite_status_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_satellite_status_data_valid_flags self); void extract_mip_gnss_satellite_status_data_valid_flags(struct mip_serializer* serializer, mip_gnss_satellite_status_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1142,6 +1164,7 @@ void extract_mip_gnss_raw_data_gnss_signal_quality(struct mip_serializer* serial void insert_mip_gnss_raw_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_raw_data_valid_flags self); void extract_mip_gnss_raw_data_valid_flags(struct mip_serializer* serializer, mip_gnss_raw_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1203,6 +1226,7 @@ bool extract_mip_gnss_gps_ephemeris_data_from_field(const struct mip_field* fiel void insert_mip_gnss_gps_ephemeris_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_gps_ephemeris_data_valid_flags self); void extract_mip_gnss_gps_ephemeris_data_valid_flags(struct mip_serializer* serializer, mip_gnss_gps_ephemeris_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1264,6 +1288,7 @@ bool extract_mip_gnss_galileo_ephemeris_data_from_field(const struct mip_field* void insert_mip_gnss_galileo_ephemeris_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_galileo_ephemeris_data_valid_flags self); void extract_mip_gnss_galileo_ephemeris_data_valid_flags(struct mip_serializer* serializer, mip_gnss_galileo_ephemeris_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1291,9 +1316,9 @@ struct mip_gnss_glo_ephemeris_data uint8_t sat_type; ///< Type of satellite (M) GLONASS = 0, GLONASS-M = 1 double gamma; ///< Relative deviation of carrier frequency from nominal [dimensionless] double tau_n; ///< Time correction relative to GLONASS Time [seconds] - double x[3]; ///< Satellite PE-90 position [m] - float v[3]; ///< Satellite PE-90 velocity [m/s] - float a[3]; ///< Satellite PE-90 acceleration due to perturbations [m/s^2] + mip_vector3d x; ///< Satellite PE-90 position [m] + mip_vector3f v; ///< Satellite PE-90 velocity [m/s] + mip_vector3f a; ///< Satellite PE-90 acceleration due to perturbations [m/s^2] uint8_t health; ///< Satellite Health (Bn), Non-zero indicates satellite malfunction uint8_t P; ///< Satellite operation mode (See GLONASS ICD) uint8_t NT; ///< Day number within a 4 year period. @@ -1315,6 +1340,7 @@ bool extract_mip_gnss_glo_ephemeris_data_from_field(const struct mip_field* fiel void insert_mip_gnss_glo_ephemeris_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_glo_ephemeris_data_valid_flags self); void extract_mip_gnss_glo_ephemeris_data_valid_flags(struct mip_serializer* serializer, mip_gnss_glo_ephemeris_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1349,6 +1375,7 @@ bool extract_mip_gnss_gps_iono_corr_data_from_field(const struct mip_field* fiel void insert_mip_gnss_gps_iono_corr_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_gps_iono_corr_data_valid_flags self); void extract_mip_gnss_gps_iono_corr_data_valid_flags(struct mip_serializer* serializer, mip_gnss_gps_iono_corr_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1370,7 +1397,7 @@ struct mip_gnss_galileo_iono_corr_data { double time_of_week; ///< GPS Time of week [seconds] uint16_t week_number; ///< GPS Week since 1980 [weeks] - double alpha[3]; ///< Coefficients for the model. + mip_vector3d alpha; ///< Coefficients for the model. uint8_t disturbance_flags; ///< Region disturbance flags (bits 1-5). mip_gnss_galileo_iono_corr_data_valid_flags valid_flags; @@ -1383,6 +1410,7 @@ bool extract_mip_gnss_galileo_iono_corr_data_from_field(const struct mip_field* void insert_mip_gnss_galileo_iono_corr_data_valid_flags(struct mip_serializer* serializer, const mip_gnss_galileo_iono_corr_data_valid_flags self); void extract_mip_gnss_galileo_iono_corr_data_valid_flags(struct mip_serializer* serializer, mip_gnss_galileo_iono_corr_data_valid_flags* self); + ///@} /// diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index cf38aef40..c1c694858 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -175,16 +176,6 @@ static const uint32_t GNSS_SV_INFO_MAX_SV_NUMBER = 32; struct PosLlh { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(latitude,longitude,ellipsoid_height,msl_height,horizontal_accuracy,vertical_accuracy,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -233,10 +224,20 @@ struct PosLlh float vertical_accuracy = 0; ///< [meters] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; + + + auto as_tuple() const + { + return std::make_tuple(latitude,longitude,ellipsoid_height,msl_height,horizontal_accuracy,vertical_accuracy,valid_flags); + } + }; void insert(Serializer& serializer, const PosLlh& self); void extract(Serializer& serializer, PosLlh& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -247,16 +248,6 @@ void extract(Serializer& serializer, PosLlh& self); struct PosEcef { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(x[0],x[1],x[2],x_accuracy,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -288,14 +279,24 @@ struct PosEcef void setAll() { value |= ALL; } }; - double x[3] = {0}; ///< [meters] + Vector3d x; ///< [meters] float x_accuracy = 0; ///< [meters] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; + + + auto as_tuple() const + { + return std::make_tuple(x[0],x[1],x[2],x_accuracy,valid_flags); + } + }; void insert(Serializer& serializer, const PosEcef& self); void extract(Serializer& serializer, PosEcef& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -306,16 +307,6 @@ void extract(Serializer& serializer, PosEcef& self); struct VelNed { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(v[0],v[1],v[2],speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -359,7 +350,7 @@ struct VelNed void setAll() { value |= ALL; } }; - float v[3] = {0}; ///< [meters/second] + Vector3f v; ///< [meters/second] float speed = 0; ///< [meters/second] float ground_speed = 0; ///< [meters/second] float heading = 0; ///< [degrees] @@ -367,10 +358,20 @@ struct VelNed float heading_accuracy = 0; ///< [degrees] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; + + + auto as_tuple() const + { + return std::make_tuple(v[0],v[1],v[2],speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); + } + }; void insert(Serializer& serializer, const VelNed& self); void extract(Serializer& serializer, VelNed& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -381,16 +382,6 @@ void extract(Serializer& serializer, VelNed& self); struct VelEcef { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(v[0],v[1],v[2],v_accuracy,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -422,14 +413,24 @@ struct VelEcef void setAll() { value |= ALL; } }; - float v[3] = {0}; ///< [meters/second] + Vector3f v; ///< [meters/second] float v_accuracy = 0; ///< [meters/second] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; + + + auto as_tuple() const + { + return std::make_tuple(v[0],v[1],v[2],v_accuracy,valid_flags); + } + }; void insert(Serializer& serializer, const VelEcef& self); void extract(Serializer& serializer, VelEcef& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -440,16 +441,6 @@ void extract(Serializer& serializer, VelEcef& self); struct Dop { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(gdop,pdop,hdop,vdop,tdop,ndop,edop,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -505,10 +496,20 @@ struct Dop float edop = 0; ///< Easting DOP ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; + + + auto as_tuple() const + { + return std::make_tuple(gdop,pdop,hdop,vdop,tdop,ndop,edop,valid_flags); + } + }; void insert(Serializer& serializer, const Dop& self); void extract(Serializer& serializer, Dop& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -519,16 +520,6 @@ void extract(Serializer& serializer, Dop& self); struct UtcTime { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(year,month,day,hour,min,sec,msec,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -569,10 +560,20 @@ struct UtcTime uint32_t msec = 0; ///< [Milliseconds] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; + + + auto as_tuple() const + { + return std::make_tuple(year,month,day,hour,min,sec,msec,valid_flags); + } + }; void insert(Serializer& serializer, const UtcTime& self); void extract(Serializer& serializer, UtcTime& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -583,16 +584,6 @@ void extract(Serializer& serializer, UtcTime& self); struct GpsTime { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(tow,week_number,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -628,10 +619,20 @@ struct GpsTime uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; + + + auto as_tuple() const + { + return std::make_tuple(tow,week_number,valid_flags); + } + }; void insert(Serializer& serializer, const GpsTime& self); void extract(Serializer& serializer, GpsTime& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -642,16 +643,6 @@ void extract(Serializer& serializer, GpsTime& self); struct ClockInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(bias,drift,accuracy_estimate,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -691,10 +682,20 @@ struct ClockInfo double accuracy_estimate = 0; ///< [seconds] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; + + + auto as_tuple() const + { + return std::make_tuple(bias,drift,accuracy_estimate,valid_flags); + } + }; void insert(Serializer& serializer, const ClockInfo& self); void extract(Serializer& serializer, ClockInfo& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -705,16 +706,6 @@ void extract(Serializer& serializer, ClockInfo& self); struct FixInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(fix_type,num_sv,fix_flags,valid_flags); - } - enum class FixType : uint8_t { FIX_3D = 0, ///< @@ -793,10 +784,20 @@ struct FixInfo FixFlags fix_flags; ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; + + + auto as_tuple() const + { + return std::make_tuple(fix_type,num_sv,fix_flags,valid_flags); + } + }; void insert(Serializer& serializer, const FixInfo& self); void extract(Serializer& serializer, FixInfo& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -809,16 +810,6 @@ void extract(Serializer& serializer, FixInfo& self); struct SvInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(channel,sv_id,carrier_noise_ratio,azimuth,elevation,sv_flags,valid_flags); - } - struct SVFlags : Bitfield { enum _enumType : uint16_t @@ -898,10 +889,20 @@ struct SvInfo SVFlags sv_flags; ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; + + + auto as_tuple() const + { + return std::make_tuple(channel,sv_id,carrier_noise_ratio,azimuth,elevation,sv_flags,valid_flags); + } + }; void insert(Serializer& serializer, const SvInfo& self); void extract(Serializer& serializer, SvInfo& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -912,16 +913,6 @@ void extract(Serializer& serializer, SvInfo& self); struct HwStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(receiver_state,antenna_state,antenna_power,valid_flags); - } - enum class ReceiverState : uint8_t { OFF = 0, ///< @@ -984,10 +975,20 @@ struct HwStatus AntennaPower antenna_power = static_cast(0); ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; + + + auto as_tuple() const + { + return std::make_tuple(receiver_state,antenna_state,antenna_power,valid_flags); + } + }; void insert(Serializer& serializer, const HwStatus& self); void extract(Serializer& serializer, HwStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1010,16 +1011,6 @@ void extract(Serializer& serializer, HwStatus& self); struct DgpsInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1063,10 +1054,20 @@ struct DgpsInfo float range_rate_correction = 0; ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; + + + auto as_tuple() const + { + return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); + } + }; void insert(Serializer& serializer, const DgpsInfo& self); void extract(Serializer& serializer, DgpsInfo& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1079,16 +1080,6 @@ void extract(Serializer& serializer, DgpsInfo& self); struct DgpsChannel { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1132,10 +1123,20 @@ struct DgpsChannel float range_rate_correction = 0; ///< [m/s] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; + + + auto as_tuple() const + { + return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); + } + }; void insert(Serializer& serializer, const DgpsChannel& self); void extract(Serializer& serializer, DgpsChannel& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1148,16 +1149,6 @@ void extract(Serializer& serializer, DgpsChannel& self); struct ClockInfo2 { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(bias,drift,bias_accuracy_estimate,drift_accuracy_estimate,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1201,10 +1192,20 @@ struct ClockInfo2 double drift_accuracy_estimate = 0; ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; + + + auto as_tuple() const + { + return std::make_tuple(bias,drift,bias_accuracy_estimate,drift_accuracy_estimate,valid_flags); + } + }; void insert(Serializer& serializer, const ClockInfo2& self); void extract(Serializer& serializer, ClockInfo2& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1215,16 +1216,6 @@ void extract(Serializer& serializer, ClockInfo2& self); struct GpsLeapSeconds { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(leap_seconds,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1253,10 +1244,20 @@ struct GpsLeapSeconds uint8_t leap_seconds = 0; ///< [s] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; + + + auto as_tuple() const + { + return std::make_tuple(leap_seconds,valid_flags); + } + }; void insert(Serializer& serializer, const GpsLeapSeconds& self); void extract(Serializer& serializer, GpsLeapSeconds& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1267,16 +1268,6 @@ void extract(Serializer& serializer, GpsLeapSeconds& self); struct SbasInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(time_of_week,week_number,sbas_system,sbas_id,count,sbas_status,valid_flags); - } - struct SbasStatus : Bitfield { enum _enumType : uint8_t @@ -1362,10 +1353,20 @@ struct SbasInfo SbasStatus sbas_status; ///< Status of the SBAS service ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; + + + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,sbas_system,sbas_id,count,sbas_status,valid_flags); + } + }; void insert(Serializer& serializer, const SbasInfo& self); void extract(Serializer& serializer, SbasInfo& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1398,16 +1399,6 @@ void extract(Serializer& serializer, SbasInfo& self); struct SbasCorrection { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(index,count,time_of_week,week_number,gnss_id,sv_id,udrei,pseudorange_correction,iono_correction,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1453,10 +1444,20 @@ struct SbasCorrection float iono_correction = 0; ///< Ionospheric correction [meters]. ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; + + + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,gnss_id,sv_id,udrei,pseudorange_correction,iono_correction,valid_flags); + } + }; void insert(Serializer& serializer, const SbasCorrection& self); void extract(Serializer& serializer, SbasCorrection& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1467,16 +1468,6 @@ void extract(Serializer& serializer, SbasCorrection& self); struct RfErrorDetection { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(rf_band,jamming_state,spoofing_state,reserved,valid_flags); - } - enum class RFBand : uint8_t { UNKNOWN = 0, ///< @@ -1541,10 +1532,20 @@ struct RfErrorDetection uint8_t reserved[4] = {0}; ///< Reserved for future use ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; + + + auto as_tuple() const + { + return std::make_tuple(rf_band,jamming_state,spoofing_state,reserved,valid_flags); + } + }; void insert(Serializer& serializer, const RfErrorDetection& self); void extract(Serializer& serializer, RfErrorDetection& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1557,16 +1558,6 @@ void extract(Serializer& serializer, RfErrorDetection& self); struct BaseStationInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(time_of_week,week_number,ecef_pos[0],ecef_pos[1],ecef_pos[2],height,station_id,indicators,valid_flags); - } - struct IndicatorFlags : Bitfield { enum _enumType : uint16_t @@ -1661,16 +1652,26 @@ struct BaseStationInfo double time_of_week = 0; ///< GPS Time of week the message was received [seconds] uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] - double ecef_pos[3] = {0}; ///< Earth-centered, Earth-fixed [m] + Vector3d ecef_pos; ///< Earth-centered, Earth-fixed [m] float height = 0; ///< Antenna Height above the marker used in the survey [m] uint16_t station_id = 0; ///< Range: 0-4095 IndicatorFlags indicators; ///< Bitfield ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; + + + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,ecef_pos[0],ecef_pos[1],ecef_pos[2],height,station_id,indicators,valid_flags); + } + }; void insert(Serializer& serializer, const BaseStationInfo& self); void extract(Serializer& serializer, BaseStationInfo& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1680,16 +1681,6 @@ void extract(Serializer& serializer, BaseStationInfo& self); struct RtkCorrectionsStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved[0],reserved[1],reserved[2],reserved[3],valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1799,10 +1790,20 @@ struct RtkCorrectionsStatus uint32_t reserved[4] = {0}; ///< Reserved for future use ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; + + + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved,valid_flags); + } + }; void insert(Serializer& serializer, const RtkCorrectionsStatus& self); void extract(Serializer& serializer, RtkCorrectionsStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1813,16 +1814,6 @@ void extract(Serializer& serializer, RtkCorrectionsStatus& self); struct SatelliteStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(index,count,time_of_week,week_number,gnss_id,satellite_id,elevation,azimuth,health,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -1880,10 +1871,20 @@ struct SatelliteStatus bool health = 0; ///< True if the satellite is healthy. ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; + + + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,gnss_id,satellite_id,elevation,azimuth,health,valid_flags); + } + }; void insert(Serializer& serializer, const SatelliteStatus& self); void extract(Serializer& serializer, SatelliteStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -1894,16 +1895,6 @@ void extract(Serializer& serializer, SatelliteStatus& self); struct Raw { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(index,count,time_of_week,week_number,receiver_id,tracking_channel,gnss_id,satellite_id,signal_id,signal_strength,quality,pseudorange,carrier_phase,doppler,range_uncert,phase_uncert,doppler_uncert,lock_time,valid_flags); - } - enum class GnssSignalQuality : uint8_t { NONE = 0, ///< @@ -2007,10 +1998,20 @@ struct Raw float lock_time = 0; ///< DOC Minimum carrier phase lock time [s]. Note: the maximum value is dependent on the receiver. ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; + + + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,receiver_id,tracking_channel,gnss_id,satellite_id,signal_id,signal_strength,quality,pseudorange,carrier_phase,doppler,range_uncert,phase_uncert,doppler_uncert,lock_time,valid_flags); + } + }; void insert(Serializer& serializer, const Raw& self); void extract(Serializer& serializer, Raw& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2021,16 +2022,6 @@ void extract(Serializer& serializer, Raw& self); struct GpsEphemeris { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -2097,10 +2088,20 @@ struct GpsEphemeris double c_rs = 0; ///< Harmonic Correction Term. ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; + + + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); + } + }; void insert(Serializer& serializer, const GpsEphemeris& self); void extract(Serializer& serializer, GpsEphemeris& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2111,16 +2112,6 @@ void extract(Serializer& serializer, GpsEphemeris& self); struct GalileoEphemeris { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -2187,10 +2178,20 @@ struct GalileoEphemeris double c_rs = 0; ///< Harmonic Correction Term. ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; + + + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); + } + }; void insert(Serializer& serializer, const GalileoEphemeris& self); void extract(Serializer& serializer, GalileoEphemeris& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2201,16 +2202,6 @@ void extract(Serializer& serializer, GalileoEphemeris& self); struct GloEphemeris { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x[0],x[1],x[2],v[0],v[1],v[2],a[0],a[1],a[2],health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -2250,9 +2241,9 @@ struct GloEphemeris uint8_t sat_type = 0; ///< Type of satellite (M) GLONASS = 0, GLONASS-M = 1 double gamma = 0; ///< Relative deviation of carrier frequency from nominal [dimensionless] double tau_n = 0; ///< Time correction relative to GLONASS Time [seconds] - double x[3] = {0}; ///< Satellite PE-90 position [m] - float v[3] = {0}; ///< Satellite PE-90 velocity [m/s] - float a[3] = {0}; ///< Satellite PE-90 acceleration due to perturbations [m/s^2] + Vector3d x; ///< Satellite PE-90 position [m] + Vector3f v; ///< Satellite PE-90 velocity [m/s] + Vector3f a; ///< Satellite PE-90 acceleration due to perturbations [m/s^2] uint8_t health = 0; ///< Satellite Health (Bn), Non-zero indicates satellite malfunction uint8_t P = 0; ///< Satellite operation mode (See GLONASS ICD) uint8_t NT = 0; ///< Day number within a 4 year period. @@ -2265,10 +2256,20 @@ struct GloEphemeris uint8_t P4 = 0; ///< Flag indicating ephemeris parameters are present ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; + + + auto as_tuple() const + { + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x[0],x[1],x[2],v[0],v[1],v[2],a[0],a[1],a[2],health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); + } + }; void insert(Serializer& serializer, const GloEphemeris& self); void extract(Serializer& serializer, GloEphemeris& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2279,16 +2280,6 @@ void extract(Serializer& serializer, GloEphemeris& self); struct GpsIonoCorr { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],alpha[3],beta[0],beta[1],beta[2],beta[3],valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -2332,10 +2323,20 @@ struct GpsIonoCorr double beta[4] = {0}; ///< Ionospheric Correction Terms. ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; + + + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,alpha,beta,valid_flags); + } + }; void insert(Serializer& serializer, const GpsIonoCorr& self); void extract(Serializer& serializer, GpsIonoCorr& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -2346,16 +2347,6 @@ void extract(Serializer& serializer, GpsIonoCorr& self); struct GalileoIonoCorr { - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],disturbance_flags,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -2395,14 +2386,24 @@ struct GalileoIonoCorr double time_of_week = 0; ///< GPS Time of week [seconds] uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] - double alpha[3] = {0}; ///< Coefficients for the model. + Vector3d alpha; ///< Coefficients for the model. uint8_t disturbance_flags = 0; ///< Region disturbance flags (bits 1-5). ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; + + + auto as_tuple() const + { + return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],disturbance_flags,valid_flags); + } + }; void insert(Serializer& serializer, const GalileoIonoCorr& self); void extract(Serializer& serializer, GalileoIonoCorr& self); + ///@} /// diff --git a/src/mip/definitions/data_sensor.h b/src/mip/definitions/data_sensor.h index 40267b92a..bae38d0a0 100644 --- a/src/mip/definitions/data_sensor.h +++ b/src/mip/definitions/data_sensor.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -79,7 +80,7 @@ enum struct mip_sensor_raw_accel_data { - float raw_accel[3]; ///< Native sensor counts + mip_vector3f raw_accel; ///< Native sensor counts }; typedef struct mip_sensor_raw_accel_data mip_sensor_raw_accel_data; @@ -87,6 +88,7 @@ void insert_mip_sensor_raw_accel_data(struct mip_serializer* serializer, const m void extract_mip_sensor_raw_accel_data(struct mip_serializer* serializer, mip_sensor_raw_accel_data* self); bool extract_mip_sensor_raw_accel_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -98,7 +100,7 @@ bool extract_mip_sensor_raw_accel_data_from_field(const struct mip_field* field, struct mip_sensor_raw_gyro_data { - float raw_gyro[3]; ///< Native sensor counts + mip_vector3f raw_gyro; ///< Native sensor counts }; typedef struct mip_sensor_raw_gyro_data mip_sensor_raw_gyro_data; @@ -106,6 +108,7 @@ void insert_mip_sensor_raw_gyro_data(struct mip_serializer* serializer, const mi void extract_mip_sensor_raw_gyro_data(struct mip_serializer* serializer, mip_sensor_raw_gyro_data* self); bool extract_mip_sensor_raw_gyro_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -117,7 +120,7 @@ bool extract_mip_sensor_raw_gyro_data_from_field(const struct mip_field* field, struct mip_sensor_raw_mag_data { - float raw_mag[3]; ///< Native sensor counts + mip_vector3f raw_mag; ///< Native sensor counts }; typedef struct mip_sensor_raw_mag_data mip_sensor_raw_mag_data; @@ -125,6 +128,7 @@ void insert_mip_sensor_raw_mag_data(struct mip_serializer* serializer, const mip void extract_mip_sensor_raw_mag_data(struct mip_serializer* serializer, mip_sensor_raw_mag_data* self); bool extract_mip_sensor_raw_mag_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -144,6 +148,7 @@ void insert_mip_sensor_raw_pressure_data(struct mip_serializer* serializer, cons void extract_mip_sensor_raw_pressure_data(struct mip_serializer* serializer, mip_sensor_raw_pressure_data* self); bool extract_mip_sensor_raw_pressure_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -155,7 +160,7 @@ bool extract_mip_sensor_raw_pressure_data_from_field(const struct mip_field* fie struct mip_sensor_scaled_accel_data { - float scaled_accel[3]; ///< (x, y, z)[g] + mip_vector3f scaled_accel; ///< (x, y, z)[g] }; typedef struct mip_sensor_scaled_accel_data mip_sensor_scaled_accel_data; @@ -163,6 +168,7 @@ void insert_mip_sensor_scaled_accel_data(struct mip_serializer* serializer, cons void extract_mip_sensor_scaled_accel_data(struct mip_serializer* serializer, mip_sensor_scaled_accel_data* self); bool extract_mip_sensor_scaled_accel_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -174,7 +180,7 @@ bool extract_mip_sensor_scaled_accel_data_from_field(const struct mip_field* fie struct mip_sensor_scaled_gyro_data { - float scaled_gyro[3]; ///< (x, y, z) [radians/second] + mip_vector3f scaled_gyro; ///< (x, y, z) [radians/second] }; typedef struct mip_sensor_scaled_gyro_data mip_sensor_scaled_gyro_data; @@ -182,6 +188,7 @@ void insert_mip_sensor_scaled_gyro_data(struct mip_serializer* serializer, const void extract_mip_sensor_scaled_gyro_data(struct mip_serializer* serializer, mip_sensor_scaled_gyro_data* self); bool extract_mip_sensor_scaled_gyro_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -193,7 +200,7 @@ bool extract_mip_sensor_scaled_gyro_data_from_field(const struct mip_field* fiel struct mip_sensor_scaled_mag_data { - float scaled_mag[3]; ///< (x, y, z) [Gauss] + mip_vector3f scaled_mag; ///< (x, y, z) [Gauss] }; typedef struct mip_sensor_scaled_mag_data mip_sensor_scaled_mag_data; @@ -201,6 +208,7 @@ void insert_mip_sensor_scaled_mag_data(struct mip_serializer* serializer, const void extract_mip_sensor_scaled_mag_data(struct mip_serializer* serializer, mip_sensor_scaled_mag_data* self); bool extract_mip_sensor_scaled_mag_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -219,6 +227,7 @@ void insert_mip_sensor_scaled_pressure_data(struct mip_serializer* serializer, c void extract_mip_sensor_scaled_pressure_data(struct mip_serializer* serializer, mip_sensor_scaled_pressure_data* self); bool extract_mip_sensor_scaled_pressure_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -230,7 +239,7 @@ bool extract_mip_sensor_scaled_pressure_data_from_field(const struct mip_field* struct mip_sensor_delta_theta_data { - float delta_theta[3]; ///< (x, y, z) [radians] + mip_vector3f delta_theta; ///< (x, y, z) [radians] }; typedef struct mip_sensor_delta_theta_data mip_sensor_delta_theta_data; @@ -238,6 +247,7 @@ void insert_mip_sensor_delta_theta_data(struct mip_serializer* serializer, const void extract_mip_sensor_delta_theta_data(struct mip_serializer* serializer, mip_sensor_delta_theta_data* self); bool extract_mip_sensor_delta_theta_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -249,7 +259,7 @@ bool extract_mip_sensor_delta_theta_data_from_field(const struct mip_field* fiel struct mip_sensor_delta_velocity_data { - float delta_velocity[3]; ///< (x, y, z) [g*sec] + mip_vector3f delta_velocity; ///< (x, y, z) [g*sec] }; typedef struct mip_sensor_delta_velocity_data mip_sensor_delta_velocity_data; @@ -257,6 +267,7 @@ void insert_mip_sensor_delta_velocity_data(struct mip_serializer* serializer, co void extract_mip_sensor_delta_velocity_data(struct mip_serializer* serializer, mip_sensor_delta_velocity_data* self); bool extract_mip_sensor_delta_velocity_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -277,7 +288,7 @@ bool extract_mip_sensor_delta_velocity_data_from_field(const struct mip_field* f struct mip_sensor_comp_orientation_matrix_data { - float m[9]; ///< Matrix elements in row-major order. + mip_matrix3f m; ///< Matrix elements in row-major order. }; typedef struct mip_sensor_comp_orientation_matrix_data mip_sensor_comp_orientation_matrix_data; @@ -285,6 +296,7 @@ void insert_mip_sensor_comp_orientation_matrix_data(struct mip_serializer* seria void extract_mip_sensor_comp_orientation_matrix_data(struct mip_serializer* serializer, mip_sensor_comp_orientation_matrix_data* self); bool extract_mip_sensor_comp_orientation_matrix_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -303,7 +315,7 @@ bool extract_mip_sensor_comp_orientation_matrix_data_from_field(const struct mip struct mip_sensor_comp_quaternion_data { - float q[4]; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND + mip_quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND }; typedef struct mip_sensor_comp_quaternion_data mip_sensor_comp_quaternion_data; @@ -311,6 +323,7 @@ void insert_mip_sensor_comp_quaternion_data(struct mip_serializer* serializer, c void extract_mip_sensor_comp_quaternion_data(struct mip_serializer* serializer, mip_sensor_comp_quaternion_data* self); bool extract_mip_sensor_comp_quaternion_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -332,6 +345,7 @@ void insert_mip_sensor_comp_euler_angles_data(struct mip_serializer* serializer, void extract_mip_sensor_comp_euler_angles_data(struct mip_serializer* serializer, mip_sensor_comp_euler_angles_data* self); bool extract_mip_sensor_comp_euler_angles_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -342,7 +356,7 @@ bool extract_mip_sensor_comp_euler_angles_data_from_field(const struct mip_field struct mip_sensor_comp_orientation_update_matrix_data { - float m[9]; + mip_matrix3f m; }; typedef struct mip_sensor_comp_orientation_update_matrix_data mip_sensor_comp_orientation_update_matrix_data; @@ -350,6 +364,7 @@ void insert_mip_sensor_comp_orientation_update_matrix_data(struct mip_serializer void extract_mip_sensor_comp_orientation_update_matrix_data(struct mip_serializer* serializer, mip_sensor_comp_orientation_update_matrix_data* self); bool extract_mip_sensor_comp_orientation_update_matrix_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -368,6 +383,7 @@ void insert_mip_sensor_orientation_raw_temp_data(struct mip_serializer* serializ void extract_mip_sensor_orientation_raw_temp_data(struct mip_serializer* serializer, mip_sensor_orientation_raw_temp_data* self); bool extract_mip_sensor_orientation_raw_temp_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -386,6 +402,7 @@ void insert_mip_sensor_internal_timestamp_data(struct mip_serializer* serializer void extract_mip_sensor_internal_timestamp_data(struct mip_serializer* serializer, mip_sensor_internal_timestamp_data* self); bool extract_mip_sensor_internal_timestamp_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -405,6 +422,7 @@ void insert_mip_sensor_pps_timestamp_data(struct mip_serializer* serializer, con void extract_mip_sensor_pps_timestamp_data(struct mip_serializer* serializer, mip_sensor_pps_timestamp_data* self); bool extract_mip_sensor_pps_timestamp_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -443,6 +461,7 @@ bool extract_mip_sensor_gps_timestamp_data_from_field(const struct mip_field* fi void insert_mip_sensor_gps_timestamp_data_valid_flags(struct mip_serializer* serializer, const mip_sensor_gps_timestamp_data_valid_flags self); void extract_mip_sensor_gps_timestamp_data_valid_flags(struct mip_serializer* serializer, mip_sensor_gps_timestamp_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -467,6 +486,7 @@ void insert_mip_sensor_temperature_abs_data(struct mip_serializer* serializer, c void extract_mip_sensor_temperature_abs_data(struct mip_serializer* serializer, mip_sensor_temperature_abs_data* self); bool extract_mip_sensor_temperature_abs_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -483,7 +503,7 @@ bool extract_mip_sensor_temperature_abs_data_from_field(const struct mip_field* struct mip_sensor_up_vector_data { - float up[3]; ///< [Gs] + mip_vector3f up; ///< [Gs] }; typedef struct mip_sensor_up_vector_data mip_sensor_up_vector_data; @@ -491,6 +511,7 @@ void insert_mip_sensor_up_vector_data(struct mip_serializer* serializer, const m void extract_mip_sensor_up_vector_data(struct mip_serializer* serializer, mip_sensor_up_vector_data* self); bool extract_mip_sensor_up_vector_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -504,7 +525,7 @@ bool extract_mip_sensor_up_vector_data_from_field(const struct mip_field* field, struct mip_sensor_north_vector_data { - float north[3]; ///< [Gauss] + mip_vector3f north; ///< [Gauss] }; typedef struct mip_sensor_north_vector_data mip_sensor_north_vector_data; @@ -512,6 +533,7 @@ void insert_mip_sensor_north_vector_data(struct mip_serializer* serializer, cons void extract_mip_sensor_north_vector_data(struct mip_serializer* serializer, mip_sensor_north_vector_data* self); bool extract_mip_sensor_north_vector_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -546,6 +568,7 @@ bool extract_mip_sensor_overrange_status_data_from_field(const struct mip_field* void insert_mip_sensor_overrange_status_data_status(struct mip_serializer* serializer, const mip_sensor_overrange_status_data_status self); void extract_mip_sensor_overrange_status_data_status(struct mip_serializer* serializer, mip_sensor_overrange_status_data_status* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -565,6 +588,7 @@ void insert_mip_sensor_odometer_data_data(struct mip_serializer* serializer, con void extract_mip_sensor_odometer_data_data(struct mip_serializer* serializer, mip_sensor_odometer_data_data* self); bool extract_mip_sensor_odometer_data_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 7ed980dd1..f645fc38c 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -78,22 +79,22 @@ enum struct RawAccel { + Vector3f raw_accel; ///< Native sensor counts + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(raw_accel[0],raw_accel[1],raw_accel[2]); } - float raw_accel[3] = {0}; ///< Native sensor counts - }; void insert(Serializer& serializer, const RawAccel& self); void extract(Serializer& serializer, RawAccel& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -105,22 +106,22 @@ void extract(Serializer& serializer, RawAccel& self); struct RawGyro { + Vector3f raw_gyro; ///< Native sensor counts + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(raw_gyro[0],raw_gyro[1],raw_gyro[2]); } - float raw_gyro[3] = {0}; ///< Native sensor counts - }; void insert(Serializer& serializer, const RawGyro& self); void extract(Serializer& serializer, RawGyro& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -132,22 +133,22 @@ void extract(Serializer& serializer, RawGyro& self); struct RawMag { + Vector3f raw_mag; ///< Native sensor counts + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(raw_mag[0],raw_mag[1],raw_mag[2]); } - float raw_mag[3] = {0}; ///< Native sensor counts - }; void insert(Serializer& serializer, const RawMag& self); void extract(Serializer& serializer, RawMag& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -159,22 +160,22 @@ void extract(Serializer& serializer, RawMag& self); struct RawPressure { + float raw_pressure = 0; ///< Native sensor counts + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(raw_pressure); } - float raw_pressure = 0; ///< Native sensor counts - }; void insert(Serializer& serializer, const RawPressure& self); void extract(Serializer& serializer, RawPressure& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -186,22 +187,22 @@ void extract(Serializer& serializer, RawPressure& self); struct ScaledAccel { + Vector3f scaled_accel; ///< (x, y, z)[g] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scaled_accel[0],scaled_accel[1],scaled_accel[2]); } - float scaled_accel[3] = {0}; ///< (x, y, z)[g] - }; void insert(Serializer& serializer, const ScaledAccel& self); void extract(Serializer& serializer, ScaledAccel& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -213,22 +214,22 @@ void extract(Serializer& serializer, ScaledAccel& self); struct ScaledGyro { + Vector3f scaled_gyro; ///< (x, y, z) [radians/second] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scaled_gyro[0],scaled_gyro[1],scaled_gyro[2]); } - float scaled_gyro[3] = {0}; ///< (x, y, z) [radians/second] - }; void insert(Serializer& serializer, const ScaledGyro& self); void extract(Serializer& serializer, ScaledGyro& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -240,22 +241,22 @@ void extract(Serializer& serializer, ScaledGyro& self); struct ScaledMag { + Vector3f scaled_mag; ///< (x, y, z) [Gauss] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scaled_mag[0],scaled_mag[1],scaled_mag[2]); } - float scaled_mag[3] = {0}; ///< (x, y, z) [Gauss] - }; void insert(Serializer& serializer, const ScaledMag& self); void extract(Serializer& serializer, ScaledMag& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -266,22 +267,22 @@ void extract(Serializer& serializer, ScaledMag& self); struct ScaledPressure { + float scaled_pressure = 0; ///< [mBar] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(scaled_pressure); } - float scaled_pressure = 0; ///< [mBar] - }; void insert(Serializer& serializer, const ScaledPressure& self); void extract(Serializer& serializer, ScaledPressure& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -293,22 +294,22 @@ void extract(Serializer& serializer, ScaledPressure& self); struct DeltaTheta { + Vector3f delta_theta; ///< (x, y, z) [radians] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(delta_theta[0],delta_theta[1],delta_theta[2]); } - float delta_theta[3] = {0}; ///< (x, y, z) [radians] - }; void insert(Serializer& serializer, const DeltaTheta& self); void extract(Serializer& serializer, DeltaTheta& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -320,22 +321,22 @@ void extract(Serializer& serializer, DeltaTheta& self); struct DeltaVelocity { + Vector3f delta_velocity; ///< (x, y, z) [g*sec] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(delta_velocity[0],delta_velocity[1],delta_velocity[2]); } - float delta_velocity[3] = {0}; ///< (x, y, z) [g*sec] - }; void insert(Serializer& serializer, const DeltaVelocity& self); void extract(Serializer& serializer, DeltaVelocity& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -356,22 +357,22 @@ void extract(Serializer& serializer, DeltaVelocity& self); struct CompOrientationMatrix { + Matrix3f m; ///< Matrix elements in row-major order. + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } - float m[9] = {0}; ///< Matrix elements in row-major order. - }; void insert(Serializer& serializer, const CompOrientationMatrix& self); void extract(Serializer& serializer, CompOrientationMatrix& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -390,22 +391,22 @@ void extract(Serializer& serializer, CompOrientationMatrix& self); struct CompQuaternion { + Quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(q[0],q[1],q[2],q[3]); } - float q[4] = {0}; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND - }; void insert(Serializer& serializer, const CompQuaternion& self); void extract(Serializer& serializer, CompQuaternion& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -417,24 +418,24 @@ void extract(Serializer& serializer, CompQuaternion& self); struct CompEulerAngles { + float roll = 0; ///< [radians] + float pitch = 0; ///< [radians] + float yaw = 0; ///< [radians] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(roll,pitch,yaw); } - float roll = 0; ///< [radians] - float pitch = 0; ///< [radians] - float yaw = 0; ///< [radians] - }; void insert(Serializer& serializer, const CompEulerAngles& self); void extract(Serializer& serializer, CompEulerAngles& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -445,22 +446,22 @@ void extract(Serializer& serializer, CompEulerAngles& self); struct CompOrientationUpdateMatrix { + Matrix3f m; + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } - float m[9] = {0}; - }; void insert(Serializer& serializer, const CompOrientationUpdateMatrix& self); void extract(Serializer& serializer, CompOrientationUpdateMatrix& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -471,22 +472,22 @@ void extract(Serializer& serializer, CompOrientationUpdateMatrix& self); struct OrientationRawTemp { + uint16_t raw_temp[4] = {0}; + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { - return std::make_tuple(raw_temp[0],raw_temp[1],raw_temp[2],raw_temp[3]); + return std::make_tuple(raw_temp); } - uint16_t raw_temp[4] = {0}; - }; void insert(Serializer& serializer, const OrientationRawTemp& self); void extract(Serializer& serializer, OrientationRawTemp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -497,22 +498,22 @@ void extract(Serializer& serializer, OrientationRawTemp& self); struct InternalTimestamp { + uint32_t counts = 0; + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(counts); } - uint32_t counts = 0; - }; void insert(Serializer& serializer, const InternalTimestamp& self); void extract(Serializer& serializer, InternalTimestamp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -523,23 +524,23 @@ void extract(Serializer& serializer, InternalTimestamp& self); struct PpsTimestamp { + uint32_t seconds = 0; + uint32_t useconds = 0; + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(seconds,useconds); } - uint32_t seconds = 0; - uint32_t useconds = 0; - }; void insert(Serializer& serializer, const PpsTimestamp& self); void extract(Serializer& serializer, PpsTimestamp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -556,16 +557,6 @@ void extract(Serializer& serializer, PpsTimestamp& self); struct GpsTimestamp { - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(tow,week_number,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -607,10 +598,20 @@ struct GpsTimestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; + + + auto as_tuple() const + { + return std::make_tuple(tow,week_number,valid_flags); + } + }; void insert(Serializer& serializer, const GpsTimestamp& self); void extract(Serializer& serializer, GpsTimestamp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -625,24 +626,24 @@ void extract(Serializer& serializer, GpsTimestamp& self); struct TemperatureAbs { + float min_temp = 0; ///< [degC] + float max_temp = 0; ///< [degC] + float mean_temp = 0; ///< [degC] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(min_temp,max_temp,mean_temp); } - float min_temp = 0; ///< [degC] - float max_temp = 0; ///< [degC] - float mean_temp = 0; ///< [degC] - }; void insert(Serializer& serializer, const TemperatureAbs& self); void extract(Serializer& serializer, TemperatureAbs& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -659,22 +660,22 @@ void extract(Serializer& serializer, TemperatureAbs& self); struct UpVector { + Vector3f up; ///< [Gs] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(up[0],up[1],up[2]); } - float up[3] = {0}; ///< [Gs] - }; void insert(Serializer& serializer, const UpVector& self); void extract(Serializer& serializer, UpVector& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -688,22 +689,22 @@ void extract(Serializer& serializer, UpVector& self); struct NorthVector { + Vector3f north; ///< [Gauss] + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(north[0],north[1],north[2]); } - float north[3] = {0}; ///< [Gauss] - }; void insert(Serializer& serializer, const NorthVector& self); void extract(Serializer& serializer, NorthVector& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -713,16 +714,6 @@ void extract(Serializer& serializer, NorthVector& self); struct OverrangeStatus { - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(status); - } - struct Status : Bitfield { enum _enumType : uint16_t @@ -777,10 +768,20 @@ struct OverrangeStatus Status status; + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; + + + auto as_tuple() const + { + return std::make_tuple(status); + } + }; void insert(Serializer& serializer, const OverrangeStatus& self); void extract(Serializer& serializer, OverrangeStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -790,24 +791,24 @@ void extract(Serializer& serializer, OverrangeStatus& self); struct OdometerData { + float speed = 0; ///< Average speed over the time interval [m/s]. Can be negative for quadrature encoders. + float uncertainty = 0; ///< Uncertainty of velocity [m/s]. + uint16_t valid_flags = 0; ///< If odometer is configured, bit 0 will be set to 1. + static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(speed,uncertainty,valid_flags); } - float speed = 0; ///< Average speed over the time interval [m/s]. Can be negative for quadrature encoders. - float uncertainty = 0; ///< Uncertainty of velocity [m/s]. - uint16_t valid_flags = 0; ///< If odometer is configured, bit 0 will be set to 1. - }; void insert(Serializer& serializer, const OdometerData& self); void extract(Serializer& serializer, OdometerData& self); + ///@} /// diff --git a/src/mip/definitions/data_shared.h b/src/mip/definitions/data_shared.h index 90f2dbc6e..6c4a0f404 100644 --- a/src/mip/definitions/data_shared.h +++ b/src/mip/definitions/data_shared.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -74,6 +75,7 @@ void insert_mip_shared_event_source_data(struct mip_serializer* serializer, cons void extract_mip_shared_event_source_data(struct mip_serializer* serializer, mip_shared_event_source_data* self); bool extract_mip_shared_event_source_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -95,6 +97,7 @@ void insert_mip_shared_ticks_data(struct mip_serializer* serializer, const mip_s void extract_mip_shared_ticks_data(struct mip_serializer* serializer, mip_shared_ticks_data* self); bool extract_mip_shared_ticks_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -117,6 +120,7 @@ void insert_mip_shared_delta_ticks_data(struct mip_serializer* serializer, const void extract_mip_shared_delta_ticks_data(struct mip_serializer* serializer, mip_shared_delta_ticks_data* self); bool extract_mip_shared_delta_ticks_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -150,6 +154,7 @@ bool extract_mip_shared_gps_timestamp_data_from_field(const struct mip_field* fi void insert_mip_shared_gps_timestamp_data_valid_flags(struct mip_serializer* serializer, const mip_shared_gps_timestamp_data_valid_flags self); void extract_mip_shared_gps_timestamp_data_valid_flags(struct mip_serializer* serializer, mip_shared_gps_timestamp_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -177,6 +182,7 @@ void insert_mip_shared_delta_time_data(struct mip_serializer* serializer, const void extract_mip_shared_delta_time_data(struct mip_serializer* serializer, mip_shared_delta_time_data* self); bool extract_mip_shared_delta_time_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -202,6 +208,7 @@ void insert_mip_shared_reference_timestamp_data(struct mip_serializer* serialize void extract_mip_shared_reference_timestamp_data(struct mip_serializer* serializer, mip_shared_reference_timestamp_data* self); bool extract_mip_shared_reference_timestamp_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -229,6 +236,7 @@ void insert_mip_shared_reference_time_delta_data(struct mip_serializer* serializ void extract_mip_shared_reference_time_delta_data(struct mip_serializer* serializer, mip_shared_reference_time_delta_data* self); bool extract_mip_shared_reference_time_delta_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -264,6 +272,7 @@ bool extract_mip_shared_external_timestamp_data_from_field(const struct mip_fiel void insert_mip_shared_external_timestamp_data_valid_flags(struct mip_serializer* serializer, const mip_shared_external_timestamp_data_valid_flags self); void extract_mip_shared_external_timestamp_data_valid_flags(struct mip_serializer* serializer, mip_shared_external_timestamp_data_valid_flags* self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -303,6 +312,7 @@ bool extract_mip_shared_external_time_delta_data_from_field(const struct mip_fie void insert_mip_shared_external_time_delta_data_valid_flags(struct mip_serializer* serializer, const mip_shared_external_time_delta_data_valid_flags self); void extract_mip_shared_external_time_delta_data_valid_flags(struct mip_serializer* serializer, mip_shared_external_time_delta_data_valid_flags* self); + ///@} /// diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index fef0ff682..633904ce6 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -65,22 +66,22 @@ static const uint8_t MIP_DATA_DESC_SHARED_START = 0xD0; struct EventSource { + uint8_t trigger_id = 0; ///< Trigger ID number. If 0, this message was emitted due to being scheduled in the 3DM Message Format Command (0x0C,0x0F). + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(trigger_id); } - uint8_t trigger_id = 0; ///< Trigger ID number. If 0, this message was emitted due to being scheduled in the 3DM Message Format Command (0x0C,0x0F). - }; void insert(Serializer& serializer, const EventSource& self); void extract(Serializer& serializer, EventSource& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -94,22 +95,22 @@ void extract(Serializer& serializer, EventSource& self); struct Ticks { + uint32_t ticks = 0; ///< Ticks since powerup. + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(ticks); } - uint32_t ticks = 0; ///< Ticks since powerup. - }; void insert(Serializer& serializer, const Ticks& self); void extract(Serializer& serializer, Ticks& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -124,22 +125,22 @@ void extract(Serializer& serializer, Ticks& self); struct DeltaTicks { + uint32_t ticks = 0; ///< Ticks since last output. + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(ticks); } - uint32_t ticks = 0; ///< Ticks since last output. - }; void insert(Serializer& serializer, const DeltaTicks& self); void extract(Serializer& serializer, DeltaTicks& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -153,16 +154,6 @@ void extract(Serializer& serializer, DeltaTicks& self); struct GpsTimestamp { - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(tow,week_number,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -198,10 +189,20 @@ struct GpsTimestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; + + + auto as_tuple() const + { + return std::make_tuple(tow,week_number,valid_flags); + } + }; void insert(Serializer& serializer, const GpsTimestamp& self); void extract(Serializer& serializer, GpsTimestamp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -221,22 +222,22 @@ void extract(Serializer& serializer, GpsTimestamp& self); struct DeltaTime { + double seconds = 0; ///< Seconds since last output. + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(seconds); } - double seconds = 0; ///< Seconds since last output. - }; void insert(Serializer& serializer, const DeltaTime& self); void extract(Serializer& serializer, DeltaTime& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -254,22 +255,22 @@ void extract(Serializer& serializer, DeltaTime& self); struct ReferenceTimestamp { + uint64_t nanoseconds = 0; ///< Nanoseconds since initialization. + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(nanoseconds); } - uint64_t nanoseconds = 0; ///< Nanoseconds since initialization. - }; void insert(Serializer& serializer, const ReferenceTimestamp& self); void extract(Serializer& serializer, ReferenceTimestamp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -289,22 +290,22 @@ void extract(Serializer& serializer, ReferenceTimestamp& self); struct ReferenceTimeDelta { + uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(dt_nanos); } - uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. - }; void insert(Serializer& serializer, const ReferenceTimeDelta& self); void extract(Serializer& serializer, ReferenceTimeDelta& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -323,16 +324,6 @@ void extract(Serializer& serializer, ReferenceTimeDelta& self); struct ExternalTimestamp { - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(nanoseconds,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -361,10 +352,20 @@ struct ExternalTimestamp uint64_t nanoseconds = 0; ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; + + + auto as_tuple() const + { + return std::make_tuple(nanoseconds,valid_flags); + } + }; void insert(Serializer& serializer, const ExternalTimestamp& self); void extract(Serializer& serializer, ExternalTimestamp& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -387,16 +388,6 @@ void extract(Serializer& serializer, ExternalTimestamp& self); struct ExternalTimeDelta { - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; - - static const bool HAS_FUNCTION_SELECTOR = false; - - auto as_tuple() const - { - return std::make_tuple(dt_nanos,valid_flags); - } - struct ValidFlags : Bitfield { enum _enumType : uint16_t @@ -425,10 +416,20 @@ struct ExternalTimeDelta uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. ValidFlags valid_flags; + static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; + + + auto as_tuple() const + { + return std::make_tuple(dt_nanos,valid_flags); + } + }; void insert(Serializer& serializer, const ExternalTimeDelta& self); void extract(Serializer& serializer, ExternalTimeDelta& self); + ///@} /// diff --git a/src/mip/definitions/data_system.h b/src/mip/definitions/data_system.h index 5277a4d17..e55e0b8bb 100644 --- a/src/mip/definitions/data_system.h +++ b/src/mip/definitions/data_system.h @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -82,6 +83,7 @@ void insert_mip_system_built_in_test_data(struct mip_serializer* serializer, con void extract_mip_system_built_in_test_data(struct mip_serializer* serializer, mip_system_built_in_test_data* self); bool extract_mip_system_built_in_test_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -101,6 +103,7 @@ void insert_mip_system_time_sync_status_data(struct mip_serializer* serializer, void extract_mip_system_time_sync_status_data(struct mip_serializer* serializer, mip_system_time_sync_status_data* self); bool extract_mip_system_time_sync_status_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -137,6 +140,7 @@ void insert_mip_system_gpio_state_data(struct mip_serializer* serializer, const void extract_mip_system_gpio_state_data(struct mip_serializer* serializer, mip_system_gpio_state_data* self); bool extract_mip_system_gpio_state_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -157,6 +161,7 @@ void insert_mip_system_gpio_analog_value_data(struct mip_serializer* serializer, void extract_mip_system_gpio_analog_value_data(struct mip_serializer* serializer, mip_system_gpio_analog_value_data* self); bool extract_mip_system_gpio_analog_value_data_from_field(const struct mip_field* field, void* ptr); + ///@} /// diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 84e42294b..2f45d70f7 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -1,5 +1,6 @@ #pragma once +#include "common.h" #include "descriptors.h" #include "../mip_result.h" @@ -73,22 +74,22 @@ enum struct BuiltInTest { + uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. + static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(result); } - uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. - }; void insert(Serializer& serializer, const BuiltInTest& self); void extract(Serializer& serializer, BuiltInTest& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -99,23 +100,23 @@ void extract(Serializer& serializer, BuiltInTest& self); struct TimeSyncStatus { + bool time_sync = 0; ///< True if sync with the PPS signal is currently valid. False if PPS feature is disabled or a PPS signal is not detected. + uint8_t last_pps_rcvd = 0; ///< Elapsed time in seconds since last PPS was received, with a maximum value of 255. + static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(time_sync,last_pps_rcvd); } - bool time_sync = 0; ///< True if sync with the PPS signal is currently valid. False if PPS feature is disabled or a PPS signal is not detected. - uint8_t last_pps_rcvd = 0; ///< Elapsed time in seconds since last PPS was received, with a maximum value of 255. - }; void insert(Serializer& serializer, const TimeSyncStatus& self); void extract(Serializer& serializer, TimeSyncStatus& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -144,22 +145,22 @@ void extract(Serializer& serializer, TimeSyncStatus& self); struct GpioState { + uint8_t states = 0; ///< Bitfield containing the states for each GPIO pin.
Bit 0 (0x01): pin 1
Bit 1 (0x02): pin 2
Bit 2 (0x04): pin 3
Bit 3 (0x08): pin 4
Bits for pins that don't exist will read as 0. + static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(states); } - uint8_t states = 0; ///< Bitfield containing the states for each GPIO pin.
Bit 0 (0x01): pin 1
Bit 1 (0x02): pin 2
Bit 2 (0x04): pin 3
Bit 3 (0x08): pin 4
Bits for pins that don't exist will read as 0. - }; void insert(Serializer& serializer, const GpioState& self); void extract(Serializer& serializer, GpioState& self); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -171,23 +172,23 @@ void extract(Serializer& serializer, GpioState& self); struct GpioAnalogValue { + uint8_t gpio_id = 0; ///< GPIO pin number starting with 1. + float value = 0; ///< Value of the GPIO line in scaled volts. + static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; - static const bool HAS_FUNCTION_SELECTOR = false; auto as_tuple() const { return std::make_tuple(gpio_id,value); } - uint8_t gpio_id = 0; ///< GPIO pin number starting with 1. - float value = 0; ///< Value of the GPIO line in scaled volts. - }; void insert(Serializer& serializer, const GpioAnalogValue& self); void extract(Serializer& serializer, GpioAnalogValue& self); + ///@} /// From 5ad911fc8c73766a3fc4ffcdb6a5291064cd5fe2 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Mon, 17 Jul 2023 14:47:53 -0400 Subject: [PATCH 081/252] Added support for high/non-standard baudrates on Mac --- src/mip/utils/serial_port.c | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 358a2a925..04b426cf1 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -3,6 +3,11 @@ #include "serial_port.h" +#ifdef __APPLE__ +#include +#endif + + #define COM_PORT_BUFFER_SIZE 0x200 #ifndef WIN32 //Unix only @@ -165,6 +170,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) return false; } +#ifndef __APPLE__ if (cfsetispeed(&serial_port_settings, baud_rate_to_speed(baudrate)) < 0 || cfsetospeed(&serial_port_settings, baud_rate_to_speed(baudrate)) < 0) { MIP_LOG_ERROR("Unable to set baud rate (%d): %s\n", errno, strerror(errno)); @@ -172,6 +178,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) port->handle = -1; return false; } +#endif // Other serial settings to match MSCL serial_port_settings.c_cflag |= (tcflag_t)(CLOCAL | CREAD); @@ -192,6 +199,18 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) return false; } + +#ifdef __APPLE__ + speed_t speed = baudrate; + if (ioctl(port->handle, IOSSIOSPEED, &speed) < 0) + { + MIP_LOG_ERROR("Unable to set baud rate (%d): %s\n", errno, strerror(errno)); + close(port->handle); + port->handle = -1; + return false; + } +#endif + // Flush any waiting data tcflush(port->handle, TCIOFLUSH); From fc41cd05294b27ce9a19eb3c12d4106f09934cd0 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 17 Jul 2023 18:16:36 -0400 Subject: [PATCH 082/252] Add tcp_socket_init and tcp_socket_is_open to match serial port. --- src/mip/platform/tcp_connection.cpp | 22 ++++++++++------------ src/mip/platform/tcp_connection.hpp | 1 - src/mip/utils/tcp_socket.c | 12 +++++++++++- src/mip/utils/tcp_socket.h | 2 ++ 4 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/mip/platform/tcp_connection.cpp b/src/mip/platform/tcp_connection.cpp index d4d99dd7f..8131a465c 100644 --- a/src/mip/platform/tcp_connection.cpp +++ b/src/mip/platform/tcp_connection.cpp @@ -19,42 +19,40 @@ TcpConnection::TcpConnection(const std::string& hostname, uint16_t port) mHostname = hostname; mPort = port; mType = TYPE; + + tcp_socket_init(&mSocket); } ///@brief Closes the underlying TCP socket TcpConnection::~TcpConnection() { - if(mConnected) + if(isConnected()) TcpConnection::disconnect(); } ///@brief Check if the socket is connected bool TcpConnection::isConnected() const { - return mConnected; + return tcp_socket_is_open(&mSocket); } ///@brief Connect to the socket bool TcpConnection::connect() { - if(mConnected) - return false; - - mConnected = tcp_socket_open(&mSocket, mHostname.c_str(), mPort, 3000); + if(isConnected()) + return true; - return mConnected; + return tcp_socket_open(&mSocket, mHostname.c_str(), mPort, 3000); } ///@brief Disconnect from the socket bool TcpConnection::disconnect() { - if(!mConnected) - return false; - - mConnected = tcp_socket_close(&mSocket); + if(!isConnected()) + return true; - return mConnected; + return tcp_socket_close(&mSocket); } ///@copydoc mip::Connection::sendToDevice diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index 591dc8634..e8a506269 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -44,7 +44,6 @@ class TcpConnection : public mip::Connection tcp_socket mSocket; std::string mHostname; uint16_t mPort = 0; - bool mConnected = false; }; ///@} diff --git a/src/mip/utils/tcp_socket.c b/src/mip/utils/tcp_socket.c index fe8d77620..f6a91195d 100644 --- a/src/mip/utils/tcp_socket.c +++ b/src/mip/utils/tcp_socket.c @@ -27,9 +27,19 @@ static const int SEND_FLAGS = MSG_NOSIGNAL; #include -static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, uint16_t port, unsigned int timeout_ms) +void tcp_socket_init(tcp_socket* socket_ptr) { socket_ptr->handle = INVALID_SOCKET; +} + +bool tcp_socket_is_open(const tcp_socket* socket_ptr) +{ + return socket_ptr->handle != INVALID_SOCKET; +} + +static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, uint16_t port, unsigned int timeout_ms) +{ + //assert(socket_ptr->handle == INVALID_SOCKET); // https://man7.org/linux/man-pages/man3/getaddrinfo.3.html struct addrinfo hints, *info; diff --git a/src/mip/utils/tcp_socket.h b/src/mip/utils/tcp_socket.h index 7260cce88..62827286c 100644 --- a/src/mip/utils/tcp_socket.h +++ b/src/mip/utils/tcp_socket.h @@ -41,6 +41,8 @@ typedef struct tcp_socket +void tcp_socket_init(tcp_socket* socket_ptr); +bool tcp_socket_is_open(const tcp_socket* socket_ptr); bool tcp_socket_open(tcp_socket* socket_ptr, const char* hostname, uint16_t port, unsigned int timeout_ms); bool tcp_socket_close(tcp_socket* socket_ptr); bool tcp_socket_send(tcp_socket* socket_ptr, const void* buffer, size_t num_bytes, size_t* bytes_written); From 4dabdfe382c4ff5b4d854867921fc49f0b699fee Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 17 Jul 2023 18:17:51 -0400 Subject: [PATCH 083/252] Fix connection not being opened in examples. --- examples/example_utils.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index e0d010cdc..c4bde773b 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -99,11 +99,16 @@ std::unique_ptr openFromArgs(const std::string& port_or_hostname, using SerialConnection = mip::platform::SerialConnection; example_utils->connection = std::unique_ptr(new SerialConnection(port_or_hostname, baud)); #endif // MIP_USE_EXTRAS + example_utils->device = std::unique_ptr(new mip::DeviceInterface(example_utils->connection.get(), example_utils->buffer, sizeof(example_utils->buffer), mip::C::mip_timeout_from_baudrate(baud), 500)); #else // MIP_USE_SERIAL throw std::runtime_error("This program was compiled without serial support. Recompile with -DMIP_USE_SERIAL=1.\n"); #endif //MIP_USE_SERIAL } + + if( !example_utils->connection->connect() ) + throw std::runtime_error("Failed to open the connection"); + return example_utils; } From f2ca47905df293b714fcddcecb3af4142c64c5cc Mon Sep 17 00:00:00 2001 From: Samuel Moody <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 17 Jul 2023 18:34:18 -0400 Subject: [PATCH 084/252] Fix build errors due to windows.h. --- src/mip/utils/serial_port.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/mip/utils/serial_port.h b/src/mip/utils/serial_port.h index 8f074afdd..a56b0c259 100644 --- a/src/mip/utils/serial_port.h +++ b/src/mip/utils/serial_port.h @@ -1,6 +1,10 @@ #pragma once #ifdef WIN32 +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif + #include #else #include From 2d038cc3d25693a30396fdeba1e61c64a18675fe Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 18 Jul 2023 12:09:49 -0400 Subject: [PATCH 085/252] Create PacketBuf class and rename Packet to PacketRef. --- examples/threading.cpp | 2 +- examples/watch_imu.cpp | 2 +- src/mip/mip.hpp | 112 ++++++++++++++++++++++++++++++++++------- src/mip/mip_device.hpp | 36 ++++++------- test/mip/test_mip.cpp | 4 +- 5 files changed, 113 insertions(+), 43 deletions(-) diff --git a/examples/threading.cpp b/examples/threading.cpp index 21b4a178d..33eeb93af 100644 --- a/examples/threading.cpp +++ b/examples/threading.cpp @@ -41,7 +41,7 @@ unsigned int display_progress() return count; } -void packet_callback(void*, const mip::Packet& packet, mip::Timestamp timestamp) +void packet_callback(void*, const mip::PacketRef& packet, mip::Timestamp timestamp) { numSamples++; } diff --git a/examples/watch_imu.cpp b/examples/watch_imu.cpp index 51beba8b9..c071d73f4 100644 --- a/examples/watch_imu.cpp +++ b/examples/watch_imu.cpp @@ -19,7 +19,7 @@ mip::data_sensor::ScaledAccel scaled_accel; -void handlePacket(void*, const mip::Packet& packet, mip::Timestamp timestamp) +void handlePacket(void*, const mip::PacketRef& packet, mip::Timestamp timestamp) { // if(packet.descriptorSet() != mip::MIP_SENSOR_DATA_DESC_SET) // return; diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 07c1dbd62..4bf85c1da 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -121,7 +121,7 @@ class Field : public C::mip_field //////////////////////////////////////////////////////////////////////////////// -///@brief C++ class representing a MIP Packet. +///@brief C++ class representing a MIP PacketRef. /// /// This is a thin wrapper around the mip_packet C structure. Like the C /// version, it does not contain or own the data buffer. Any of the C functions @@ -130,24 +130,24 @@ class Field : public C::mip_field /// Fields may be iterated over using the C-style methods, with an iterator, or /// with a range-based for loop: ///@code{.cpp} -/// for(Packet::Iterator iter = packet.begin(); iter != packet.end(); ++iter) { ... } +/// for(PacketRef::Iterator iter = packet.begin(); iter != packet.end(); ++iter) { ... } /// for(Field field : packet) { ... } ///@endcode /// -class Packet : public C::mip_packet +class PacketRef : public C::mip_packet { public: class FieldIterator; public: ///@copydoc mip::C::mip_packet_create - Packet(uint8_t* buffer, size_t bufferSize, uint8_t descriptorSet) { C::mip_packet_create(this, buffer, bufferSize, descriptorSet); } + PacketRef(uint8_t* buffer, size_t bufferSize, uint8_t descriptorSet) { C::mip_packet_create(this, buffer, bufferSize, descriptorSet); } ///@copydoc mip_packet_from_buffer - Packet(uint8_t* buffer, size_t length) { C::mip_packet_from_buffer(this, buffer, length); } - /// Constructs a C++ %Packet class from the base C object. - Packet(const C::mip_packet* other) { std::memcpy(static_cast(this), other, sizeof(*this)); } - /// Constructs a C++ %Packet class from the base C object. - Packet(const C::mip_packet& other) { std::memcpy(static_cast(this), &other, sizeof(*this)); } + PacketRef(uint8_t* buffer, size_t length) { C::mip_packet_from_buffer(this, buffer, length); } + /// Constructs a C++ %PacketRef class from the base C object. + PacketRef(const C::mip_packet* other) { std::memcpy(static_cast(this), other, sizeof(*this)); } + /// Constructs a C++ %PacketRef class from the base C object. + PacketRef(const C::mip_packet& other) { std::memcpy(static_cast(this), &other, sizeof(*this)); } // @@ -235,7 +235,7 @@ class Packet : public C::mip_packet return serializer.isOk(); } - ///@brief Creates a new Packet containing a single MIP field from an instance of the field type. + ///@brief Creates a new PacketRef containing a single MIP field from an instance of the field type. /// /// This works just like the addField() function but also initializes and finalizes the packet. /// It is assumed that the field will fit in an empty packet; otherwise the field can't ever be used. @@ -248,14 +248,14 @@ class Packet : public C::mip_packet ///@param field Instance of the field to add to the packet. ///@param fieldDescriptor If specified, overrides the field descriptor. /// - ///@returns A Packet object containing the field. + ///@returns A PacketRef object containing the field. /// template - static Packet createFromField(uint8_t* buffer, size_t bufferSize, const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR) + static PacketRef createFromField(uint8_t* buffer, size_t bufferSize, const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR) { if( fieldDescriptor == INVALID_FIELD_DESCRIPTOR ) fieldDescriptor = FieldType::FIELD_DESCRIPTOR; - Packet packet(buffer, bufferSize, FieldType::DESCRIPTOR_SET); + PacketRef packet(buffer, bufferSize, FieldType::DESCRIPTOR_SET); packet.addField(field, fieldDescriptor); packet.finalize(); return packet; @@ -264,7 +264,7 @@ class Packet : public C::mip_packet /// Iterator class for use with the range-based for loop or iterators. /// - /// You should generally use the begin()/end() functions on the Packet + /// You should generally use the begin()/end() functions on the PacketRef /// class instead of using this class directly. /// class FieldIterator @@ -306,6 +306,80 @@ class Packet : public C::mip_packet }; +//////////////////////////////////////////////////////////////////////////////// +///@brief A mip packet with a self-contained buffer (useful with std::vector). +/// +template +class SizedPacketBuf : public PacketRef +{ +public: + SizedPacketBuf(uint8_t descriptorSet) : PacketRef(mData, sizeof(mData), descriptorSet) {} + + ///@brief Creates a PacketBuf by copying existing data. + /// + explicit SizedPacketBuf(const uint8_t* data, size_t length) : PacketRef(mData, sizeof(mData)) { copyFrom(data, length); } + explicit SizedPacketBuf(const PacketRef& packet) : PacketRef(mData, sizeof(mData)) { copyFrom(packet); } + + // No copying or moving allowed - use the explicit functions copyFrom or copyTo. + SizedPacketBuf(const SizedPacketBuf& other) = delete; + SizedPacketBuf(SizedPacketBuf&&) = delete; + SizedPacketBuf& operator=(const SizedPacketBuf& other) = delete; + SizedPacketBuf& operator=(SizedPacketBuf&&) = delete; + + ///@brief Create a packet containing just the given field. + /// + ///@tparam FieldType Type of the MIP field. This can't be explicitly specified due to being a constructor. + /// + ///@param field The field object to serialize. + ///@param fieldDescriptor If specified (not INVALID_FIELD_DESCRIPTOR), overrides the field descriptor. + /// + template + SizedPacketBuf(const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR) : PacketRef(mData, sizeof(mData)) { createFromField(mData, sizeof(mData), field, fieldDescriptor); } + + + ///@brief Explicitly obtains a reference to the packet data. + /// + PacketRef ref() { return *this; } + + ///@brief Explicitly obtains a const reference to the packet data. + /// + const PacketRef& ref() const { return *this; } + + ///@brief Returns a pointer to the underlying buffer. + /// This is technically the same as PacketRef::pointer but is writable. + uint8_t* buffer() { return mData; } + + ///@brief Copies the data from the pointer to this buffer. The data is not inspected. + /// + ///@param data Pointer to the start of the packet. + ///@param length Total length of the packet. + /// + void copyFrom(const uint8_t* data, size_t length) { assert(length <= sizeof(mData)); std::memcpy(mData, data, length); } + + ///@brief Copies an existing packet. The packet is assumed to be valid (undefined behavior otherwise). + /// + ///@param packet A "sane" (isSane()) mip packet. + /// + void copyFrom(const PacketRef& packet) { assert(packet.isSane()); copyFrom(packet.pointer(), packet.totalLength()); } + + ///@brief Copies this packet to an external buffer. + /// + /// This packet must be sane (see isSane()). Undefined behavior otherwise due to lookup of totalLength(). + /// + ///@param buffer Data is copied into this location. + ///@param maxLength Maximum number of bytes to copy. + /// + ///@returns true if successful. + ///@returns false if maxLength is too short. + /// + bool copyTo(uint8_t* buffer, size_t maxLength) { assert(isSane()); size_t copyLength = this->totalLength(); if(copyLength > maxLength) return false; std::memcpy(buffer, mData, copyLength); } + +private: + uint8_t mData[BufferSize]; +}; + +typedef SizedPacketBuf PacketBuf; + //////////////////////////////////////////////////////////////////////////////// ///@brief C++ class representing a MIP parser. /// @@ -317,11 +391,11 @@ class Parser : public C::mip_parser ///@copydoc mip::C::mip_parser_init Parser(uint8_t* buffer, size_t bufferSize, C::mip_packet_callback callback, void* callbackObject, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, callback, callbackObject, timeout); } ///@copydoc mip::C::mip_parser_init - Parser(uint8_t* buffer, size_t bufferSize, bool (*callback)(void*,const Packet*,Timestamp), void* callbackObject, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, (C::mip_packet_callback)callback, callbackObject, timeout); } + Parser(uint8_t* buffer, size_t bufferSize, bool (*callback)(void*,const PacketRef*,Timestamp), void* callbackObject, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, (C::mip_packet_callback)callback, callbackObject, timeout); } Parser(uint8_t* buffer, size_t bufferSize, Timeout timeout) { C::mip_parser_init(this, buffer, bufferSize, nullptr, nullptr, timeout); } - template + template void setCallback(T& object); ///@copydoc mip::C::mip_parser_reset @@ -344,7 +418,7 @@ class Parser : public C::mip_parser ///@code{.cpp} /// struct MyClass /// { -/// void handlePacket(const Packet& packet, Timeout timeout); +/// void handlePacket(const PacketRef& packet, Timeout timeout); /// }; /// MyClass myInstance; /// Parser parser(myInstance); @@ -357,12 +431,12 @@ class Parser : public C::mip_parser ///@param object /// Instance of T to call the callback. /// -template +template void Parser::setCallback(T& object) { C::mip_packet_callback callback = [](void* obj, const C::mip_packet* pkt, Timestamp timestamp)->bool { - return (static_cast(obj)->*Callback)(Packet(pkt), timestamp); + return (static_cast(obj)->*Callback)(PacketRef(pkt), timestamp); }; C::mip_parser_set_callback(this, callback, &object); diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 3318c0698..8eec41cce 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -286,10 +286,10 @@ class DeviceInterface : public C::mip_interface void registerFieldCallback(C::mip_dispatch_handler& handler, uint8_t descriptorSet, uint8_t fieldDescriptor, C::mip_dispatch_field_callback callback, void* userData) { C::mip_interface_register_field_callback(this, &handler, descriptorSet, fieldDescriptor, callback, userData); } - template + template void registerPacketCallback(C::mip_dispatch_handler& handler, uint8_t descriptorSet, bool afterFields, void* userData=nullptr); - template + template void registerPacketCallback(C::mip_dispatch_handler& handler, uint8_t descriptorSet, bool afterFields, Object* object); @@ -540,7 +540,7 @@ void DeviceInterface::setCallbacks(T* object) /// /// Example usage: ///@code{.cpp} -/// void handle_packet(void* context, const Packet& packet, Timestamp timestamp) +/// void handle_packet(void* context, const PacketRef& packet, Timestamp timestamp) /// { /// // Use the packet data /// } @@ -557,12 +557,12 @@ void DeviceInterface::setCallbacks(T* object) /// ///@endcode /// -template +template void DeviceInterface::registerPacketCallback(C::mip_dispatch_handler& handler, uint8_t descriptorSet, bool afterFields, void* userData) { auto callback = [](void* context, const C::mip_packet* packet, Timestamp timestamp) { - Callback(context, Packet(*packet), timestamp); + Callback(context, PacketRef(*packet), timestamp); }; registerPacketCallback(handler, descriptorSet, afterFields, callback, userData); @@ -584,7 +584,7 @@ void DeviceInterface::registerPacketCallback(C::mip_dispatch_handler& handler, u ///@code{.cpp} /// class MySystem /// { -/// void handlePacket(const Packet& packet, Timestamp timestamp) +/// void handlePacket(const PacketRef& packet, Timestamp timestamp) /// { /// } /// @@ -599,13 +599,13 @@ void DeviceInterface::registerPacketCallback(C::mip_dispatch_handler& handler, u /// }; ///@endcode /// -template +template void DeviceInterface::registerPacketCallback(C::mip_dispatch_handler& handler, uint8_t descriptorSet, bool afterFields, Object* object) { auto callback = [](void* pointer, const mip::C::mip_packet* packet, Timestamp timestamp) { Object* obj = static_cast(pointer); - (obj->*Callback)(Packet(*packet), timestamp); + (obj->*Callback)(PacketRef(*packet), timestamp); }; registerPacketCallback(handler, descriptorSet, afterFields, callback, object); @@ -714,7 +714,7 @@ void DeviceInterface::registerFieldCallback(C::mip_dispatch_handler& handler, ui /// /// Example usage: ///@code{.cpp} -/// void handle_packet(void* context, const Packet& packet, Timestamp timestamp) +/// void handle_packet(void* context, const PacketRef& packet, Timestamp timestamp) /// { /// // Use the packet data /// } @@ -773,7 +773,7 @@ void DeviceInterface::registerDataCallback(C::mip_dispatch_handler& handler, voi /// /// Example usage: ///@code{.cpp} -/// void handle_packet(void* context, uint8_t descriptorSet, const Packet& packet, Timestamp timestamp) +/// void handle_packet(void* context, uint8_t descriptorSet, const PacketRef& packet, Timestamp timestamp) /// { /// // Use the packet data /// } @@ -952,8 +952,7 @@ void DeviceInterface::registerExtractor(C::mip_dispatch_handler& handler, DataFi template CmdResult runCommand(C::mip_interface& device, const Cmd& cmd, Timeout additionalTime) { - uint8_t buffer[PACKET_LENGTH_MAX]; - Packet packet = Packet::createFromField(buffer, sizeof(buffer), cmd); + PacketBuf packet(cmd); C::mip_pending_cmd pending; C::mip_pending_cmd_init_with_timeout(&pending, Cmd::DESCRIPTOR_SET, Cmd::FIELD_DESCRIPTOR, additionalTime); @@ -971,11 +970,10 @@ CmdResult runCommand(C::mip_interface& device, const Args&&... args, Timeout add template CmdResult runCommand(C::mip_interface& device, const Cmd& cmd, typename Cmd::Response& response, Timeout additionalTime) { - uint8_t buffer[PACKET_LENGTH_MAX]; - Packet packet = Packet::createFromField(buffer, sizeof(buffer), cmd); + PacketBuf packet(cmd); C::mip_pending_cmd pending; - C::mip_pending_cmd_init_full(&pending, Cmd::DESCRIPTOR_SET, Cmd::FIELD_DESCRIPTOR, Cmd::Response::FIELD_DESCRIPTOR, buffer, FIELD_PAYLOAD_LENGTH_MAX, additionalTime); + C::mip_pending_cmd_init_full(&pending, Cmd::DESCRIPTOR_SET, Cmd::FIELD_DESCRIPTOR, Cmd::Response::FIELD_DESCRIPTOR, packet.buffer(), FIELD_PAYLOAD_LENGTH_MAX, additionalTime); CmdResult result = C::mip_interface_run_command_packet(&device, &packet, &pending); if( result != C::MIP_ACK_OK ) @@ -983,15 +981,14 @@ CmdResult runCommand(C::mip_interface& device, const Cmd& cmd, typename Cmd::Res size_t responseLength = C::mip_pending_cmd_response_length(&pending); - return extract(response, buffer, responseLength, 0) ? CmdResult::ACK_OK : CmdResult::STATUS_ERROR; + return extract(response, packet.buffer(), responseLength, 0) ? CmdResult::ACK_OK : CmdResult::STATUS_ERROR; } template bool startCommand(C::mip_interface& device, C::mip_pending_cmd& pending, const Cmd& cmd, Timeout additionalTime) { - uint8_t buffer[PACKET_LENGTH_MAX]; - Packet packet = Packet::createFromField(buffer, sizeof(buffer), cmd); + PacketBuf packet(cmd); C::mip_pending_cmd_init_with_timeout(&pending, Cmd::DESCRIPTOR_SET, Cmd::FIELD_DESCRIPTOR, additionalTime); @@ -1001,8 +998,7 @@ bool startCommand(C::mip_interface& device, C::mip_pending_cmd& pending, const C //template //bool startCommand(C::mip_interface& device, C::mip_pending_cmd& pending, const Cmd& cmd, uint8_t* responseBuffer, uint8_t responseBufferSize, Timeout additionalTime) //{ -// uint8_t buffer[PACKET_LENGTH_MAX]; -// Packet packet = Packet::createFromField(buffer, sizeof(buffer), cmd); +// PacketBuf packet(cmd); // // C::mip_pending_cmd_init_full(&pending, Cmd::descriptorSet, Cmd::fieldDescriptor, Cmd::Response::fieldDescriptor, responseBuffer, responseBufferSize, additionalTime); // diff --git a/test/mip/test_mip.cpp b/test/mip/test_mip.cpp index 190ae84b6..2c08bcde4 100644 --- a/test/mip/test_mip.cpp +++ b/test/mip/test_mip.cpp @@ -23,7 +23,7 @@ int main(int argc, const char* argv[]) { srand(0); - auto callback = [](void*, const Packet* parsedPacket, Timestamp timestamp)->bool + auto callback = [](void*, const PacketRef* parsedPacket, Timestamp timestamp)->bool { unsigned int parsedFields = 0; bool error = false; @@ -78,7 +78,7 @@ int main(int argc, const char* argv[]) for(unsigned int i=0; i Date: Tue, 18 Jul 2023 12:38:51 -0400 Subject: [PATCH 086/252] Fix missing return and update comments. --- src/mip/mip.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 4bf85c1da..9bacd8850 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -372,14 +372,21 @@ class SizedPacketBuf : public PacketRef ///@returns true if successful. ///@returns false if maxLength is too short. /// - bool copyTo(uint8_t* buffer, size_t maxLength) { assert(isSane()); size_t copyLength = this->totalLength(); if(copyLength > maxLength) return false; std::memcpy(buffer, mData, copyLength); } + bool copyTo(uint8_t* buffer, size_t maxLength) { assert(isSane()); size_t copyLength = this->totalLength(); if(copyLength > maxLength) return false; std::memcpy(buffer, mData, copyLength); return true; } private: uint8_t mData[BufferSize]; }; +//////////////////////////////////////////////////////////////////////////////// +///@brief Typedef for SizedPacketBuf of max possible size. +/// +/// Generally you should use this instead of SizedPacketBuf directly, unless you +/// know the maximum size of your packet will be less than PACKET_LENGTH_MAX. +/// typedef SizedPacketBuf PacketBuf; + //////////////////////////////////////////////////////////////////////////////// ///@brief C++ class representing a MIP parser. /// From ff60d90767961fe953c126e64801d3a72150defd Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 18 Jul 2023 13:02:42 -0400 Subject: [PATCH 087/252] Enable copy constructor for PacketBuf so it can be used in std::vector. --- src/mip/mip.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 9bacd8850..26fab9bf8 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -320,8 +320,10 @@ class SizedPacketBuf : public PacketRef explicit SizedPacketBuf(const uint8_t* data, size_t length) : PacketRef(mData, sizeof(mData)) { copyFrom(data, length); } explicit SizedPacketBuf(const PacketRef& packet) : PacketRef(mData, sizeof(mData)) { copyFrom(packet); } - // No copying or moving allowed - use the explicit functions copyFrom or copyTo. - SizedPacketBuf(const SizedPacketBuf& other) = delete; + // Copy constructor, required to put packets into std::vector. + explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; + + // No moving allowed - use the explicit functions copyFrom or copyTo. SizedPacketBuf(SizedPacketBuf&&) = delete; SizedPacketBuf& operator=(const SizedPacketBuf& other) = delete; SizedPacketBuf& operator=(SizedPacketBuf&&) = delete; From 6834afd60ec42d07ef3b9e5d70ae34dd8393795f Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 18 Jul 2023 13:02:42 -0400 Subject: [PATCH 088/252] Enable copy constructor for PacketBuf so it can be used in std::vector. --- src/mip/mip.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 9bacd8850..26fab9bf8 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -320,8 +320,10 @@ class SizedPacketBuf : public PacketRef explicit SizedPacketBuf(const uint8_t* data, size_t length) : PacketRef(mData, sizeof(mData)) { copyFrom(data, length); } explicit SizedPacketBuf(const PacketRef& packet) : PacketRef(mData, sizeof(mData)) { copyFrom(packet); } - // No copying or moving allowed - use the explicit functions copyFrom or copyTo. - SizedPacketBuf(const SizedPacketBuf& other) = delete; + // Copy constructor, required to put packets into std::vector. + explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; + + // No moving allowed - use the explicit functions copyFrom or copyTo. SizedPacketBuf(SizedPacketBuf&&) = delete; SizedPacketBuf& operator=(const SizedPacketBuf& other) = delete; SizedPacketBuf& operator=(SizedPacketBuf&&) = delete; From 086d307c7cab08e7905f6ae2c2532722542cc882 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 18 Jul 2023 13:23:48 -0400 Subject: [PATCH 089/252] Enable default construction of PacketBuf. --- src/mip/mip.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 26fab9bf8..f8e91eabe 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -313,7 +313,7 @@ template class SizedPacketBuf : public PacketRef { public: - SizedPacketBuf(uint8_t descriptorSet) : PacketRef(mData, sizeof(mData), descriptorSet) {} + SizedPacketBuf(uint8_t descriptorSet=INVALID_DESCRIPTOR_SET) : PacketRef(mData, sizeof(mData), descriptorSet) {} ///@brief Creates a PacketBuf by copying existing data. /// From 2f6c505c239d6dd6d280818697309aa65887589b Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 20 Jul 2023 12:36:52 -0400 Subject: [PATCH 090/252] Undelete PacketBuf move constructor. --- src/mip/mip.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index f8e91eabe..6b7835408 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -324,7 +324,6 @@ class SizedPacketBuf : public PacketRef explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; // No moving allowed - use the explicit functions copyFrom or copyTo. - SizedPacketBuf(SizedPacketBuf&&) = delete; SizedPacketBuf& operator=(const SizedPacketBuf& other) = delete; SizedPacketBuf& operator=(SizedPacketBuf&&) = delete; From b529e5737e3d84e137abd8a409c763e831536d96 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Mon, 24 Jul 2023 14:42:51 -0400 Subject: [PATCH 091/252] Added diagnostics to recording_connection to track bytes written --- src/mip/extras/recording_connection.cpp | 8 ++++++++ src/mip/extras/recording_connection.hpp | 14 ++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp index 62ef0e2af..237703b31 100644 --- a/src/mip/extras/recording_connection.cpp +++ b/src/mip/extras/recording_connection.cpp @@ -21,7 +21,10 @@ bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) { const bool ok = mConnection->sendToDevice(data, length); if( ok && mSendFile != nullptr && mConnection->isConnected()) + { mSendFile->write(reinterpret_cast(data), length); + } + return ok; } @@ -30,7 +33,12 @@ bool RecordingConnection::recvFromDevice(uint8_t* buffer, size_t max_length, Tim { const bool ok = mConnection->recvFromDevice(buffer, max_length, wait_time, count_out, timestamp_out); if (ok && mRecvFile != nullptr && mConnection->isConnected()) + { mRecvFile->write(reinterpret_cast(buffer), *count_out); + + mRecvFileWritten += *count_out; + } + return ok; } diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index 8552d2069..9afeb64e6 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace mip { @@ -49,12 +50,25 @@ class RecordingConnection : public Connection return false; }; + uint64_t recvFileBytesWritten() + { + return mRecvFileWritten; + } + + uint64_t sendFileBytesWritten() + { + return mSendFileWritten; + } + protected: Connection* mConnection; // Files may be NULL to not record one direction or the other std::ostream* mRecvFile; std::ostream* mSendFile; + + uint64_t mRecvFileWritten = 0; + uint64_t mSendFileWritten = 0; }; //////////////////////////////////////////////////////////////////////////////// From a33805c825553532f03d7f607ddad7f49455157f Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Mon, 24 Jul 2023 14:48:02 -0400 Subject: [PATCH 092/252] Update recording_connection.cpp --- src/mip/extras/recording_connection.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mip/extras/recording_connection.cpp b/src/mip/extras/recording_connection.cpp index 237703b31..b9a3e7d7a 100644 --- a/src/mip/extras/recording_connection.cpp +++ b/src/mip/extras/recording_connection.cpp @@ -23,6 +23,8 @@ bool RecordingConnection::sendToDevice(const uint8_t* data, size_t length) if( ok && mSendFile != nullptr && mConnection->isConnected()) { mSendFile->write(reinterpret_cast(data), length); + + mSendFileWritten += length; } return ok; From 3ede19c501631e5626fb4e52263b09e2839b9a2c Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Mon, 24 Jul 2023 14:49:21 -0400 Subject: [PATCH 093/252] Fixed a bug --- src/mip/extras/recording_connection.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index 9afeb64e6..8c83c1493 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -5,7 +5,6 @@ #include #include #include -#include namespace mip { From a70256ff75f7663b4df5fa3bb20f8d8680af7006 Mon Sep 17 00:00:00 2001 From: Nick DaCosta Date: Wed, 26 Jul 2023 09:34:56 -0400 Subject: [PATCH 094/252] Fixed TCP socket size typedef for MinGW --- src/mip/utils/tcp_socket.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/mip/utils/tcp_socket.c b/src/mip/utils/tcp_socket.c index f6a91195d..89c573d54 100644 --- a/src/mip/utils/tcp_socket.c +++ b/src/mip/utils/tcp_socket.c @@ -9,7 +9,10 @@ #include static const int SEND_FLAGS = 0; + +#ifdef _MSC_VER typedef int ssize_t; +#endif #else From da34ef2881592eb0525daccf00bf9071002f9db5 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 2 Aug 2023 16:57:44 -0400 Subject: [PATCH 095/252] Add missing files to MIP SDK. --- src/mip/definitions/common.c | 39 ++++++ src/mip/definitions/common.h | 248 +++++++++++++++++++++++++++++++++++ 2 files changed, 287 insertions(+) create mode 100644 src/mip/definitions/common.c create mode 100644 src/mip/definitions/common.h diff --git a/src/mip/definitions/common.c b/src/mip/definitions/common.c new file mode 100644 index 000000000..686d4885a --- /dev/null +++ b/src/mip/definitions/common.c @@ -0,0 +1,39 @@ + +#include "common.h" + +#include "../utils/serialization.h" + + +void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self) +{ + insert_u8(serializer, self->descriptor); + insert_u16(serializer, self->decimation); +} + +void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self) +{ + extract_u8(serializer, &self->descriptor); + extract_u16(serializer, &self->decimation); +} + +#define IMPLEMENT_MIP_VECTOR_FUNCTIONS(n,type,name) \ +void insert_##name(mip_serializer* serializer, const name self) \ +{ \ + for(unsigned int i=0; i +#include +#include +#include "../utils/serialization.h" + +#ifdef __cplusplus + +#include +#include + +namespace mip { +namespace C { +extern "C" { +#endif // __cplusplus + + +typedef struct mip_descriptor_rate +{ + uint8_t descriptor; + uint16_t decimation; +} mip_descriptor_rate; + +void insert_mip_descriptor_rate(mip_serializer* serializer, const mip_descriptor_rate* self); +void extract_mip_descriptor_rate(mip_serializer* serializer, mip_descriptor_rate* self); + +#define DECLARE_MIP_VECTOR_TYPE(n,type,name) \ +typedef type name[n]; \ +\ +void insert_##name(mip_serializer* serializer, const name self); \ +void extract_##name(mip_serializer* serializer, name self); + +DECLARE_MIP_VECTOR_TYPE(3, float, mip_vector3f) +DECLARE_MIP_VECTOR_TYPE(4, float, mip_vector4f) +DECLARE_MIP_VECTOR_TYPE(9, float, mip_matrix3f) +DECLARE_MIP_VECTOR_TYPE(3, double, mip_vector3d) +DECLARE_MIP_VECTOR_TYPE(4, double, mip_vector4d) +DECLARE_MIP_VECTOR_TYPE(9, double, mip_matrix3d) +DECLARE_MIP_VECTOR_TYPE(4, float, mip_quatf) + +#undef DECLARE_MIP_VECTOR_TYPE + +//typedef struct mip_vector3f +//{ +// float m_data[3]; +//} mip_vector3f; +// +//void insert_mip_vector3(mip_serializer* serializer, const mip_vector3* self); +//void extract_mip_vector3(mip_serializer* serializer, mip_vector3* self); +// +//typedef struct mip_vector4f +//{ +// float m_data[4]; +//} mip_vector4f; +// +//void insert_mip_vector3(mip_serializer* serializer, const mip_vector3* self); +//void extract_mip_vector3(mip_serializer* serializer, mip_vector3* self); +// +//typedef struct mip_matrix3 +//{ +// float m[9]; +//} mip_matrix3; +// +//void insert_mip_matrix3(mip_serializer* serializer, const mip_matrix3* self); +//void extract_mip_matrix3(mip_serializer* serializer, mip_matrix3* self); + + +#ifdef __cplusplus + +} // extern "C" +} // namespace "C" + + +using DescriptorRate = C::mip_descriptor_rate; + +inline void insert(Serializer& serializer, const DescriptorRate& self) { return C::insert_mip_descriptor_rate(&serializer, &self); } +inline void extract(Serializer& serializer, DescriptorRate& self) { return C::extract_mip_descriptor_rate(&serializer, &self); } + + +template +using Vector = T[N]; + +using Vector3f = Vector; +using Vector4f = Vector; +using Matrix3f = Vector; +using Vector3d = Vector; +using Vector4d = Vector; +using Matrix3d = Vector; + +using Quatf = Vector4f; + +template +void insert(Serializer& serializer, const Vector& v) { for(size_t i=0; i +void extract(Serializer& serializer, Vector& v) { for(size_t i=0; i +//class Vector +//{ +//public: +// Vector() { *this = T(0); } +// Vector(const Vector&) = default; +// Vector(const T (&ptr)[N]) { copyFrom(ptr); } +// +// Vector& operator=(const Vector&) = default; +// Vector& operator=(const T* ptr) { copyFrom(ptr); return this; } +// Vector& operator=(T value) { for(size_t i=0; i +// void copyFrom(const U* ptr) { for(size_t i=0; i +// void copyTo(U* ptr) const { for(size_t i=0; i +//class Vector2 : public Vector +//{ +// using Vector::Vector; +// +// template +// Vector2(U x_, U y_) { x()=x_; y()=y_;} +// +// T& x() { return this->m_data[0]; } +// T& y() { return this->m_data[1]; } +// +// T x() const { return this->m_data[0]; } +// T y() const { return this->m_data[1]; } +//}; +// +/////@brief A 3D vector which provides .x(), .y() and .z() accessors. +///// +//template +//class Vector3 : public Vector +//{ +// //using Vector::Vector; +// template +// Vector3(const U* ptr) { copyFrom(ptr); } +// +// template +// Vector3(U x_, U y_, U z_) { x()=x_; y()=y_; z()=z_; } +// +// T& x() { return this->m_data[0]; } +// T& y() { return this->m_data[1]; } +// T& z() { return this->m_data[2]; } +// +// T x() const { return this->m_data[0]; } +// T y() const { return this->m_data[1]; } +// T z() const { return this->m_data[2]; } +//}; +// +/////@brief A 4D vector which provides .x(), .y(), .z(), and .w() accessors. +///// +//template +//class Vector4 : public Vector +//{ +// using Vector::Vector; +// +// template +// Vector4(U x_, U y_, U z_, U w_) { x()=x_; y()=y_; z()=z_; w()=w_; } +// +// T& x() { return this->m_data[0]; } +// T& y() { return this->m_data[1]; } +// T& z() { return this->m_data[2]; } +// T& w() { return this->m_data[3]; } +// +// T x() const { return this->m_data[0]; } +// T y() const { return this->m_data[1]; } +// T z() const { return this->m_data[2]; } +// T w() const { return this->m_data[3]; } +//}; +// +/////@brief A quaternion which provides .x(), .y(), .z(), and .w() accessors. +///// +//template +//class Quaternion : public Vector4 +//{ +// using Vector4::Vector; +//}; +// +//typedef Vector2 Vector2f; +//typedef Vector3 Vector3f; +//typedef Vector4 Vector4f; +// +//typedef Vector2 Vector2d; +//typedef Vector3 Vector3d; +//typedef Vector4 Vector4d; +// +//typedef Quaternion Quatf; +//typedef Vector Matrix3f; +// +//template +//void insert(Serializer& serializer, const Vector& self) +//{ +// for(size_t i=0; i +//void extract(Serializer& serializer, Vector& self) +//{ +// for(size_t i=0; i +//void insert(Serializer& serializer, const Quaternion& self) +//{ +// insert(serializer, self.w()); // w comes first in mip +// insert(serializer, self.x()); +// insert(serializer, self.y()); +// insert(serializer, self.z()); +//} +// +//template +//void extract(Serializer& serializer, Quaternion& self) +//{ +// extract(serializer, self.w()); // w comes first in mip +// extract(serializer, self.x()); +// extract(serializer, self.y()); +// extract(serializer, self.z()); +//} + + +} // namespace mip + +#endif // __cplusplus From cb0b3165e37fb1d186bbb6fe82f11762c139a80c Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 2 Aug 2023 17:08:22 -0400 Subject: [PATCH 096/252] Generate MIP definitions from branches/dev@55196. --- src/mip/definitions/commands_aiding.h | 5 +++-- src/mip/definitions/commands_aiding.hpp | 5 +++-- src/mip/definitions/commands_filter.c | 2 +- src/mip/definitions/commands_filter.cpp | 2 +- src/mip/definitions/commands_filter.hpp | 6 +++--- 5 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index 3c207f0ab..6e08c1b8b 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -61,8 +61,9 @@ enum //////////////////////////////////////////////////////////////////////////////// typedef uint8_t mip_time_timebase; -static const mip_time_timebase MIP_TIME_TIMEBASE_INTERNAL_REFERENCE = 1; ///< Internal reference time from the device. -static const mip_time_timebase MIP_TIME_TIMEBASE_EXTERNAL_TIME = 2; ///< External reference time from PPS. +static const mip_time_timebase MIP_TIME_TIMEBASE_INTERNAL_REFERENCE = 1; ///< Timestamp provided is with respect to internal clock. +static const mip_time_timebase MIP_TIME_TIMEBASE_EXTERNAL_TIME = 2; ///< Timestamp provided is with respect to external clock, synced by PPS source. +static const mip_time_timebase MIP_TIME_TIMEBASE_TIME_OF_ARRIVAL = 3; ///< Timestamp provided is a fixed latency relative to time of message arrival. struct mip_time { diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 4e0470c00..3edeeb989 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -63,8 +63,9 @@ struct Time { enum class Timebase : uint8_t { - INTERNAL_REFERENCE = 1, ///< Internal reference time from the device. - EXTERNAL_TIME = 2, ///< External reference time from PPS. + INTERNAL_REFERENCE = 1, ///< Timestamp provided is with respect to internal clock. + EXTERNAL_TIME = 2, ///< Timestamp provided is with respect to external clock, synced by PPS source. + TIME_OF_ARRIVAL = 3, ///< Timestamp provided is a fixed latency relative to time of message arrival. }; Timebase timebase = static_cast(0); ///< Timebase reference, e.g. internal, external, GPS, UTC, etc. diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index 8e0e06bff..df9fb53d1 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -2725,7 +2725,7 @@ mip_cmd_result mip_filter_read_pressure_altitude_noise(struct mip_interface* dev assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); + mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_FILTER_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 04a70d0e9..16ea819b2 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -2418,7 +2418,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), CMD_PRESSURE_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index bdfde21b3..5106ab99f 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -2162,16 +2162,16 @@ struct PressureAltitudeNoise struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; static const uint32_t ECHOED_PARAMS = 0x0000; static const uint32_t COUNTER_PARAMS = 0x00000000; float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] - auto as_tuple() const + auto as_tuple() { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } }; From d96e509c8377e40280425c7dc9c61fa9c671eae3 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 3 Aug 2023 14:03:46 -0400 Subject: [PATCH 097/252] Fix PacketBuf copy constructor. --- src/mip/mip.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index f8e91eabe..137634c6f 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -320,13 +320,13 @@ class SizedPacketBuf : public PacketRef explicit SizedPacketBuf(const uint8_t* data, size_t length) : PacketRef(mData, sizeof(mData)) { copyFrom(data, length); } explicit SizedPacketBuf(const PacketRef& packet) : PacketRef(mData, sizeof(mData)) { copyFrom(packet); } - // Copy constructor, required to put packets into std::vector. - explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; + ///@brief Copy constructor (required to put packets into std::vector in some cases). + template + explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; - // No moving allowed - use the explicit functions copyFrom or copyTo. - SizedPacketBuf(SizedPacketBuf&&) = delete; - SizedPacketBuf& operator=(const SizedPacketBuf& other) = delete; - SizedPacketBuf& operator=(SizedPacketBuf&&) = delete; + ///@brief Assignment operator, copies data from another buffer to this one. + template + SizedPacketBuf& operator=(const SizedPacketBuf& other) { copyFrom(other); } ///@brief Create a packet containing just the given field. /// From 42bc633c2531afc06bc48ec527c4990bc9fcf278 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 3 Aug 2023 17:25:06 -0400 Subject: [PATCH 098/252] Make mip::Vectors an actual type. --- src/mip/definitions/common.h | 35 ++++++++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 17f6d8390..51610aad2 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -78,8 +78,41 @@ inline void insert(Serializer& serializer, const DescriptorRate& self) { return inline void extract(Serializer& serializer, DescriptorRate& self) { return C::extract_mip_descriptor_rate(&serializer, &self); } +////////////////////////////////////////////////////////////////////////////////// +///@brief Vector is a wrapper around an array of some type T, usually float or double. +/// +/// Implicit conversion to/from C-style pointers is provided to allow simple +/// integration with code using plain arrays. +/// template -using Vector = T[N]; +struct Vector : public std::array +{ + Vector() {} + + template + Vector(U value) { this->fill(value); } + + template + Vector(const U (&ptr)[N]) { copyFrom(ptr, N); } + + template + explicit Vector(const U (&ptr)[M]) { static_assert(M>=N, "Input array is too small"); copyFrom(ptr, M); } + + template + explicit Vector(const U* ptr, size_t n) { copyFrom(ptr, n); } + + Vector(const Vector&) = default; + Vector& operator=(const Vector&) = default; + + template + Vector& operator=(const Vector& other) { copyFrom(other, N); } + + operator const T*() const { return this->data(); } + operator T*() { return this->data(); } + + template + void copyFrom(const U* ptr, size_t n) { if(n>N) n=N; for(size_t i=0; i; using Vector4f = Vector; From 4b8a6bfd721a8f469b232ee7831df31a712dcd7b Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 4 Aug 2023 12:00:52 -0400 Subject: [PATCH 099/252] Generate MIP definitions from branches/dev@55207. --- src/mip/definitions/commands_3dm.hpp | 84 +++++++++--------- src/mip/definitions/commands_aiding.hpp | 18 ++-- src/mip/definitions/commands_base.hpp | 4 +- src/mip/definitions/commands_filter.hpp | 96 ++++++++++----------- src/mip/definitions/commands_gnss.hpp | 4 +- src/mip/definitions/commands_rtk.hpp | 8 +- src/mip/definitions/commands_system.hpp | 2 +- src/mip/definitions/data_filter.hpp | 110 ++++++++++++------------ src/mip/definitions/data_gnss.hpp | 54 ++++++------ src/mip/definitions/data_sensor.hpp | 46 +++++----- src/mip/definitions/data_shared.hpp | 18 ++-- src/mip/definitions/data_system.hpp | 8 +- 12 files changed, 226 insertions(+), 226 deletions(-) diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index bf45ba209..abae7e91d 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -218,7 +218,7 @@ struct PollImuMessage auto as_tuple() const { - return std::make_tuple(suppress_ack,num_descriptors,descriptors); + return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); } typedef void Response; @@ -257,7 +257,7 @@ struct PollGnssMessage auto as_tuple() const { - return std::make_tuple(suppress_ack,num_descriptors,descriptors); + return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); } typedef void Response; @@ -296,7 +296,7 @@ struct PollFilterMessage auto as_tuple() const { - return std::make_tuple(suppress_ack,num_descriptors,descriptors); + return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); } typedef void Response; @@ -336,7 +336,7 @@ struct ImuMessageFormat auto as_tuple() const { - return std::make_tuple(num_descriptors,descriptors); + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); } @@ -407,7 +407,7 @@ struct GpsMessageFormat auto as_tuple() const { - return std::make_tuple(num_descriptors,descriptors); + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); } @@ -478,7 +478,7 @@ struct FilterMessageFormat auto as_tuple() const { - return std::make_tuple(num_descriptors,descriptors); + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); } @@ -702,7 +702,7 @@ struct PollData auto as_tuple() const { - return std::make_tuple(desc_set,suppress_ack,num_descriptors,descriptors); + return std::make_tuple(std::ref(desc_set),std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); } typedef void Response; @@ -733,7 +733,7 @@ struct GetBaseRate auto as_tuple() const { - return std::make_tuple(desc_set); + return std::make_tuple(std::ref(desc_set)); } struct Response @@ -793,7 +793,7 @@ struct MessageFormat auto as_tuple() const { - return std::make_tuple(desc_set,num_descriptors,descriptors); + return std::make_tuple(std::ref(desc_set),std::ref(num_descriptors),std::ref(descriptors)); } @@ -864,7 +864,7 @@ struct NmeaPollData auto as_tuple() const { - return std::make_tuple(suppress_ack,count,format_entries); + return std::make_tuple(std::ref(suppress_ack),std::ref(count),std::ref(format_entries)); } typedef void Response; @@ -902,7 +902,7 @@ struct NmeaMessageFormat auto as_tuple() const { - return std::make_tuple(count,format_entries); + return std::make_tuple(std::ref(count),std::ref(format_entries)); } @@ -1033,7 +1033,7 @@ struct UartBaudrate auto as_tuple() const { - return std::make_tuple(baud); + return std::make_tuple(std::ref(baud)); } @@ -1104,7 +1104,7 @@ struct FactoryStreaming auto as_tuple() const { - return std::make_tuple(action,reserved); + return std::make_tuple(std::ref(action),std::ref(reserved)); } typedef void Response; @@ -1151,7 +1151,7 @@ struct DatastreamControl auto as_tuple() const { - return std::make_tuple(desc_set,enable); + return std::make_tuple(std::ref(desc_set),std::ref(enable)); } @@ -1283,7 +1283,7 @@ struct ConstellationSettings auto as_tuple() const { - return std::make_tuple(max_channels,config_count,settings); + return std::make_tuple(std::ref(max_channels),std::ref(config_count),std::ref(settings)); } @@ -1393,7 +1393,7 @@ struct GnssSbasSettings auto as_tuple() const { - return std::make_tuple(enable_sbas,sbas_options,num_included_prns,included_prns); + return std::make_tuple(std::ref(enable_sbas),std::ref(sbas_options),std::ref(num_included_prns),std::ref(included_prns)); } @@ -1480,7 +1480,7 @@ struct GnssAssistedFix auto as_tuple() const { - return std::make_tuple(option,flags); + return std::make_tuple(std::ref(option),std::ref(flags)); } @@ -1553,7 +1553,7 @@ struct GnssTimeAssistance auto as_tuple() const { - return std::make_tuple(tow,week_number,accuracy); + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(accuracy)); } @@ -1638,7 +1638,7 @@ struct ImuLowpassFilter auto as_tuple() const { - return std::make_tuple(target_descriptor,enable,manual,frequency,reserved); + return std::make_tuple(std::ref(target_descriptor),std::ref(enable),std::ref(manual),std::ref(frequency),std::ref(reserved)); } @@ -1719,7 +1719,7 @@ struct PpsSource auto as_tuple() const { - return std::make_tuple(source); + return std::make_tuple(std::ref(source)); } @@ -1868,7 +1868,7 @@ struct GpioConfig auto as_tuple() const { - return std::make_tuple(pin,feature,behavior,pin_mode); + return std::make_tuple(std::ref(pin),std::ref(feature),std::ref(behavior),std::ref(pin_mode)); } @@ -1955,7 +1955,7 @@ struct GpioState auto as_tuple() const { - return std::make_tuple(pin,state); + return std::make_tuple(std::ref(pin),std::ref(state)); } @@ -2029,7 +2029,7 @@ struct Odometer auto as_tuple() const { - return std::make_tuple(mode,scaling,uncertainty); + return std::make_tuple(std::ref(mode),std::ref(scaling),std::ref(uncertainty)); } @@ -2120,7 +2120,7 @@ struct GetEventSupport auto as_tuple() const { - return std::make_tuple(query); + return std::make_tuple(std::ref(query)); } struct Response @@ -2199,7 +2199,7 @@ struct EventControl auto as_tuple() const { - return std::make_tuple(instance,mode); + return std::make_tuple(std::ref(instance),std::ref(mode)); } @@ -2299,7 +2299,7 @@ struct GetEventTriggerStatus auto as_tuple() const { - return std::make_tuple(requested_count,requested_instances); + return std::make_tuple(std::ref(requested_count),std::ref(requested_instances)); } struct Response @@ -2358,7 +2358,7 @@ struct GetEventActionStatus auto as_tuple() const { - return std::make_tuple(requested_count,requested_instances); + return std::make_tuple(std::ref(requested_count),std::ref(requested_instances)); } struct Response @@ -2492,7 +2492,7 @@ struct EventTrigger auto as_tuple() const { - return std::make_tuple(instance,type,parameters); + return std::make_tuple(std::ref(instance),std::ref(type),std::ref(parameters)); } @@ -2612,7 +2612,7 @@ struct EventAction auto as_tuple() const { - return std::make_tuple(instance,trigger,type,parameters); + return std::make_tuple(std::ref(instance),std::ref(trigger),std::ref(type),std::ref(parameters)); } @@ -2691,7 +2691,7 @@ struct AccelBias auto as_tuple() const { - return std::make_tuple(bias); + return std::make_tuple(std::ref(bias)); } @@ -2760,7 +2760,7 @@ struct GyroBias auto as_tuple() const { - return std::make_tuple(bias); + return std::make_tuple(std::ref(bias)); } @@ -2826,7 +2826,7 @@ struct CaptureGyroBias auto as_tuple() const { - return std::make_tuple(averaging_time_ms); + return std::make_tuple(std::ref(averaging_time_ms)); } struct Response @@ -2887,7 +2887,7 @@ struct MagHardIronOffset auto as_tuple() const { - return std::make_tuple(offset); + return std::make_tuple(std::ref(offset)); } @@ -2964,7 +2964,7 @@ struct MagSoftIronMatrix auto as_tuple() const { - return std::make_tuple(offset); + return std::make_tuple(std::ref(offset)); } @@ -3031,7 +3031,7 @@ struct ConingScullingEnable auto as_tuple() const { - return std::make_tuple(enable); + return std::make_tuple(std::ref(enable)); } @@ -3124,7 +3124,7 @@ struct Sensor2VehicleTransformEuler auto as_tuple() const { - return std::make_tuple(roll,pitch,yaw); + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); } @@ -3225,7 +3225,7 @@ struct Sensor2VehicleTransformQuaternion auto as_tuple() const { - return std::make_tuple(q); + return std::make_tuple(std::ref(q)); } @@ -3322,7 +3322,7 @@ struct Sensor2VehicleTransformDcm auto as_tuple() const { - return std::make_tuple(dcm); + return std::make_tuple(std::ref(dcm)); } @@ -3396,7 +3396,7 @@ struct ComplementaryFilter auto as_tuple() const { - return std::make_tuple(pitch_roll_enable,heading_enable,pitch_roll_time_constant,heading_time_constant); + return std::make_tuple(std::ref(pitch_roll_enable),std::ref(heading_enable),std::ref(pitch_roll_time_constant),std::ref(heading_time_constant)); } @@ -3474,7 +3474,7 @@ struct SensorRange auto as_tuple() const { - return std::make_tuple(sensor,setting); + return std::make_tuple(std::ref(sensor),std::ref(setting)); } @@ -3546,7 +3546,7 @@ struct CalibratedSensorRanges auto as_tuple() const { - return std::make_tuple(sensor); + return std::make_tuple(std::ref(sensor)); } struct Response @@ -3623,7 +3623,7 @@ struct LowpassFilter auto as_tuple() const { - return std::make_tuple(desc_set,field_desc,enable,manual,frequency); + return std::make_tuple(std::ref(desc_set),std::ref(field_desc),std::ref(enable),std::ref(manual),std::ref(frequency)); } diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 3edeeb989..5212421b6 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -106,7 +106,7 @@ struct SensorFrameMapping auto as_tuple() const { - return std::make_tuple(sensor_id,frame_id); + return std::make_tuple(std::ref(sensor_id),std::ref(frame_id)); } @@ -182,7 +182,7 @@ struct ReferenceFrame auto as_tuple() const { - return std::make_tuple(frame_id,format,translation,rotation); + return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation)); } @@ -259,7 +259,7 @@ struct AidingEchoControl auto as_tuple() const { - return std::make_tuple(mode); + return std::make_tuple(std::ref(mode)); } @@ -354,7 +354,7 @@ struct EcefPos auto as_tuple() const { - return std::make_tuple(time,sensor_id,position,uncertainty,valid_flags); + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(position),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; @@ -422,7 +422,7 @@ struct LlhPos auto as_tuple() const { - return std::make_tuple(time,sensor_id,latitude,longitude,height,uncertainty,valid_flags); + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; @@ -487,7 +487,7 @@ struct EcefVel auto as_tuple() const { - return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; @@ -552,7 +552,7 @@ struct NedVel auto as_tuple() const { - return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; @@ -618,7 +618,7 @@ struct VehicleFixedFrameVelocity auto as_tuple() const { - return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; @@ -651,7 +651,7 @@ struct TrueHeading auto as_tuple() const { - return std::make_tuple(time,sensor_id,heading,uncertainty,valid_flags); + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(heading),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 82b724c20..274d06a96 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -578,7 +578,7 @@ struct CommSpeed auto as_tuple() const { - return std::make_tuple(port,baud); + return std::make_tuple(std::ref(port),std::ref(baud)); } @@ -656,7 +656,7 @@ struct GpsTimeUpdate auto as_tuple() const { - return std::make_tuple(field_id,value); + return std::make_tuple(std::ref(field_id),std::ref(value)); } diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 5106ab99f..d7d709387 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -243,7 +243,7 @@ struct SetInitialAttitude auto as_tuple() const { - return std::make_tuple(roll,pitch,heading); + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(heading)); } typedef void Response; @@ -333,7 +333,7 @@ struct EstimationControl auto as_tuple() const { - return std::make_tuple(enable); + return std::make_tuple(std::ref(enable)); } @@ -404,7 +404,7 @@ struct ExternalGnssUpdate auto as_tuple() const { - return std::make_tuple(gps_time,gps_week,latitude,longitude,height,velocity,pos_uncertainty,vel_uncertainty); + return std::make_tuple(std::ref(gps_time),std::ref(gps_week),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(velocity),std::ref(pos_uncertainty),std::ref(vel_uncertainty)); } typedef void Response; @@ -448,7 +448,7 @@ struct ExternalHeadingUpdate auto as_tuple() const { - return std::make_tuple(heading,heading_uncertainty,type); + return std::make_tuple(std::ref(heading),std::ref(heading_uncertainty),std::ref(type)); } typedef void Response; @@ -498,7 +498,7 @@ struct ExternalHeadingUpdateWithTime auto as_tuple() const { - return std::make_tuple(gps_time,gps_week,heading,heading_uncertainty,type); + return std::make_tuple(std::ref(gps_time),std::ref(gps_week),std::ref(heading),std::ref(heading_uncertainty),std::ref(type)); } typedef void Response; @@ -570,7 +570,7 @@ struct TareOrientation auto as_tuple() const { - return std::make_tuple(axes); + return std::make_tuple(std::ref(axes)); } @@ -645,7 +645,7 @@ struct VehicleDynamicsMode auto as_tuple() const { - return std::make_tuple(mode); + return std::make_tuple(std::ref(mode)); } @@ -736,7 +736,7 @@ struct SensorToVehicleRotationEuler auto as_tuple() const { - return std::make_tuple(roll,pitch,yaw); + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); } @@ -833,7 +833,7 @@ struct SensorToVehicleRotationDcm auto as_tuple() const { - return std::make_tuple(dcm); + return std::make_tuple(std::ref(dcm)); } @@ -927,7 +927,7 @@ struct SensorToVehicleRotationQuaternion auto as_tuple() const { - return std::make_tuple(quat); + return std::make_tuple(std::ref(quat)); } @@ -1002,7 +1002,7 @@ struct SensorToVehicleOffset auto as_tuple() const { - return std::make_tuple(offset); + return std::make_tuple(std::ref(offset)); } @@ -1074,7 +1074,7 @@ struct AntennaOffset auto as_tuple() const { - return std::make_tuple(offset); + return std::make_tuple(std::ref(offset)); } @@ -1153,7 +1153,7 @@ struct GnssSource auto as_tuple() const { - return std::make_tuple(source); + return std::make_tuple(std::ref(source)); } @@ -1243,7 +1243,7 @@ struct HeadingSource auto as_tuple() const { - return std::make_tuple(source); + return std::make_tuple(std::ref(source)); } @@ -1318,7 +1318,7 @@ struct AutoInitControl auto as_tuple() const { - return std::make_tuple(enable); + return std::make_tuple(std::ref(enable)); } @@ -1391,7 +1391,7 @@ struct AccelNoise auto as_tuple() const { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } @@ -1464,7 +1464,7 @@ struct GyroNoise auto as_tuple() const { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } @@ -1535,7 +1535,7 @@ struct AccelBiasModel auto as_tuple() const { - return std::make_tuple(beta,noise); + return std::make_tuple(std::ref(beta),std::ref(noise)); } @@ -1607,7 +1607,7 @@ struct GyroBiasModel auto as_tuple() const { - return std::make_tuple(beta,noise); + return std::make_tuple(std::ref(beta),std::ref(noise)); } @@ -1685,7 +1685,7 @@ struct AltitudeAiding auto as_tuple() const { - return std::make_tuple(selector); + return std::make_tuple(std::ref(selector)); } @@ -1759,7 +1759,7 @@ struct PitchRollAiding auto as_tuple() const { - return std::make_tuple(source); + return std::make_tuple(std::ref(source)); } @@ -1828,7 +1828,7 @@ struct AutoZupt auto as_tuple() const { - return std::make_tuple(enable,threshold); + return std::make_tuple(std::ref(enable),std::ref(threshold)); } @@ -1899,7 +1899,7 @@ struct AutoAngularZupt auto as_tuple() const { - return std::make_tuple(enable,threshold); + return std::make_tuple(std::ref(enable),std::ref(threshold)); } @@ -2076,7 +2076,7 @@ struct GravityNoise auto as_tuple() const { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } @@ -2148,7 +2148,7 @@ struct PressureAltitudeNoise auto as_tuple() const { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } @@ -2222,7 +2222,7 @@ struct HardIronOffsetNoise auto as_tuple() const { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } @@ -2295,7 +2295,7 @@ struct SoftIronMatrixNoise auto as_tuple() const { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } @@ -2368,7 +2368,7 @@ struct MagNoise auto as_tuple() const { - return std::make_tuple(noise); + return std::make_tuple(std::ref(noise)); } @@ -2440,7 +2440,7 @@ struct InclinationSource auto as_tuple() const { - return std::make_tuple(source,inclination); + return std::make_tuple(std::ref(source),std::ref(inclination)); } @@ -2513,7 +2513,7 @@ struct MagneticDeclinationSource auto as_tuple() const { - return std::make_tuple(source,declination); + return std::make_tuple(std::ref(source),std::ref(declination)); } @@ -2585,7 +2585,7 @@ struct MagFieldMagnitudeSource auto as_tuple() const { - return std::make_tuple(source,magnitude); + return std::make_tuple(std::ref(source),std::ref(magnitude)); } @@ -2659,7 +2659,7 @@ struct ReferencePosition auto as_tuple() const { - return std::make_tuple(enable,latitude,longitude,altitude); + return std::make_tuple(std::ref(enable),std::ref(latitude),std::ref(longitude),std::ref(altitude)); } @@ -2748,7 +2748,7 @@ struct AccelMagnitudeErrorAdaptiveMeasurement auto as_tuple() const { - return std::make_tuple(adaptive_measurement,frequency,low_limit,high_limit,low_limit_uncertainty,high_limit_uncertainty,minimum_uncertainty); + return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); } @@ -2835,7 +2835,7 @@ struct MagMagnitudeErrorAdaptiveMeasurement auto as_tuple() const { - return std::make_tuple(adaptive_measurement,frequency,low_limit,high_limit,low_limit_uncertainty,high_limit_uncertainty,minimum_uncertainty); + return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); } @@ -2922,7 +2922,7 @@ struct MagDipAngleErrorAdaptiveMeasurement auto as_tuple() const { - return std::make_tuple(enable,frequency,high_limit,high_limit_uncertainty,minimum_uncertainty); + return std::make_tuple(std::ref(enable),std::ref(frequency),std::ref(high_limit),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); } @@ -3007,7 +3007,7 @@ struct AidingMeasurementEnable auto as_tuple() const { - return std::make_tuple(aiding_source,enable); + return std::make_tuple(std::ref(aiding_source),std::ref(enable)); } @@ -3111,7 +3111,7 @@ struct KinematicConstraint auto as_tuple() const { - return std::make_tuple(acceleration_constraint_selection,velocity_constraint_selection,angular_constraint_selection); + return std::make_tuple(std::ref(acceleration_constraint_selection),std::ref(velocity_constraint_selection),std::ref(angular_constraint_selection)); } @@ -3234,7 +3234,7 @@ struct InitializationConfiguration auto as_tuple() const { - return std::make_tuple(wait_for_run_command,initial_cond_src,auto_heading_alignment_selector,initial_heading,initial_pitch,initial_roll,initial_position,initial_velocity,reference_frame_selector); + return std::make_tuple(std::ref(wait_for_run_command),std::ref(initial_cond_src),std::ref(auto_heading_alignment_selector),std::ref(initial_heading),std::ref(initial_pitch),std::ref(initial_roll),std::ref(initial_position),std::ref(initial_velocity),std::ref(reference_frame_selector)); } @@ -3310,7 +3310,7 @@ struct AdaptiveFilterOptions auto as_tuple() const { - return std::make_tuple(level,time_limit); + return std::make_tuple(std::ref(level),std::ref(time_limit)); } @@ -3382,7 +3382,7 @@ struct MultiAntennaOffset auto as_tuple() const { - return std::make_tuple(receiver_id,antenna_offset); + return std::make_tuple(std::ref(receiver_id),std::ref(antenna_offset)); } @@ -3453,7 +3453,7 @@ struct RelPosConfiguration auto as_tuple() const { - return std::make_tuple(source,reference_frame_selector,reference_coordinates); + return std::make_tuple(std::ref(source),std::ref(reference_frame_selector),std::ref(reference_coordinates)); } @@ -3535,7 +3535,7 @@ struct RefPointLeverArm auto as_tuple() const { - return std::make_tuple(ref_point_sel,lever_arm_offset); + return std::make_tuple(std::ref(ref_point_sel),std::ref(lever_arm_offset)); } @@ -3601,7 +3601,7 @@ struct SpeedMeasurement auto as_tuple() const { - return std::make_tuple(source,time_of_week,speed,speed_uncertainty); + return std::make_tuple(std::ref(source),std::ref(time_of_week),std::ref(speed),std::ref(speed_uncertainty)); } typedef void Response; @@ -3645,7 +3645,7 @@ struct SpeedLeverArm auto as_tuple() const { - return std::make_tuple(source,lever_arm_offset); + return std::make_tuple(std::ref(source),std::ref(lever_arm_offset)); } @@ -3720,7 +3720,7 @@ struct WheeledVehicleConstraintControl auto as_tuple() const { - return std::make_tuple(enable); + return std::make_tuple(std::ref(enable)); } @@ -3791,7 +3791,7 @@ struct VerticalGyroConstraintControl auto as_tuple() const { - return std::make_tuple(enable); + return std::make_tuple(std::ref(enable)); } @@ -3861,7 +3861,7 @@ struct GnssAntennaCalControl auto as_tuple() const { - return std::make_tuple(enable,max_offset); + return std::make_tuple(std::ref(enable),std::ref(max_offset)); } @@ -3925,7 +3925,7 @@ struct SetInitialHeading auto as_tuple() const { - return std::make_tuple(heading); + return std::make_tuple(std::ref(heading)); } typedef void Response; diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 605fec5ee..dc524896d 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -148,7 +148,7 @@ struct SignalConfiguration auto as_tuple() const { - return std::make_tuple(gps_enable,glonass_enable,galileo_enable,beidou_enable,reserved); + return std::make_tuple(std::ref(gps_enable),std::ref(glonass_enable),std::ref(galileo_enable),std::ref(beidou_enable),std::ref(reserved)); } @@ -221,7 +221,7 @@ struct RtkDongleConfiguration auto as_tuple() const { - return std::make_tuple(enable,reserved); + return std::make_tuple(std::ref(enable),std::ref(reserved)); } diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 3ad5ccafd..5d2c049b2 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -409,7 +409,7 @@ struct ConnectedDeviceType auto as_tuple() const { - return std::make_tuple(devType); + return std::make_tuple(std::ref(devType)); } @@ -646,7 +646,7 @@ struct ServiceStatus auto as_tuple() const { - return std::make_tuple(reserved1,reserved2); + return std::make_tuple(std::ref(reserved1),std::ref(reserved2)); } struct Response @@ -698,7 +698,7 @@ struct ProdEraseStorage auto as_tuple() const { - return std::make_tuple(media); + return std::make_tuple(std::ref(media)); } typedef void Response; @@ -731,7 +731,7 @@ struct LedControl auto as_tuple() const { - return std::make_tuple(primaryColor,altColor,act,period); + return std::make_tuple(std::ref(primaryColor),std::ref(altColor),std::ref(act),std::ref(period)); } typedef void Response; diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index 01e4b1e6c..892a7c385 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -88,7 +88,7 @@ struct CommMode auto as_tuple() const { - return std::make_tuple(mode); + return std::make_tuple(std::ref(mode)); } diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 347a04dc4..14f5a2815 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -369,7 +369,7 @@ struct PositionLlh auto as_tuple() const { - return std::make_tuple(latitude,longitude,ellipsoid_height,valid_flags); + return std::make_tuple(std::ref(latitude),std::ref(longitude),std::ref(ellipsoid_height),std::ref(valid_flags)); } }; @@ -398,7 +398,7 @@ struct VelocityNed auto as_tuple() const { - return std::make_tuple(north,east,down,valid_flags); + return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); } }; @@ -433,7 +433,7 @@ struct AttitudeQuaternion auto as_tuple() const { - return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); + return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3]),std::ref(valid_flags)); } }; @@ -470,7 +470,7 @@ struct AttitudeDcm auto as_tuple() const { - return std::make_tuple(dcm[0],dcm[1],dcm[2],dcm[3],dcm[4],dcm[5],dcm[6],dcm[7],dcm[8],valid_flags); + return std::make_tuple(std::ref(dcm[0]),std::ref(dcm[1]),std::ref(dcm[2]),std::ref(dcm[3]),std::ref(dcm[4]),std::ref(dcm[5]),std::ref(dcm[6]),std::ref(dcm[7]),std::ref(dcm[8]),std::ref(valid_flags)); } }; @@ -500,7 +500,7 @@ struct EulerAngles auto as_tuple() const { - return std::make_tuple(roll,pitch,yaw,valid_flags); + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw),std::ref(valid_flags)); } }; @@ -527,7 +527,7 @@ struct GyroBias auto as_tuple() const { - return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); + return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); } }; @@ -554,7 +554,7 @@ struct AccelBias auto as_tuple() const { - return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); + return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); } }; @@ -583,7 +583,7 @@ struct PositionLlhUncertainty auto as_tuple() const { - return std::make_tuple(north,east,down,valid_flags); + return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); } }; @@ -612,7 +612,7 @@ struct VelocityNedUncertainty auto as_tuple() const { - return std::make_tuple(north,east,down,valid_flags); + return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); } }; @@ -642,7 +642,7 @@ struct EulerAnglesUncertainty auto as_tuple() const { - return std::make_tuple(roll,pitch,yaw,valid_flags); + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw),std::ref(valid_flags)); } }; @@ -669,7 +669,7 @@ struct GyroBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); + return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); } }; @@ -696,7 +696,7 @@ struct AccelBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); + return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); } }; @@ -730,7 +730,7 @@ struct Timestamp auto as_tuple() const { - return std::make_tuple(tow,week_number,valid_flags); + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); } }; @@ -758,7 +758,7 @@ struct Status auto as_tuple() const { - return std::make_tuple(filter_state,dynamics_mode,status_flags); + return std::make_tuple(std::ref(filter_state),std::ref(dynamics_mode),std::ref(status_flags)); } }; @@ -786,7 +786,7 @@ struct LinearAccel auto as_tuple() const { - return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); + return std::make_tuple(std::ref(accel[0]),std::ref(accel[1]),std::ref(accel[2]),std::ref(valid_flags)); } }; @@ -813,7 +813,7 @@ struct GravityVector auto as_tuple() const { - return std::make_tuple(gravity[0],gravity[1],gravity[2],valid_flags); + return std::make_tuple(std::ref(gravity[0]),std::ref(gravity[1]),std::ref(gravity[2]),std::ref(valid_flags)); } }; @@ -840,7 +840,7 @@ struct CompAccel auto as_tuple() const { - return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); + return std::make_tuple(std::ref(accel[0]),std::ref(accel[1]),std::ref(accel[2]),std::ref(valid_flags)); } }; @@ -867,7 +867,7 @@ struct CompAngularRate auto as_tuple() const { - return std::make_tuple(gyro[0],gyro[1],gyro[2],valid_flags); + return std::make_tuple(std::ref(gyro[0]),std::ref(gyro[1]),std::ref(gyro[2]),std::ref(valid_flags)); } }; @@ -894,7 +894,7 @@ struct QuaternionAttitudeUncertainty auto as_tuple() const { - return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); + return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3]),std::ref(valid_flags)); } }; @@ -921,7 +921,7 @@ struct Wgs84GravityMag auto as_tuple() const { - return std::make_tuple(magnitude,valid_flags); + return std::make_tuple(std::ref(magnitude),std::ref(valid_flags)); } }; @@ -962,7 +962,7 @@ struct HeadingUpdateState auto as_tuple() const { - return std::make_tuple(heading,heading_1sigma,source,valid_flags); + return std::make_tuple(std::ref(heading),std::ref(heading_1sigma),std::ref(source),std::ref(valid_flags)); } }; @@ -994,7 +994,7 @@ struct MagneticModel auto as_tuple() const { - return std::make_tuple(intensity_north,intensity_east,intensity_down,inclination,declination,valid_flags); + return std::make_tuple(std::ref(intensity_north),std::ref(intensity_east),std::ref(intensity_down),std::ref(inclination),std::ref(declination),std::ref(valid_flags)); } }; @@ -1021,7 +1021,7 @@ struct AccelScaleFactor auto as_tuple() const { - return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); + return std::make_tuple(std::ref(scale_factor[0]),std::ref(scale_factor[1]),std::ref(scale_factor[2]),std::ref(valid_flags)); } }; @@ -1048,7 +1048,7 @@ struct AccelScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); + return std::make_tuple(std::ref(scale_factor_uncert[0]),std::ref(scale_factor_uncert[1]),std::ref(scale_factor_uncert[2]),std::ref(valid_flags)); } }; @@ -1075,7 +1075,7 @@ struct GyroScaleFactor auto as_tuple() const { - return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); + return std::make_tuple(std::ref(scale_factor[0]),std::ref(scale_factor[1]),std::ref(scale_factor[2]),std::ref(valid_flags)); } }; @@ -1102,7 +1102,7 @@ struct GyroScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); + return std::make_tuple(std::ref(scale_factor_uncert[0]),std::ref(scale_factor_uncert[1]),std::ref(scale_factor_uncert[2]),std::ref(valid_flags)); } }; @@ -1129,7 +1129,7 @@ struct MagBias auto as_tuple() const { - return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); + return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); } }; @@ -1156,7 +1156,7 @@ struct MagBiasUncertainty auto as_tuple() const { - return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); + return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); } }; @@ -1189,7 +1189,7 @@ struct StandardAtmosphere auto as_tuple() const { - return std::make_tuple(geometric_altitude,geopotential_altitude,standard_temperature,standard_pressure,standard_density,valid_flags); + return std::make_tuple(std::ref(geometric_altitude),std::ref(geopotential_altitude),std::ref(standard_temperature),std::ref(standard_pressure),std::ref(standard_density),std::ref(valid_flags)); } }; @@ -1220,7 +1220,7 @@ struct PressureAltitude auto as_tuple() const { - return std::make_tuple(pressure_altitude,valid_flags); + return std::make_tuple(std::ref(pressure_altitude),std::ref(valid_flags)); } }; @@ -1246,7 +1246,7 @@ struct DensityAltitude auto as_tuple() const { - return std::make_tuple(density_altitude,valid_flags); + return std::make_tuple(std::ref(density_altitude),std::ref(valid_flags)); } }; @@ -1275,7 +1275,7 @@ struct AntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(offset[0],offset[1],offset[2],valid_flags); + return std::make_tuple(std::ref(offset[0]),std::ref(offset[1]),std::ref(offset[2]),std::ref(valid_flags)); } }; @@ -1302,7 +1302,7 @@ struct AntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); + return std::make_tuple(std::ref(offset_uncert[0]),std::ref(offset_uncert[1]),std::ref(offset_uncert[2]),std::ref(valid_flags)); } }; @@ -1332,7 +1332,7 @@ struct MultiAntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(receiver_id,offset[0],offset[1],offset[2],valid_flags); + return std::make_tuple(std::ref(receiver_id),std::ref(offset[0]),std::ref(offset[1]),std::ref(offset[2]),std::ref(valid_flags)); } }; @@ -1360,7 +1360,7 @@ struct MultiAntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(receiver_id,offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); + return std::make_tuple(std::ref(receiver_id),std::ref(offset_uncert[0]),std::ref(offset_uncert[1]),std::ref(offset_uncert[2]),std::ref(valid_flags)); } }; @@ -1389,7 +1389,7 @@ struct MagnetometerOffset auto as_tuple() const { - return std::make_tuple(hard_iron[0],hard_iron[1],hard_iron[2],valid_flags); + return std::make_tuple(std::ref(hard_iron[0]),std::ref(hard_iron[1]),std::ref(hard_iron[2]),std::ref(valid_flags)); } }; @@ -1418,7 +1418,7 @@ struct MagnetometerMatrix auto as_tuple() const { - return std::make_tuple(soft_iron[0],soft_iron[1],soft_iron[2],soft_iron[3],soft_iron[4],soft_iron[5],soft_iron[6],soft_iron[7],soft_iron[8],valid_flags); + return std::make_tuple(std::ref(soft_iron[0]),std::ref(soft_iron[1]),std::ref(soft_iron[2]),std::ref(soft_iron[3]),std::ref(soft_iron[4]),std::ref(soft_iron[5]),std::ref(soft_iron[6]),std::ref(soft_iron[7]),std::ref(soft_iron[8]),std::ref(valid_flags)); } }; @@ -1445,7 +1445,7 @@ struct MagnetometerOffsetUncertainty auto as_tuple() const { - return std::make_tuple(hard_iron_uncertainty[0],hard_iron_uncertainty[1],hard_iron_uncertainty[2],valid_flags); + return std::make_tuple(std::ref(hard_iron_uncertainty[0]),std::ref(hard_iron_uncertainty[1]),std::ref(hard_iron_uncertainty[2]),std::ref(valid_flags)); } }; @@ -1472,7 +1472,7 @@ struct MagnetometerMatrixUncertainty auto as_tuple() const { - return std::make_tuple(soft_iron_uncertainty[0],soft_iron_uncertainty[1],soft_iron_uncertainty[2],soft_iron_uncertainty[3],soft_iron_uncertainty[4],soft_iron_uncertainty[5],soft_iron_uncertainty[6],soft_iron_uncertainty[7],soft_iron_uncertainty[8],valid_flags); + return std::make_tuple(std::ref(soft_iron_uncertainty[0]),std::ref(soft_iron_uncertainty[1]),std::ref(soft_iron_uncertainty[2]),std::ref(soft_iron_uncertainty[3]),std::ref(soft_iron_uncertainty[4]),std::ref(soft_iron_uncertainty[5]),std::ref(soft_iron_uncertainty[6]),std::ref(soft_iron_uncertainty[7]),std::ref(soft_iron_uncertainty[8]),std::ref(valid_flags)); } }; @@ -1498,7 +1498,7 @@ struct MagnetometerCovarianceMatrix auto as_tuple() const { - return std::make_tuple(covariance[0],covariance[1],covariance[2],covariance[3],covariance[4],covariance[5],covariance[6],covariance[7],covariance[8],valid_flags); + return std::make_tuple(std::ref(covariance[0]),std::ref(covariance[1]),std::ref(covariance[2]),std::ref(covariance[3]),std::ref(covariance[4]),std::ref(covariance[5]),std::ref(covariance[6]),std::ref(covariance[7]),std::ref(covariance[8]),std::ref(valid_flags)); } }; @@ -1525,7 +1525,7 @@ struct MagnetometerResidualVector auto as_tuple() const { - return std::make_tuple(residual[0],residual[1],residual[2],valid_flags); + return std::make_tuple(std::ref(residual[0]),std::ref(residual[1]),std::ref(residual[2]),std::ref(valid_flags)); } }; @@ -1554,7 +1554,7 @@ struct ClockCorrection auto as_tuple() const { - return std::make_tuple(receiver_id,bias,bias_drift,valid_flags); + return std::make_tuple(std::ref(receiver_id),std::ref(bias),std::ref(bias_drift),std::ref(valid_flags)); } }; @@ -1583,7 +1583,7 @@ struct ClockCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(receiver_id,bias_uncertainty,bias_drift_uncertainty,valid_flags); + return std::make_tuple(std::ref(receiver_id),std::ref(bias_uncertainty),std::ref(bias_drift_uncertainty),std::ref(valid_flags)); } }; @@ -1612,7 +1612,7 @@ struct GnssPosAidStatus auto as_tuple() const { - return std::make_tuple(receiver_id,time_of_week,status,reserved); + return std::make_tuple(std::ref(receiver_id),std::ref(time_of_week),std::ref(status),std::ref(reserved)); } }; @@ -1640,7 +1640,7 @@ struct GnssAttAidStatus auto as_tuple() const { - return std::make_tuple(time_of_week,status,reserved); + return std::make_tuple(std::ref(time_of_week),std::ref(status),std::ref(reserved)); } }; @@ -1674,7 +1674,7 @@ struct HeadAidStatus auto as_tuple() const { - return std::make_tuple(time_of_week,type,reserved); + return std::make_tuple(std::ref(time_of_week),std::ref(type),std::ref(reserved)); } }; @@ -1701,7 +1701,7 @@ struct RelPosNed auto as_tuple() const { - return std::make_tuple(relative_position[0],relative_position[1],relative_position[2],valid_flags); + return std::make_tuple(std::ref(relative_position[0]),std::ref(relative_position[1]),std::ref(relative_position[2]),std::ref(valid_flags)); } }; @@ -1728,7 +1728,7 @@ struct EcefPos auto as_tuple() const { - return std::make_tuple(position_ecef[0],position_ecef[1],position_ecef[2],valid_flags); + return std::make_tuple(std::ref(position_ecef[0]),std::ref(position_ecef[1]),std::ref(position_ecef[2]),std::ref(valid_flags)); } }; @@ -1755,7 +1755,7 @@ struct EcefVel auto as_tuple() const { - return std::make_tuple(velocity_ecef[0],velocity_ecef[1],velocity_ecef[2],valid_flags); + return std::make_tuple(std::ref(velocity_ecef[0]),std::ref(velocity_ecef[1]),std::ref(velocity_ecef[2]),std::ref(valid_flags)); } }; @@ -1782,7 +1782,7 @@ struct EcefPosUncertainty auto as_tuple() const { - return std::make_tuple(pos_uncertainty[0],pos_uncertainty[1],pos_uncertainty[2],valid_flags); + return std::make_tuple(std::ref(pos_uncertainty[0]),std::ref(pos_uncertainty[1]),std::ref(pos_uncertainty[2]),std::ref(valid_flags)); } }; @@ -1809,7 +1809,7 @@ struct EcefVelUncertainty auto as_tuple() const { - return std::make_tuple(vel_uncertainty[0],vel_uncertainty[1],vel_uncertainty[2],valid_flags); + return std::make_tuple(std::ref(vel_uncertainty[0]),std::ref(vel_uncertainty[1]),std::ref(vel_uncertainty[2]),std::ref(valid_flags)); } }; @@ -1838,7 +1838,7 @@ struct AidingMeasurementSummary auto as_tuple() const { - return std::make_tuple(time_of_week,source,type,indicator); + return std::make_tuple(std::ref(time_of_week),std::ref(source),std::ref(type),std::ref(indicator)); } }; @@ -1865,7 +1865,7 @@ struct OdometerScaleFactorError auto as_tuple() const { - return std::make_tuple(scale_factor_error,valid_flags); + return std::make_tuple(std::ref(scale_factor_error),std::ref(valid_flags)); } }; @@ -1892,7 +1892,7 @@ struct OdometerScaleFactorErrorUncertainty auto as_tuple() const { - return std::make_tuple(scale_factor_error_uncertainty,valid_flags); + return std::make_tuple(std::ref(scale_factor_error_uncertainty),std::ref(valid_flags)); } }; @@ -1961,7 +1961,7 @@ struct GnssDualAntennaStatus auto as_tuple() const { - return std::make_tuple(time_of_week,heading,heading_unc,fix_type,status_flags,valid_flags); + return std::make_tuple(std::ref(time_of_week),std::ref(heading),std::ref(heading_unc),std::ref(fix_type),std::ref(status_flags),std::ref(valid_flags)); } }; diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index c1c694858..e8425f0b5 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -230,7 +230,7 @@ struct PosLlh auto as_tuple() const { - return std::make_tuple(latitude,longitude,ellipsoid_height,msl_height,horizontal_accuracy,vertical_accuracy,valid_flags); + return std::make_tuple(std::ref(latitude),std::ref(longitude),std::ref(ellipsoid_height),std::ref(msl_height),std::ref(horizontal_accuracy),std::ref(vertical_accuracy),std::ref(valid_flags)); } }; @@ -289,7 +289,7 @@ struct PosEcef auto as_tuple() const { - return std::make_tuple(x[0],x[1],x[2],x_accuracy,valid_flags); + return std::make_tuple(std::ref(x[0]),std::ref(x[1]),std::ref(x[2]),std::ref(x_accuracy),std::ref(valid_flags)); } }; @@ -364,7 +364,7 @@ struct VelNed auto as_tuple() const { - return std::make_tuple(v[0],v[1],v[2],speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); + return std::make_tuple(std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(speed),std::ref(ground_speed),std::ref(heading),std::ref(speed_accuracy),std::ref(heading_accuracy),std::ref(valid_flags)); } }; @@ -423,7 +423,7 @@ struct VelEcef auto as_tuple() const { - return std::make_tuple(v[0],v[1],v[2],v_accuracy,valid_flags); + return std::make_tuple(std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(v_accuracy),std::ref(valid_flags)); } }; @@ -502,7 +502,7 @@ struct Dop auto as_tuple() const { - return std::make_tuple(gdop,pdop,hdop,vdop,tdop,ndop,edop,valid_flags); + return std::make_tuple(std::ref(gdop),std::ref(pdop),std::ref(hdop),std::ref(vdop),std::ref(tdop),std::ref(ndop),std::ref(edop),std::ref(valid_flags)); } }; @@ -566,7 +566,7 @@ struct UtcTime auto as_tuple() const { - return std::make_tuple(year,month,day,hour,min,sec,msec,valid_flags); + return std::make_tuple(std::ref(year),std::ref(month),std::ref(day),std::ref(hour),std::ref(min),std::ref(sec),std::ref(msec),std::ref(valid_flags)); } }; @@ -625,7 +625,7 @@ struct GpsTime auto as_tuple() const { - return std::make_tuple(tow,week_number,valid_flags); + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); } }; @@ -688,7 +688,7 @@ struct ClockInfo auto as_tuple() const { - return std::make_tuple(bias,drift,accuracy_estimate,valid_flags); + return std::make_tuple(std::ref(bias),std::ref(drift),std::ref(accuracy_estimate),std::ref(valid_flags)); } }; @@ -790,7 +790,7 @@ struct FixInfo auto as_tuple() const { - return std::make_tuple(fix_type,num_sv,fix_flags,valid_flags); + return std::make_tuple(std::ref(fix_type),std::ref(num_sv),std::ref(fix_flags),std::ref(valid_flags)); } }; @@ -895,7 +895,7 @@ struct SvInfo auto as_tuple() const { - return std::make_tuple(channel,sv_id,carrier_noise_ratio,azimuth,elevation,sv_flags,valid_flags); + return std::make_tuple(std::ref(channel),std::ref(sv_id),std::ref(carrier_noise_ratio),std::ref(azimuth),std::ref(elevation),std::ref(sv_flags),std::ref(valid_flags)); } }; @@ -981,7 +981,7 @@ struct HwStatus auto as_tuple() const { - return std::make_tuple(receiver_state,antenna_state,antenna_power,valid_flags); + return std::make_tuple(std::ref(receiver_state),std::ref(antenna_state),std::ref(antenna_power),std::ref(valid_flags)); } }; @@ -1060,7 +1060,7 @@ struct DgpsInfo auto as_tuple() const { - return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); + return std::make_tuple(std::ref(sv_id),std::ref(age),std::ref(range_correction),std::ref(range_rate_correction),std::ref(valid_flags)); } }; @@ -1129,7 +1129,7 @@ struct DgpsChannel auto as_tuple() const { - return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); + return std::make_tuple(std::ref(sv_id),std::ref(age),std::ref(range_correction),std::ref(range_rate_correction),std::ref(valid_flags)); } }; @@ -1198,7 +1198,7 @@ struct ClockInfo2 auto as_tuple() const { - return std::make_tuple(bias,drift,bias_accuracy_estimate,drift_accuracy_estimate,valid_flags); + return std::make_tuple(std::ref(bias),std::ref(drift),std::ref(bias_accuracy_estimate),std::ref(drift_accuracy_estimate),std::ref(valid_flags)); } }; @@ -1250,7 +1250,7 @@ struct GpsLeapSeconds auto as_tuple() const { - return std::make_tuple(leap_seconds,valid_flags); + return std::make_tuple(std::ref(leap_seconds),std::ref(valid_flags)); } }; @@ -1359,7 +1359,7 @@ struct SbasInfo auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,sbas_system,sbas_id,count,sbas_status,valid_flags); + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(sbas_system),std::ref(sbas_id),std::ref(count),std::ref(sbas_status),std::ref(valid_flags)); } }; @@ -1450,7 +1450,7 @@ struct SbasCorrection auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,gnss_id,sv_id,udrei,pseudorange_correction,iono_correction,valid_flags); + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(gnss_id),std::ref(sv_id),std::ref(udrei),std::ref(pseudorange_correction),std::ref(iono_correction),std::ref(valid_flags)); } }; @@ -1538,7 +1538,7 @@ struct RfErrorDetection auto as_tuple() const { - return std::make_tuple(rf_band,jamming_state,spoofing_state,reserved,valid_flags); + return std::make_tuple(std::ref(rf_band),std::ref(jamming_state),std::ref(spoofing_state),std::ref(reserved),std::ref(valid_flags)); } }; @@ -1664,7 +1664,7 @@ struct BaseStationInfo auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,ecef_pos[0],ecef_pos[1],ecef_pos[2],height,station_id,indicators,valid_flags); + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(ecef_pos[0]),std::ref(ecef_pos[1]),std::ref(ecef_pos[2]),std::ref(height),std::ref(station_id),std::ref(indicators),std::ref(valid_flags)); } }; @@ -1796,7 +1796,7 @@ struct RtkCorrectionsStatus auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved,valid_flags); + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(epoch_status),std::ref(dongle_status),std::ref(gps_correction_latency),std::ref(glonass_correction_latency),std::ref(galileo_correction_latency),std::ref(beidou_correction_latency),std::ref(reserved),std::ref(valid_flags)); } }; @@ -1877,7 +1877,7 @@ struct SatelliteStatus auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,gnss_id,satellite_id,elevation,azimuth,health,valid_flags); + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(gnss_id),std::ref(satellite_id),std::ref(elevation),std::ref(azimuth),std::ref(health),std::ref(valid_flags)); } }; @@ -2004,7 +2004,7 @@ struct Raw auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,receiver_id,tracking_channel,gnss_id,satellite_id,signal_id,signal_strength,quality,pseudorange,carrier_phase,doppler,range_uncert,phase_uncert,doppler_uncert,lock_time,valid_flags); + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(receiver_id),std::ref(tracking_channel),std::ref(gnss_id),std::ref(satellite_id),std::ref(signal_id),std::ref(signal_strength),std::ref(quality),std::ref(pseudorange),std::ref(carrier_phase),std::ref(doppler),std::ref(range_uncert),std::ref(phase_uncert),std::ref(doppler_uncert),std::ref(lock_time),std::ref(valid_flags)); } }; @@ -2094,7 +2094,7 @@ struct GpsEphemeris auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(health),std::ref(iodc),std::ref(iode),std::ref(t_oc),std::ref(af0),std::ref(af1),std::ref(af2),std::ref(t_gd),std::ref(ISC_L1CA),std::ref(ISC_L2C),std::ref(t_oe),std::ref(a),std::ref(a_dot),std::ref(mean_anomaly),std::ref(delta_mean_motion),std::ref(delta_mean_motion_dot),std::ref(eccentricity),std::ref(argument_of_perigee),std::ref(omega),std::ref(omega_dot),std::ref(inclination),std::ref(inclination_dot),std::ref(c_ic),std::ref(c_is),std::ref(c_uc),std::ref(c_us),std::ref(c_rc),std::ref(c_rs),std::ref(valid_flags)); } }; @@ -2184,7 +2184,7 @@ struct GalileoEphemeris auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(health),std::ref(iodc),std::ref(iode),std::ref(t_oc),std::ref(af0),std::ref(af1),std::ref(af2),std::ref(t_gd),std::ref(ISC_L1CA),std::ref(ISC_L2C),std::ref(t_oe),std::ref(a),std::ref(a_dot),std::ref(mean_anomaly),std::ref(delta_mean_motion),std::ref(delta_mean_motion_dot),std::ref(eccentricity),std::ref(argument_of_perigee),std::ref(omega),std::ref(omega_dot),std::ref(inclination),std::ref(inclination_dot),std::ref(c_ic),std::ref(c_is),std::ref(c_uc),std::ref(c_us),std::ref(c_rc),std::ref(c_rs),std::ref(valid_flags)); } }; @@ -2262,7 +2262,7 @@ struct GloEphemeris auto as_tuple() const { - return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x[0],x[1],x[2],v[0],v[1],v[2],a[0],a[1],a[2],health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(freq_number),std::ref(tk),std::ref(tb),std::ref(sat_type),std::ref(gamma),std::ref(tau_n),std::ref(x[0]),std::ref(x[1]),std::ref(x[2]),std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(a[0]),std::ref(a[1]),std::ref(a[2]),std::ref(health),std::ref(P),std::ref(NT),std::ref(delta_tau_n),std::ref(Ft),std::ref(En),std::ref(P1),std::ref(P2),std::ref(P3),std::ref(P4),std::ref(valid_flags)); } }; @@ -2329,7 +2329,7 @@ struct GpsIonoCorr auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,alpha,beta,valid_flags); + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(alpha),std::ref(beta),std::ref(valid_flags)); } }; @@ -2396,7 +2396,7 @@ struct GalileoIonoCorr auto as_tuple() const { - return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],disturbance_flags,valid_flags); + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(alpha[0]),std::ref(alpha[1]),std::ref(alpha[2]),std::ref(disturbance_flags),std::ref(valid_flags)); } }; diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index f645fc38c..2b7591329 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -87,7 +87,7 @@ struct RawAccel auto as_tuple() const { - return std::make_tuple(raw_accel[0],raw_accel[1],raw_accel[2]); + return std::make_tuple(std::ref(raw_accel[0]),std::ref(raw_accel[1]),std::ref(raw_accel[2])); } }; @@ -114,7 +114,7 @@ struct RawGyro auto as_tuple() const { - return std::make_tuple(raw_gyro[0],raw_gyro[1],raw_gyro[2]); + return std::make_tuple(std::ref(raw_gyro[0]),std::ref(raw_gyro[1]),std::ref(raw_gyro[2])); } }; @@ -141,7 +141,7 @@ struct RawMag auto as_tuple() const { - return std::make_tuple(raw_mag[0],raw_mag[1],raw_mag[2]); + return std::make_tuple(std::ref(raw_mag[0]),std::ref(raw_mag[1]),std::ref(raw_mag[2])); } }; @@ -168,7 +168,7 @@ struct RawPressure auto as_tuple() const { - return std::make_tuple(raw_pressure); + return std::make_tuple(std::ref(raw_pressure)); } }; @@ -195,7 +195,7 @@ struct ScaledAccel auto as_tuple() const { - return std::make_tuple(scaled_accel[0],scaled_accel[1],scaled_accel[2]); + return std::make_tuple(std::ref(scaled_accel[0]),std::ref(scaled_accel[1]),std::ref(scaled_accel[2])); } }; @@ -222,7 +222,7 @@ struct ScaledGyro auto as_tuple() const { - return std::make_tuple(scaled_gyro[0],scaled_gyro[1],scaled_gyro[2]); + return std::make_tuple(std::ref(scaled_gyro[0]),std::ref(scaled_gyro[1]),std::ref(scaled_gyro[2])); } }; @@ -249,7 +249,7 @@ struct ScaledMag auto as_tuple() const { - return std::make_tuple(scaled_mag[0],scaled_mag[1],scaled_mag[2]); + return std::make_tuple(std::ref(scaled_mag[0]),std::ref(scaled_mag[1]),std::ref(scaled_mag[2])); } }; @@ -275,7 +275,7 @@ struct ScaledPressure auto as_tuple() const { - return std::make_tuple(scaled_pressure); + return std::make_tuple(std::ref(scaled_pressure)); } }; @@ -302,7 +302,7 @@ struct DeltaTheta auto as_tuple() const { - return std::make_tuple(delta_theta[0],delta_theta[1],delta_theta[2]); + return std::make_tuple(std::ref(delta_theta[0]),std::ref(delta_theta[1]),std::ref(delta_theta[2])); } }; @@ -329,7 +329,7 @@ struct DeltaVelocity auto as_tuple() const { - return std::make_tuple(delta_velocity[0],delta_velocity[1],delta_velocity[2]); + return std::make_tuple(std::ref(delta_velocity[0]),std::ref(delta_velocity[1]),std::ref(delta_velocity[2])); } }; @@ -365,7 +365,7 @@ struct CompOrientationMatrix auto as_tuple() const { - return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); + return std::make_tuple(std::ref(m[0]),std::ref(m[1]),std::ref(m[2]),std::ref(m[3]),std::ref(m[4]),std::ref(m[5]),std::ref(m[6]),std::ref(m[7]),std::ref(m[8])); } }; @@ -399,7 +399,7 @@ struct CompQuaternion auto as_tuple() const { - return std::make_tuple(q[0],q[1],q[2],q[3]); + return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3])); } }; @@ -428,7 +428,7 @@ struct CompEulerAngles auto as_tuple() const { - return std::make_tuple(roll,pitch,yaw); + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); } }; @@ -454,7 +454,7 @@ struct CompOrientationUpdateMatrix auto as_tuple() const { - return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); + return std::make_tuple(std::ref(m[0]),std::ref(m[1]),std::ref(m[2]),std::ref(m[3]),std::ref(m[4]),std::ref(m[5]),std::ref(m[6]),std::ref(m[7]),std::ref(m[8])); } }; @@ -480,7 +480,7 @@ struct OrientationRawTemp auto as_tuple() const { - return std::make_tuple(raw_temp); + return std::make_tuple(std::ref(raw_temp)); } }; @@ -506,7 +506,7 @@ struct InternalTimestamp auto as_tuple() const { - return std::make_tuple(counts); + return std::make_tuple(std::ref(counts)); } }; @@ -533,7 +533,7 @@ struct PpsTimestamp auto as_tuple() const { - return std::make_tuple(seconds,useconds); + return std::make_tuple(std::ref(seconds),std::ref(useconds)); } }; @@ -604,7 +604,7 @@ struct GpsTimestamp auto as_tuple() const { - return std::make_tuple(tow,week_number,valid_flags); + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); } }; @@ -636,7 +636,7 @@ struct TemperatureAbs auto as_tuple() const { - return std::make_tuple(min_temp,max_temp,mean_temp); + return std::make_tuple(std::ref(min_temp),std::ref(max_temp),std::ref(mean_temp)); } }; @@ -668,7 +668,7 @@ struct UpVector auto as_tuple() const { - return std::make_tuple(up[0],up[1],up[2]); + return std::make_tuple(std::ref(up[0]),std::ref(up[1]),std::ref(up[2])); } }; @@ -697,7 +697,7 @@ struct NorthVector auto as_tuple() const { - return std::make_tuple(north[0],north[1],north[2]); + return std::make_tuple(std::ref(north[0]),std::ref(north[1]),std::ref(north[2])); } }; @@ -774,7 +774,7 @@ struct OverrangeStatus auto as_tuple() const { - return std::make_tuple(status); + return std::make_tuple(std::ref(status)); } }; @@ -801,7 +801,7 @@ struct OdometerData auto as_tuple() const { - return std::make_tuple(speed,uncertainty,valid_flags); + return std::make_tuple(std::ref(speed),std::ref(uncertainty),std::ref(valid_flags)); } }; diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 633904ce6..1927ae2cd 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -74,7 +74,7 @@ struct EventSource auto as_tuple() const { - return std::make_tuple(trigger_id); + return std::make_tuple(std::ref(trigger_id)); } }; @@ -103,7 +103,7 @@ struct Ticks auto as_tuple() const { - return std::make_tuple(ticks); + return std::make_tuple(std::ref(ticks)); } }; @@ -133,7 +133,7 @@ struct DeltaTicks auto as_tuple() const { - return std::make_tuple(ticks); + return std::make_tuple(std::ref(ticks)); } }; @@ -195,7 +195,7 @@ struct GpsTimestamp auto as_tuple() const { - return std::make_tuple(tow,week_number,valid_flags); + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); } }; @@ -230,7 +230,7 @@ struct DeltaTime auto as_tuple() const { - return std::make_tuple(seconds); + return std::make_tuple(std::ref(seconds)); } }; @@ -263,7 +263,7 @@ struct ReferenceTimestamp auto as_tuple() const { - return std::make_tuple(nanoseconds); + return std::make_tuple(std::ref(nanoseconds)); } }; @@ -298,7 +298,7 @@ struct ReferenceTimeDelta auto as_tuple() const { - return std::make_tuple(dt_nanos); + return std::make_tuple(std::ref(dt_nanos)); } }; @@ -358,7 +358,7 @@ struct ExternalTimestamp auto as_tuple() const { - return std::make_tuple(nanoseconds,valid_flags); + return std::make_tuple(std::ref(nanoseconds),std::ref(valid_flags)); } }; @@ -422,7 +422,7 @@ struct ExternalTimeDelta auto as_tuple() const { - return std::make_tuple(dt_nanos,valid_flags); + return std::make_tuple(std::ref(dt_nanos),std::ref(valid_flags)); } }; diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 2f45d70f7..7568b3754 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -82,7 +82,7 @@ struct BuiltInTest auto as_tuple() const { - return std::make_tuple(result); + return std::make_tuple(std::ref(result)); } }; @@ -109,7 +109,7 @@ struct TimeSyncStatus auto as_tuple() const { - return std::make_tuple(time_sync,last_pps_rcvd); + return std::make_tuple(std::ref(time_sync),std::ref(last_pps_rcvd)); } }; @@ -153,7 +153,7 @@ struct GpioState auto as_tuple() const { - return std::make_tuple(states); + return std::make_tuple(std::ref(states)); } }; @@ -181,7 +181,7 @@ struct GpioAnalogValue auto as_tuple() const { - return std::make_tuple(gpio_id,value); + return std::make_tuple(std::ref(gpio_id),std::ref(value)); } }; From 7889c9946ccf42d892e630e574d4f582e876ae34 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 4 Aug 2023 12:10:39 -0400 Subject: [PATCH 100/252] Add missing include. --- src/mip/definitions/common.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 51610aad2..ec283653a 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -7,6 +7,7 @@ #ifdef __cplusplus +#include #include #include From 96cbc117561a7596075e731e75aea86123eddc98 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 4 Aug 2023 13:07:51 -0400 Subject: [PATCH 101/252] Generate MIP definitions from branches/dev@55208. --- src/mip/definitions/commands_3dm.hpp | 307 +++++++++++++++------ src/mip/definitions/commands_aiding.hpp | 57 +++- src/mip/definitions/commands_base.hpp | 54 +++- src/mip/definitions/commands_filter.hpp | 350 ++++++++++++++++++------ src/mip/definitions/commands_gnss.hpp | 19 +- src/mip/definitions/commands_rtk.hpp | 65 ++++- src/mip/definitions/commands_system.hpp | 7 +- src/mip/definitions/data_filter.hpp | 330 ++++++++++++++++++---- src/mip/definitions/data_gnss.hpp | 162 +++++++++-- src/mip/definitions/data_sensor.hpp | 138 ++++++++-- src/mip/definitions/data_shared.hpp | 54 +++- src/mip/definitions/data_system.hpp | 24 +- 12 files changed, 1238 insertions(+), 329 deletions(-) diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index abae7e91d..93e639253 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -218,9 +218,13 @@ struct PollImuMessage auto as_tuple() const { - return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(suppress_ack,num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + } typedef void Response; }; void insert(Serializer& serializer, const PollImuMessage& self); @@ -257,9 +261,13 @@ struct PollGnssMessage auto as_tuple() const { - return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(suppress_ack,num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + } typedef void Response; }; void insert(Serializer& serializer, const PollGnssMessage& self); @@ -296,9 +304,13 @@ struct PollFilterMessage auto as_tuple() const { - return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(suppress_ack,num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + } typedef void Response; }; void insert(Serializer& serializer, const PollFilterMessage& self); @@ -336,9 +348,13 @@ struct ImuMessageFormat auto as_tuple() const { - return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + } static ImuMessageFormat create_sld_all(::mip::FunctionSelector function) { @@ -362,7 +378,6 @@ struct ImuMessageFormat { return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); } - }; }; void insert(Serializer& serializer, const ImuMessageFormat& self); @@ -407,9 +422,13 @@ struct GpsMessageFormat auto as_tuple() const { - return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + } static GpsMessageFormat create_sld_all(::mip::FunctionSelector function) { @@ -433,7 +452,6 @@ struct GpsMessageFormat { return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); } - }; }; void insert(Serializer& serializer, const GpsMessageFormat& self); @@ -478,9 +496,13 @@ struct FilterMessageFormat auto as_tuple() const { - return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); + } static FilterMessageFormat create_sld_all(::mip::FunctionSelector function) { @@ -504,7 +526,6 @@ struct FilterMessageFormat { return std::make_tuple(std::ref(num_descriptors),std::ref(descriptors)); } - }; }; void insert(Serializer& serializer, const FilterMessageFormat& self); @@ -545,6 +566,10 @@ struct ImuGetBaseRate return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -559,7 +584,6 @@ struct ImuGetBaseRate { return std::make_tuple(std::ref(rate)); } - }; }; void insert(Serializer& serializer, const ImuGetBaseRate& self); @@ -596,6 +620,10 @@ struct GpsGetBaseRate return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -610,7 +638,6 @@ struct GpsGetBaseRate { return std::make_tuple(std::ref(rate)); } - }; }; void insert(Serializer& serializer, const GpsGetBaseRate& self); @@ -647,6 +674,10 @@ struct FilterGetBaseRate return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -661,7 +692,6 @@ struct FilterGetBaseRate { return std::make_tuple(std::ref(rate)); } - }; }; void insert(Serializer& serializer, const FilterGetBaseRate& self); @@ -702,9 +732,13 @@ struct PollData auto as_tuple() const { - return std::make_tuple(std::ref(desc_set),std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(desc_set,suppress_ack,num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(suppress_ack),std::ref(num_descriptors),std::ref(descriptors)); + } typedef void Response; }; void insert(Serializer& serializer, const PollData& self); @@ -733,9 +767,13 @@ struct GetBaseRate auto as_tuple() const { - return std::make_tuple(std::ref(desc_set)); + return std::make_tuple(desc_set); } + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set)); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -751,7 +789,6 @@ struct GetBaseRate { return std::make_tuple(std::ref(desc_set),std::ref(rate)); } - }; }; void insert(Serializer& serializer, const GetBaseRate& self); @@ -793,9 +830,13 @@ struct MessageFormat auto as_tuple() const { - return std::make_tuple(std::ref(desc_set),std::ref(num_descriptors),std::ref(descriptors)); + return std::make_tuple(desc_set,num_descriptors,descriptors); } + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(num_descriptors),std::ref(descriptors)); + } static MessageFormat create_sld_all(::mip::FunctionSelector function) { @@ -821,7 +862,6 @@ struct MessageFormat { return std::make_tuple(std::ref(desc_set),std::ref(num_descriptors),std::ref(descriptors)); } - }; }; void insert(Serializer& serializer, const MessageFormat& self); @@ -864,9 +904,13 @@ struct NmeaPollData auto as_tuple() const { - return std::make_tuple(std::ref(suppress_ack),std::ref(count),std::ref(format_entries)); + return std::make_tuple(suppress_ack,count,format_entries); } + auto as_tuple() + { + return std::make_tuple(std::ref(suppress_ack),std::ref(count),std::ref(format_entries)); + } typedef void Response; }; void insert(Serializer& serializer, const NmeaPollData& self); @@ -902,9 +946,13 @@ struct NmeaMessageFormat auto as_tuple() const { - return std::make_tuple(std::ref(count),std::ref(format_entries)); + return std::make_tuple(count,format_entries); } + auto as_tuple() + { + return std::make_tuple(std::ref(count),std::ref(format_entries)); + } static NmeaMessageFormat create_sld_all(::mip::FunctionSelector function) { @@ -928,7 +976,6 @@ struct NmeaMessageFormat { return std::make_tuple(std::ref(count),std::ref(format_entries)); } - }; }; void insert(Serializer& serializer, const NmeaMessageFormat& self); @@ -975,6 +1022,10 @@ struct DeviceSettings return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } static DeviceSettings create_sld_all(::mip::FunctionSelector function) { @@ -1033,9 +1084,13 @@ struct UartBaudrate auto as_tuple() const { - return std::make_tuple(std::ref(baud)); + return std::make_tuple(baud); } + auto as_tuple() + { + return std::make_tuple(std::ref(baud)); + } static UartBaudrate create_sld_all(::mip::FunctionSelector function) { @@ -1058,7 +1113,6 @@ struct UartBaudrate { return std::make_tuple(std::ref(baud)); } - }; }; void insert(Serializer& serializer, const UartBaudrate& self); @@ -1104,9 +1158,13 @@ struct FactoryStreaming auto as_tuple() const { - return std::make_tuple(std::ref(action),std::ref(reserved)); + return std::make_tuple(action,reserved); } + auto as_tuple() + { + return std::make_tuple(std::ref(action),std::ref(reserved)); + } typedef void Response; }; void insert(Serializer& serializer, const FactoryStreaming& self); @@ -1151,9 +1209,13 @@ struct DatastreamControl auto as_tuple() const { - return std::make_tuple(std::ref(desc_set),std::ref(enable)); + return std::make_tuple(desc_set,enable); } + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(enable)); + } static DatastreamControl create_sld_all(::mip::FunctionSelector function) { @@ -1178,7 +1240,6 @@ struct DatastreamControl { return std::make_tuple(std::ref(desc_set),std::ref(enabled)); } - }; }; void insert(Serializer& serializer, const DatastreamControl& self); @@ -1283,9 +1344,13 @@ struct ConstellationSettings auto as_tuple() const { - return std::make_tuple(std::ref(max_channels),std::ref(config_count),std::ref(settings)); + return std::make_tuple(max_channels,config_count,settings); } + auto as_tuple() + { + return std::make_tuple(std::ref(max_channels),std::ref(config_count),std::ref(settings)); + } static ConstellationSettings create_sld_all(::mip::FunctionSelector function) { @@ -1311,7 +1376,6 @@ struct ConstellationSettings { return std::make_tuple(std::ref(max_channels_available),std::ref(max_channels_use),std::ref(config_count),std::ref(settings)); } - }; }; void insert(Serializer& serializer, const ConstellationSettings& self); @@ -1393,9 +1457,13 @@ struct GnssSbasSettings auto as_tuple() const { - return std::make_tuple(std::ref(enable_sbas),std::ref(sbas_options),std::ref(num_included_prns),std::ref(included_prns)); + return std::make_tuple(enable_sbas,sbas_options,num_included_prns,included_prns); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable_sbas),std::ref(sbas_options),std::ref(num_included_prns),std::ref(included_prns)); + } static GnssSbasSettings create_sld_all(::mip::FunctionSelector function) { @@ -1421,7 +1489,6 @@ struct GnssSbasSettings { return std::make_tuple(std::ref(enable_sbas),std::ref(sbas_options),std::ref(num_included_prns),std::ref(included_prns)); } - }; }; void insert(Serializer& serializer, const GnssSbasSettings& self); @@ -1480,9 +1547,13 @@ struct GnssAssistedFix auto as_tuple() const { - return std::make_tuple(std::ref(option),std::ref(flags)); + return std::make_tuple(option,flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(option),std::ref(flags)); + } static GnssAssistedFix create_sld_all(::mip::FunctionSelector function) { @@ -1506,7 +1577,6 @@ struct GnssAssistedFix { return std::make_tuple(std::ref(option),std::ref(flags)); } - }; }; void insert(Serializer& serializer, const GnssAssistedFix& self); @@ -1553,9 +1623,13 @@ struct GnssTimeAssistance auto as_tuple() const { - return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(accuracy)); + return std::make_tuple(tow,week_number,accuracy); } + auto as_tuple() + { + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(accuracy)); + } static GnssTimeAssistance create_sld_all(::mip::FunctionSelector function) { @@ -1580,7 +1654,6 @@ struct GnssTimeAssistance { return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(accuracy)); } - }; }; void insert(Serializer& serializer, const GnssTimeAssistance& self); @@ -1638,9 +1711,13 @@ struct ImuLowpassFilter auto as_tuple() const { - return std::make_tuple(std::ref(target_descriptor),std::ref(enable),std::ref(manual),std::ref(frequency),std::ref(reserved)); + return std::make_tuple(target_descriptor,enable,manual,frequency,reserved); } + auto as_tuple() + { + return std::make_tuple(std::ref(target_descriptor),std::ref(enable),std::ref(manual),std::ref(frequency),std::ref(reserved)); + } static ImuLowpassFilter create_sld_all(::mip::FunctionSelector function) { @@ -1668,7 +1745,6 @@ struct ImuLowpassFilter { return std::make_tuple(std::ref(target_descriptor),std::ref(enable),std::ref(manual),std::ref(frequency),std::ref(reserved)); } - }; }; void insert(Serializer& serializer, const ImuLowpassFilter& self); @@ -1719,9 +1795,13 @@ struct PpsSource auto as_tuple() const { - return std::make_tuple(std::ref(source)); + return std::make_tuple(source); } + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } static PpsSource create_sld_all(::mip::FunctionSelector function) { @@ -1744,7 +1824,6 @@ struct PpsSource { return std::make_tuple(std::ref(source)); } - }; }; void insert(Serializer& serializer, const PpsSource& self); @@ -1868,9 +1947,13 @@ struct GpioConfig auto as_tuple() const { - return std::make_tuple(std::ref(pin),std::ref(feature),std::ref(behavior),std::ref(pin_mode)); + return std::make_tuple(pin,feature,behavior,pin_mode); } + auto as_tuple() + { + return std::make_tuple(std::ref(pin),std::ref(feature),std::ref(behavior),std::ref(pin_mode)); + } static GpioConfig create_sld_all(::mip::FunctionSelector function) { @@ -1897,7 +1980,6 @@ struct GpioConfig { return std::make_tuple(std::ref(pin),std::ref(feature),std::ref(behavior),std::ref(pin_mode)); } - }; }; void insert(Serializer& serializer, const GpioConfig& self); @@ -1955,9 +2037,13 @@ struct GpioState auto as_tuple() const { - return std::make_tuple(std::ref(pin),std::ref(state)); + return std::make_tuple(pin,state); } + auto as_tuple() + { + return std::make_tuple(std::ref(pin),std::ref(state)); + } static GpioState create_sld_all(::mip::FunctionSelector function) { @@ -1981,7 +2067,6 @@ struct GpioState { return std::make_tuple(std::ref(pin),std::ref(state)); } - }; }; void insert(Serializer& serializer, const GpioState& self); @@ -2029,9 +2114,13 @@ struct Odometer auto as_tuple() const { - return std::make_tuple(std::ref(mode),std::ref(scaling),std::ref(uncertainty)); + return std::make_tuple(mode,scaling,uncertainty); } + auto as_tuple() + { + return std::make_tuple(std::ref(mode),std::ref(scaling),std::ref(uncertainty)); + } static Odometer create_sld_all(::mip::FunctionSelector function) { @@ -2056,7 +2145,6 @@ struct Odometer { return std::make_tuple(std::ref(mode),std::ref(scaling),std::ref(uncertainty)); } - }; }; void insert(Serializer& serializer, const Odometer& self); @@ -2120,9 +2208,13 @@ struct GetEventSupport auto as_tuple() const { - return std::make_tuple(std::ref(query)); + return std::make_tuple(query); } + auto as_tuple() + { + return std::make_tuple(std::ref(query)); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -2140,7 +2232,6 @@ struct GetEventSupport { return std::make_tuple(std::ref(query),std::ref(max_instances),std::ref(num_entries),std::ref(entries)); } - }; }; void insert(Serializer& serializer, const GetEventSupport& self); @@ -2199,9 +2290,13 @@ struct EventControl auto as_tuple() const { - return std::make_tuple(std::ref(instance),std::ref(mode)); + return std::make_tuple(instance,mode); } + auto as_tuple() + { + return std::make_tuple(std::ref(instance),std::ref(mode)); + } static EventControl create_sld_all(::mip::FunctionSelector function) { @@ -2226,7 +2321,6 @@ struct EventControl { return std::make_tuple(std::ref(instance),std::ref(mode)); } - }; }; void insert(Serializer& serializer, const EventControl& self); @@ -2299,9 +2393,13 @@ struct GetEventTriggerStatus auto as_tuple() const { - return std::make_tuple(std::ref(requested_count),std::ref(requested_instances)); + return std::make_tuple(requested_count,requested_instances); } + auto as_tuple() + { + return std::make_tuple(std::ref(requested_count),std::ref(requested_instances)); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -2317,7 +2415,6 @@ struct GetEventTriggerStatus { return std::make_tuple(std::ref(count),std::ref(triggers)); } - }; }; void insert(Serializer& serializer, const GetEventTriggerStatus& self); @@ -2358,9 +2455,13 @@ struct GetEventActionStatus auto as_tuple() const { - return std::make_tuple(std::ref(requested_count),std::ref(requested_instances)); + return std::make_tuple(requested_count,requested_instances); } + auto as_tuple() + { + return std::make_tuple(std::ref(requested_count),std::ref(requested_instances)); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -2376,7 +2477,6 @@ struct GetEventActionStatus { return std::make_tuple(std::ref(count),std::ref(actions)); } - }; }; void insert(Serializer& serializer, const GetEventActionStatus& self); @@ -2492,9 +2592,13 @@ struct EventTrigger auto as_tuple() const { - return std::make_tuple(std::ref(instance),std::ref(type),std::ref(parameters)); + return std::make_tuple(instance,type,parameters); } + auto as_tuple() + { + return std::make_tuple(std::ref(instance),std::ref(type),std::ref(parameters)); + } static EventTrigger create_sld_all(::mip::FunctionSelector function) { @@ -2520,7 +2624,6 @@ struct EventTrigger { return std::make_tuple(std::ref(instance),std::ref(type),std::ref(parameters)); } - }; }; void insert(Serializer& serializer, const EventTrigger& self); @@ -2612,9 +2715,13 @@ struct EventAction auto as_tuple() const { - return std::make_tuple(std::ref(instance),std::ref(trigger),std::ref(type),std::ref(parameters)); + return std::make_tuple(instance,trigger,type,parameters); } + auto as_tuple() + { + return std::make_tuple(std::ref(instance),std::ref(trigger),std::ref(type),std::ref(parameters)); + } static EventAction create_sld_all(::mip::FunctionSelector function) { @@ -2641,7 +2748,6 @@ struct EventAction { return std::make_tuple(std::ref(instance),std::ref(trigger),std::ref(type),std::ref(parameters)); } - }; }; void insert(Serializer& serializer, const EventAction& self); @@ -2691,9 +2797,13 @@ struct AccelBias auto as_tuple() const { - return std::make_tuple(std::ref(bias)); + return std::make_tuple(bias); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias)); + } static AccelBias create_sld_all(::mip::FunctionSelector function) { @@ -2716,7 +2826,6 @@ struct AccelBias { return std::make_tuple(std::ref(bias)); } - }; }; void insert(Serializer& serializer, const AccelBias& self); @@ -2760,9 +2869,13 @@ struct GyroBias auto as_tuple() const { - return std::make_tuple(std::ref(bias)); + return std::make_tuple(bias); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias)); + } static GyroBias create_sld_all(::mip::FunctionSelector function) { @@ -2785,7 +2898,6 @@ struct GyroBias { return std::make_tuple(std::ref(bias)); } - }; }; void insert(Serializer& serializer, const GyroBias& self); @@ -2826,9 +2938,13 @@ struct CaptureGyroBias auto as_tuple() const { - return std::make_tuple(std::ref(averaging_time_ms)); + return std::make_tuple(averaging_time_ms); } + auto as_tuple() + { + return std::make_tuple(std::ref(averaging_time_ms)); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -2843,7 +2959,6 @@ struct CaptureGyroBias { return std::make_tuple(std::ref(bias)); } - }; }; void insert(Serializer& serializer, const CaptureGyroBias& self); @@ -2887,9 +3002,13 @@ struct MagHardIronOffset auto as_tuple() const { - return std::make_tuple(std::ref(offset)); + return std::make_tuple(offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } static MagHardIronOffset create_sld_all(::mip::FunctionSelector function) { @@ -2912,7 +3031,6 @@ struct MagHardIronOffset { return std::make_tuple(std::ref(offset)); } - }; }; void insert(Serializer& serializer, const MagHardIronOffset& self); @@ -2964,9 +3082,13 @@ struct MagSoftIronMatrix auto as_tuple() const { - return std::make_tuple(std::ref(offset)); + return std::make_tuple(offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } static MagSoftIronMatrix create_sld_all(::mip::FunctionSelector function) { @@ -2989,7 +3111,6 @@ struct MagSoftIronMatrix { return std::make_tuple(std::ref(offset)); } - }; }; void insert(Serializer& serializer, const MagSoftIronMatrix& self); @@ -3031,9 +3152,13 @@ struct ConingScullingEnable auto as_tuple() const { - return std::make_tuple(std::ref(enable)); + return std::make_tuple(enable); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } static ConingScullingEnable create_sld_all(::mip::FunctionSelector function) { @@ -3056,7 +3181,6 @@ struct ConingScullingEnable { return std::make_tuple(std::ref(enable)); } - }; }; void insert(Serializer& serializer, const ConingScullingEnable& self); @@ -3124,9 +3248,13 @@ struct Sensor2VehicleTransformEuler auto as_tuple() const { - return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + return std::make_tuple(roll,pitch,yaw); } + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + } static Sensor2VehicleTransformEuler create_sld_all(::mip::FunctionSelector function) { @@ -3151,7 +3279,6 @@ struct Sensor2VehicleTransformEuler { return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); } - }; }; void insert(Serializer& serializer, const Sensor2VehicleTransformEuler& self); @@ -3225,9 +3352,13 @@ struct Sensor2VehicleTransformQuaternion auto as_tuple() const { - return std::make_tuple(std::ref(q)); + return std::make_tuple(q); } + auto as_tuple() + { + return std::make_tuple(std::ref(q)); + } static Sensor2VehicleTransformQuaternion create_sld_all(::mip::FunctionSelector function) { @@ -3250,7 +3381,6 @@ struct Sensor2VehicleTransformQuaternion { return std::make_tuple(std::ref(q)); } - }; }; void insert(Serializer& serializer, const Sensor2VehicleTransformQuaternion& self); @@ -3322,9 +3452,13 @@ struct Sensor2VehicleTransformDcm auto as_tuple() const { - return std::make_tuple(std::ref(dcm)); + return std::make_tuple(dcm); } + auto as_tuple() + { + return std::make_tuple(std::ref(dcm)); + } static Sensor2VehicleTransformDcm create_sld_all(::mip::FunctionSelector function) { @@ -3347,7 +3481,6 @@ struct Sensor2VehicleTransformDcm { return std::make_tuple(std::ref(dcm)); } - }; }; void insert(Serializer& serializer, const Sensor2VehicleTransformDcm& self); @@ -3396,9 +3529,13 @@ struct ComplementaryFilter auto as_tuple() const { - return std::make_tuple(std::ref(pitch_roll_enable),std::ref(heading_enable),std::ref(pitch_roll_time_constant),std::ref(heading_time_constant)); + return std::make_tuple(pitch_roll_enable,heading_enable,pitch_roll_time_constant,heading_time_constant); } + auto as_tuple() + { + return std::make_tuple(std::ref(pitch_roll_enable),std::ref(heading_enable),std::ref(pitch_roll_time_constant),std::ref(heading_time_constant)); + } static ComplementaryFilter create_sld_all(::mip::FunctionSelector function) { @@ -3424,7 +3561,6 @@ struct ComplementaryFilter { return std::make_tuple(std::ref(pitch_roll_enable),std::ref(heading_enable),std::ref(pitch_roll_time_constant),std::ref(heading_time_constant)); } - }; }; void insert(Serializer& serializer, const ComplementaryFilter& self); @@ -3474,9 +3610,13 @@ struct SensorRange auto as_tuple() const { - return std::make_tuple(std::ref(sensor),std::ref(setting)); + return std::make_tuple(sensor,setting); } + auto as_tuple() + { + return std::make_tuple(std::ref(sensor),std::ref(setting)); + } static SensorRange create_sld_all(::mip::FunctionSelector function) { @@ -3501,7 +3641,6 @@ struct SensorRange { return std::make_tuple(std::ref(sensor),std::ref(setting)); } - }; }; void insert(Serializer& serializer, const SensorRange& self); @@ -3546,9 +3685,13 @@ struct CalibratedSensorRanges auto as_tuple() const { - return std::make_tuple(std::ref(sensor)); + return std::make_tuple(sensor); } + auto as_tuple() + { + return std::make_tuple(std::ref(sensor)); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; @@ -3565,7 +3708,6 @@ struct CalibratedSensorRanges { return std::make_tuple(std::ref(sensor),std::ref(num_ranges),std::ref(ranges)); } - }; }; void insert(Serializer& serializer, const CalibratedSensorRanges& self); @@ -3623,9 +3765,13 @@ struct LowpassFilter auto as_tuple() const { - return std::make_tuple(std::ref(desc_set),std::ref(field_desc),std::ref(enable),std::ref(manual),std::ref(frequency)); + return std::make_tuple(desc_set,field_desc,enable,manual,frequency); } + auto as_tuple() + { + return std::make_tuple(std::ref(desc_set),std::ref(field_desc),std::ref(enable),std::ref(manual),std::ref(frequency)); + } static LowpassFilter create_sld_all(::mip::FunctionSelector function) { @@ -3654,7 +3800,6 @@ struct LowpassFilter { return std::make_tuple(std::ref(desc_set),std::ref(field_desc),std::ref(enable),std::ref(manual),std::ref(frequency)); } - }; }; void insert(Serializer& serializer, const LowpassFilter& self); diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 5212421b6..f68d44d9f 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -106,9 +106,13 @@ struct SensorFrameMapping auto as_tuple() const { - return std::make_tuple(std::ref(sensor_id),std::ref(frame_id)); + return std::make_tuple(sensor_id,frame_id); } + auto as_tuple() + { + return std::make_tuple(std::ref(sensor_id),std::ref(frame_id)); + } static SensorFrameMapping create_sld_all(::mip::FunctionSelector function) { @@ -132,7 +136,6 @@ struct SensorFrameMapping { return std::make_tuple(std::ref(sensor_id),std::ref(frame_id)); } - }; }; void insert(Serializer& serializer, const SensorFrameMapping& self); @@ -182,9 +185,13 @@ struct ReferenceFrame auto as_tuple() const { - return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation)); + return std::make_tuple(frame_id,format,translation,rotation); } + auto as_tuple() + { + return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation)); + } static ReferenceFrame create_sld_all(::mip::FunctionSelector function) { @@ -211,7 +218,6 @@ struct ReferenceFrame { return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation)); } - }; }; void insert(Serializer& serializer, const ReferenceFrame& self); @@ -259,9 +265,13 @@ struct AidingEchoControl auto as_tuple() const { - return std::make_tuple(std::ref(mode)); + return std::make_tuple(mode); } + auto as_tuple() + { + return std::make_tuple(std::ref(mode)); + } static AidingEchoControl create_sld_all(::mip::FunctionSelector function) { @@ -284,7 +294,6 @@ struct AidingEchoControl { return std::make_tuple(std::ref(mode)); } - }; }; void insert(Serializer& serializer, const AidingEchoControl& self); @@ -354,9 +363,13 @@ struct EcefPos auto as_tuple() const { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(position),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(time,sensor_id,position,uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(position),std::ref(uncertainty),std::ref(valid_flags)); + } typedef void Response; }; void insert(Serializer& serializer, const EcefPos& self); @@ -422,9 +435,13 @@ struct LlhPos auto as_tuple() const { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(time,sensor_id,latitude,longitude,height,uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); + } typedef void Response; }; void insert(Serializer& serializer, const LlhPos& self); @@ -487,9 +504,13 @@ struct EcefVel auto as_tuple() const { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + } typedef void Response; }; void insert(Serializer& serializer, const EcefVel& self); @@ -552,9 +573,13 @@ struct NedVel auto as_tuple() const { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + } typedef void Response; }; void insert(Serializer& serializer, const NedVel& self); @@ -618,9 +643,13 @@ struct VehicleFixedFrameVelocity auto as_tuple() const { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + } typedef void Response; }; void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); @@ -651,9 +680,13 @@ struct TrueHeading auto as_tuple() const { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(heading),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(time,sensor_id,heading,uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(heading),std::ref(uncertainty),std::ref(valid_flags)); + } typedef void Response; }; void insert(Serializer& serializer, const TrueHeading& self); diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 274d06a96..e75c8fd70 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -210,6 +210,10 @@ struct Ping return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const Ping& self); @@ -243,6 +247,10 @@ struct SetIdle return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const SetIdle& self); @@ -273,6 +281,10 @@ struct GetDeviceInfo return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; @@ -287,7 +299,6 @@ struct GetDeviceInfo { return std::make_tuple(std::ref(device_info)); } - }; }; void insert(Serializer& serializer, const GetDeviceInfo& self); @@ -324,6 +335,10 @@ struct GetDeviceDescriptors return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; @@ -339,7 +354,6 @@ struct GetDeviceDescriptors { return std::make_tuple(std::ref(descriptors),std::ref(descriptors_count)); } - }; }; void insert(Serializer& serializer, const GetDeviceDescriptors& self); @@ -378,6 +392,10 @@ struct BuiltInTest return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; @@ -392,7 +410,6 @@ struct BuiltInTest { return std::make_tuple(std::ref(result)); } - }; }; void insert(Serializer& serializer, const BuiltInTest& self); @@ -427,6 +444,10 @@ struct Resume return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const Resume& self); @@ -460,6 +481,10 @@ struct GetExtendedDescriptors return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; @@ -475,7 +500,6 @@ struct GetExtendedDescriptors { return std::make_tuple(std::ref(descriptors),std::ref(descriptors_count)); } - }; }; void insert(Serializer& serializer, const GetExtendedDescriptors& self); @@ -511,6 +535,10 @@ struct ContinuousBit return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; @@ -525,7 +553,6 @@ struct ContinuousBit { return std::make_tuple(std::ref(result)); } - }; }; void insert(Serializer& serializer, const ContinuousBit& self); @@ -578,9 +605,13 @@ struct CommSpeed auto as_tuple() const { - return std::make_tuple(std::ref(port),std::ref(baud)); + return std::make_tuple(port,baud); } + auto as_tuple() + { + return std::make_tuple(std::ref(port),std::ref(baud)); + } static CommSpeed create_sld_all(::mip::FunctionSelector function) { @@ -605,7 +636,6 @@ struct CommSpeed { return std::make_tuple(std::ref(port),std::ref(baud)); } - }; }; void insert(Serializer& serializer, const CommSpeed& self); @@ -656,9 +686,13 @@ struct GpsTimeUpdate auto as_tuple() const { - return std::make_tuple(std::ref(field_id),std::ref(value)); + return std::make_tuple(field_id,value); } + auto as_tuple() + { + return std::make_tuple(std::ref(field_id),std::ref(value)); + } static GpsTimeUpdate create_sld_all(::mip::FunctionSelector function) { @@ -698,6 +732,10 @@ struct SoftReset return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const SoftReset& self); diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index d7d709387..28e88a9aa 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -203,6 +203,10 @@ struct Reset return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const Reset& self); @@ -243,9 +247,13 @@ struct SetInitialAttitude auto as_tuple() const { - return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(heading)); + return std::make_tuple(roll,pitch,heading); } + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(heading)); + } typedef void Response; }; void insert(Serializer& serializer, const SetInitialAttitude& self); @@ -333,9 +341,13 @@ struct EstimationControl auto as_tuple() const { - return std::make_tuple(std::ref(enable)); + return std::make_tuple(enable); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } static EstimationControl create_sld_all(::mip::FunctionSelector function) { @@ -358,7 +370,6 @@ struct EstimationControl { return std::make_tuple(std::ref(enable)); } - }; }; void insert(Serializer& serializer, const EstimationControl& self); @@ -404,9 +415,13 @@ struct ExternalGnssUpdate auto as_tuple() const { - return std::make_tuple(std::ref(gps_time),std::ref(gps_week),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(velocity),std::ref(pos_uncertainty),std::ref(vel_uncertainty)); + return std::make_tuple(gps_time,gps_week,latitude,longitude,height,velocity,pos_uncertainty,vel_uncertainty); } + auto as_tuple() + { + return std::make_tuple(std::ref(gps_time),std::ref(gps_week),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(velocity),std::ref(pos_uncertainty),std::ref(vel_uncertainty)); + } typedef void Response; }; void insert(Serializer& serializer, const ExternalGnssUpdate& self); @@ -448,9 +463,13 @@ struct ExternalHeadingUpdate auto as_tuple() const { - return std::make_tuple(std::ref(heading),std::ref(heading_uncertainty),std::ref(type)); + return std::make_tuple(heading,heading_uncertainty,type); } + auto as_tuple() + { + return std::make_tuple(std::ref(heading),std::ref(heading_uncertainty),std::ref(type)); + } typedef void Response; }; void insert(Serializer& serializer, const ExternalHeadingUpdate& self); @@ -498,9 +517,13 @@ struct ExternalHeadingUpdateWithTime auto as_tuple() const { - return std::make_tuple(std::ref(gps_time),std::ref(gps_week),std::ref(heading),std::ref(heading_uncertainty),std::ref(type)); + return std::make_tuple(gps_time,gps_week,heading,heading_uncertainty,type); } + auto as_tuple() + { + return std::make_tuple(std::ref(gps_time),std::ref(gps_week),std::ref(heading),std::ref(heading_uncertainty),std::ref(type)); + } typedef void Response; }; void insert(Serializer& serializer, const ExternalHeadingUpdateWithTime& self); @@ -570,9 +593,13 @@ struct TareOrientation auto as_tuple() const { - return std::make_tuple(std::ref(axes)); + return std::make_tuple(axes); } + auto as_tuple() + { + return std::make_tuple(std::ref(axes)); + } static TareOrientation create_sld_all(::mip::FunctionSelector function) { @@ -595,7 +622,6 @@ struct TareOrientation { return std::make_tuple(std::ref(axes)); } - }; }; void insert(Serializer& serializer, const TareOrientation& self); @@ -645,9 +671,13 @@ struct VehicleDynamicsMode auto as_tuple() const { - return std::make_tuple(std::ref(mode)); + return std::make_tuple(mode); } + auto as_tuple() + { + return std::make_tuple(std::ref(mode)); + } static VehicleDynamicsMode create_sld_all(::mip::FunctionSelector function) { @@ -670,7 +700,6 @@ struct VehicleDynamicsMode { return std::make_tuple(std::ref(mode)); } - }; }; void insert(Serializer& serializer, const VehicleDynamicsMode& self); @@ -736,9 +765,13 @@ struct SensorToVehicleRotationEuler auto as_tuple() const { - return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + return std::make_tuple(roll,pitch,yaw); } + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + } static SensorToVehicleRotationEuler create_sld_all(::mip::FunctionSelector function) { @@ -763,7 +796,6 @@ struct SensorToVehicleRotationEuler { return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); } - }; }; void insert(Serializer& serializer, const SensorToVehicleRotationEuler& self); @@ -833,9 +865,13 @@ struct SensorToVehicleRotationDcm auto as_tuple() const { - return std::make_tuple(std::ref(dcm)); + return std::make_tuple(dcm); } + auto as_tuple() + { + return std::make_tuple(std::ref(dcm)); + } static SensorToVehicleRotationDcm create_sld_all(::mip::FunctionSelector function) { @@ -858,7 +894,6 @@ struct SensorToVehicleRotationDcm { return std::make_tuple(std::ref(dcm)); } - }; }; void insert(Serializer& serializer, const SensorToVehicleRotationDcm& self); @@ -927,9 +962,13 @@ struct SensorToVehicleRotationQuaternion auto as_tuple() const { - return std::make_tuple(std::ref(quat)); + return std::make_tuple(quat); } + auto as_tuple() + { + return std::make_tuple(std::ref(quat)); + } static SensorToVehicleRotationQuaternion create_sld_all(::mip::FunctionSelector function) { @@ -952,7 +991,6 @@ struct SensorToVehicleRotationQuaternion { return std::make_tuple(std::ref(quat)); } - }; }; void insert(Serializer& serializer, const SensorToVehicleRotationQuaternion& self); @@ -1002,9 +1040,13 @@ struct SensorToVehicleOffset auto as_tuple() const { - return std::make_tuple(std::ref(offset)); + return std::make_tuple(offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } static SensorToVehicleOffset create_sld_all(::mip::FunctionSelector function) { @@ -1027,7 +1069,6 @@ struct SensorToVehicleOffset { return std::make_tuple(std::ref(offset)); } - }; }; void insert(Serializer& serializer, const SensorToVehicleOffset& self); @@ -1074,9 +1115,13 @@ struct AntennaOffset auto as_tuple() const { - return std::make_tuple(std::ref(offset)); + return std::make_tuple(offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(offset)); + } static AntennaOffset create_sld_all(::mip::FunctionSelector function) { @@ -1099,7 +1144,6 @@ struct AntennaOffset { return std::make_tuple(std::ref(offset)); } - }; }; void insert(Serializer& serializer, const AntennaOffset& self); @@ -1153,9 +1197,13 @@ struct GnssSource auto as_tuple() const { - return std::make_tuple(std::ref(source)); + return std::make_tuple(source); } + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } static GnssSource create_sld_all(::mip::FunctionSelector function) { @@ -1178,7 +1226,6 @@ struct GnssSource { return std::make_tuple(std::ref(source)); } - }; }; void insert(Serializer& serializer, const GnssSource& self); @@ -1243,9 +1290,13 @@ struct HeadingSource auto as_tuple() const { - return std::make_tuple(std::ref(source)); + return std::make_tuple(source); } + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } static HeadingSource create_sld_all(::mip::FunctionSelector function) { @@ -1268,7 +1319,6 @@ struct HeadingSource { return std::make_tuple(std::ref(source)); } - }; }; void insert(Serializer& serializer, const HeadingSource& self); @@ -1318,9 +1368,13 @@ struct AutoInitControl auto as_tuple() const { - return std::make_tuple(std::ref(enable)); + return std::make_tuple(enable); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } static AutoInitControl create_sld_all(::mip::FunctionSelector function) { @@ -1343,7 +1397,6 @@ struct AutoInitControl { return std::make_tuple(std::ref(enable)); } - }; }; void insert(Serializer& serializer, const AutoInitControl& self); @@ -1391,9 +1444,13 @@ struct AccelNoise auto as_tuple() const { - return std::make_tuple(std::ref(noise)); + return std::make_tuple(noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } static AccelNoise create_sld_all(::mip::FunctionSelector function) { @@ -1416,7 +1473,6 @@ struct AccelNoise { return std::make_tuple(std::ref(noise)); } - }; }; void insert(Serializer& serializer, const AccelNoise& self); @@ -1464,9 +1520,13 @@ struct GyroNoise auto as_tuple() const { - return std::make_tuple(std::ref(noise)); + return std::make_tuple(noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } static GyroNoise create_sld_all(::mip::FunctionSelector function) { @@ -1489,7 +1549,6 @@ struct GyroNoise { return std::make_tuple(std::ref(noise)); } - }; }; void insert(Serializer& serializer, const GyroNoise& self); @@ -1535,9 +1594,13 @@ struct AccelBiasModel auto as_tuple() const { - return std::make_tuple(std::ref(beta),std::ref(noise)); + return std::make_tuple(beta,noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(beta),std::ref(noise)); + } static AccelBiasModel create_sld_all(::mip::FunctionSelector function) { @@ -1561,7 +1624,6 @@ struct AccelBiasModel { return std::make_tuple(std::ref(beta),std::ref(noise)); } - }; }; void insert(Serializer& serializer, const AccelBiasModel& self); @@ -1607,9 +1669,13 @@ struct GyroBiasModel auto as_tuple() const { - return std::make_tuple(std::ref(beta),std::ref(noise)); + return std::make_tuple(beta,noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(beta),std::ref(noise)); + } static GyroBiasModel create_sld_all(::mip::FunctionSelector function) { @@ -1633,7 +1699,6 @@ struct GyroBiasModel { return std::make_tuple(std::ref(beta),std::ref(noise)); } - }; }; void insert(Serializer& serializer, const GyroBiasModel& self); @@ -1685,9 +1750,13 @@ struct AltitudeAiding auto as_tuple() const { - return std::make_tuple(std::ref(selector)); + return std::make_tuple(selector); } + auto as_tuple() + { + return std::make_tuple(std::ref(selector)); + } static AltitudeAiding create_sld_all(::mip::FunctionSelector function) { @@ -1710,7 +1779,6 @@ struct AltitudeAiding { return std::make_tuple(std::ref(selector)); } - }; }; void insert(Serializer& serializer, const AltitudeAiding& self); @@ -1759,9 +1827,13 @@ struct PitchRollAiding auto as_tuple() const { - return std::make_tuple(std::ref(source)); + return std::make_tuple(source); } + auto as_tuple() + { + return std::make_tuple(std::ref(source)); + } static PitchRollAiding create_sld_all(::mip::FunctionSelector function) { @@ -1784,7 +1856,6 @@ struct PitchRollAiding { return std::make_tuple(std::ref(source)); } - }; }; void insert(Serializer& serializer, const PitchRollAiding& self); @@ -1828,9 +1899,13 @@ struct AutoZupt auto as_tuple() const { - return std::make_tuple(std::ref(enable),std::ref(threshold)); + return std::make_tuple(enable,threshold); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(threshold)); + } static AutoZupt create_sld_all(::mip::FunctionSelector function) { @@ -1854,7 +1929,6 @@ struct AutoZupt { return std::make_tuple(std::ref(enable),std::ref(threshold)); } - }; }; void insert(Serializer& serializer, const AutoZupt& self); @@ -1899,9 +1973,13 @@ struct AutoAngularZupt auto as_tuple() const { - return std::make_tuple(std::ref(enable),std::ref(threshold)); + return std::make_tuple(enable,threshold); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(threshold)); + } static AutoAngularZupt create_sld_all(::mip::FunctionSelector function) { @@ -1925,7 +2003,6 @@ struct AutoAngularZupt { return std::make_tuple(std::ref(enable),std::ref(threshold)); } - }; }; void insert(Serializer& serializer, const AutoAngularZupt& self); @@ -1962,6 +2039,10 @@ struct CommandedZupt return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const CommandedZupt& self); @@ -1991,6 +2072,10 @@ struct CommandedAngularZupt return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const CommandedAngularZupt& self); @@ -2028,6 +2113,10 @@ struct MagCaptureAutoCal return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } static MagCaptureAutoCal create_sld_all(::mip::FunctionSelector function) { @@ -2076,9 +2165,13 @@ struct GravityNoise auto as_tuple() const { - return std::make_tuple(std::ref(noise)); + return std::make_tuple(noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } static GravityNoise create_sld_all(::mip::FunctionSelector function) { @@ -2101,7 +2194,6 @@ struct GravityNoise { return std::make_tuple(std::ref(noise)); } - }; }; void insert(Serializer& serializer, const GravityNoise& self); @@ -2148,9 +2240,13 @@ struct PressureAltitudeNoise auto as_tuple() const { - return std::make_tuple(std::ref(noise)); + return std::make_tuple(noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } static PressureAltitudeNoise create_sld_all(::mip::FunctionSelector function) { @@ -2173,7 +2269,6 @@ struct PressureAltitudeNoise { return std::make_tuple(std::ref(noise)); } - }; }; void insert(Serializer& serializer, const PressureAltitudeNoise& self); @@ -2222,9 +2317,13 @@ struct HardIronOffsetNoise auto as_tuple() const { - return std::make_tuple(std::ref(noise)); + return std::make_tuple(noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } static HardIronOffsetNoise create_sld_all(::mip::FunctionSelector function) { @@ -2247,7 +2346,6 @@ struct HardIronOffsetNoise { return std::make_tuple(std::ref(noise)); } - }; }; void insert(Serializer& serializer, const HardIronOffsetNoise& self); @@ -2295,9 +2393,13 @@ struct SoftIronMatrixNoise auto as_tuple() const { - return std::make_tuple(std::ref(noise)); + return std::make_tuple(noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } static SoftIronMatrixNoise create_sld_all(::mip::FunctionSelector function) { @@ -2320,7 +2422,6 @@ struct SoftIronMatrixNoise { return std::make_tuple(std::ref(noise)); } - }; }; void insert(Serializer& serializer, const SoftIronMatrixNoise& self); @@ -2368,9 +2469,13 @@ struct MagNoise auto as_tuple() const { - return std::make_tuple(std::ref(noise)); + return std::make_tuple(noise); } + auto as_tuple() + { + return std::make_tuple(std::ref(noise)); + } static MagNoise create_sld_all(::mip::FunctionSelector function) { @@ -2393,7 +2498,6 @@ struct MagNoise { return std::make_tuple(std::ref(noise)); } - }; }; void insert(Serializer& serializer, const MagNoise& self); @@ -2440,9 +2544,13 @@ struct InclinationSource auto as_tuple() const { - return std::make_tuple(std::ref(source),std::ref(inclination)); + return std::make_tuple(source,inclination); } + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(inclination)); + } static InclinationSource create_sld_all(::mip::FunctionSelector function) { @@ -2466,7 +2574,6 @@ struct InclinationSource { return std::make_tuple(std::ref(source),std::ref(inclination)); } - }; }; void insert(Serializer& serializer, const InclinationSource& self); @@ -2513,9 +2620,13 @@ struct MagneticDeclinationSource auto as_tuple() const { - return std::make_tuple(std::ref(source),std::ref(declination)); + return std::make_tuple(source,declination); } + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(declination)); + } static MagneticDeclinationSource create_sld_all(::mip::FunctionSelector function) { @@ -2539,7 +2650,6 @@ struct MagneticDeclinationSource { return std::make_tuple(std::ref(source),std::ref(declination)); } - }; }; void insert(Serializer& serializer, const MagneticDeclinationSource& self); @@ -2585,9 +2695,13 @@ struct MagFieldMagnitudeSource auto as_tuple() const { - return std::make_tuple(std::ref(source),std::ref(magnitude)); + return std::make_tuple(source,magnitude); } + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(magnitude)); + } static MagFieldMagnitudeSource create_sld_all(::mip::FunctionSelector function) { @@ -2611,7 +2725,6 @@ struct MagFieldMagnitudeSource { return std::make_tuple(std::ref(source),std::ref(magnitude)); } - }; }; void insert(Serializer& serializer, const MagFieldMagnitudeSource& self); @@ -2659,9 +2772,13 @@ struct ReferencePosition auto as_tuple() const { - return std::make_tuple(std::ref(enable),std::ref(latitude),std::ref(longitude),std::ref(altitude)); + return std::make_tuple(enable,latitude,longitude,altitude); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(latitude),std::ref(longitude),std::ref(altitude)); + } static ReferencePosition create_sld_all(::mip::FunctionSelector function) { @@ -2687,7 +2804,6 @@ struct ReferencePosition { return std::make_tuple(std::ref(enable),std::ref(latitude),std::ref(longitude),std::ref(altitude)); } - }; }; void insert(Serializer& serializer, const ReferencePosition& self); @@ -2748,9 +2864,13 @@ struct AccelMagnitudeErrorAdaptiveMeasurement auto as_tuple() const { - return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + return std::make_tuple(adaptive_measurement,frequency,low_limit,high_limit,low_limit_uncertainty,high_limit_uncertainty,minimum_uncertainty); } + auto as_tuple() + { + return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + } static AccelMagnitudeErrorAdaptiveMeasurement create_sld_all(::mip::FunctionSelector function) { @@ -2779,7 +2899,6 @@ struct AccelMagnitudeErrorAdaptiveMeasurement { return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); } - }; }; void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement& self); @@ -2835,9 +2954,13 @@ struct MagMagnitudeErrorAdaptiveMeasurement auto as_tuple() const { - return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + return std::make_tuple(adaptive_measurement,frequency,low_limit,high_limit,low_limit_uncertainty,high_limit_uncertainty,minimum_uncertainty); } + auto as_tuple() + { + return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + } static MagMagnitudeErrorAdaptiveMeasurement create_sld_all(::mip::FunctionSelector function) { @@ -2866,7 +2989,6 @@ struct MagMagnitudeErrorAdaptiveMeasurement { return std::make_tuple(std::ref(adaptive_measurement),std::ref(frequency),std::ref(low_limit),std::ref(high_limit),std::ref(low_limit_uncertainty),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); } - }; }; void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement& self); @@ -2922,9 +3044,13 @@ struct MagDipAngleErrorAdaptiveMeasurement auto as_tuple() const { - return std::make_tuple(std::ref(enable),std::ref(frequency),std::ref(high_limit),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + return std::make_tuple(enable,frequency,high_limit,high_limit_uncertainty,minimum_uncertainty); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(frequency),std::ref(high_limit),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); + } static MagDipAngleErrorAdaptiveMeasurement create_sld_all(::mip::FunctionSelector function) { @@ -2951,7 +3077,6 @@ struct MagDipAngleErrorAdaptiveMeasurement { return std::make_tuple(std::ref(enable),std::ref(frequency),std::ref(high_limit),std::ref(high_limit_uncertainty),std::ref(minimum_uncertainty)); } - }; }; void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement& self); @@ -3007,9 +3132,13 @@ struct AidingMeasurementEnable auto as_tuple() const { - return std::make_tuple(std::ref(aiding_source),std::ref(enable)); + return std::make_tuple(aiding_source,enable); } + auto as_tuple() + { + return std::make_tuple(std::ref(aiding_source),std::ref(enable)); + } static AidingMeasurementEnable create_sld_all(::mip::FunctionSelector function) { @@ -3034,7 +3163,6 @@ struct AidingMeasurementEnable { return std::make_tuple(std::ref(aiding_source),std::ref(enable)); } - }; }; void insert(Serializer& serializer, const AidingMeasurementEnable& self); @@ -3073,6 +3201,10 @@ struct Run return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const Run& self); @@ -3111,9 +3243,13 @@ struct KinematicConstraint auto as_tuple() const { - return std::make_tuple(std::ref(acceleration_constraint_selection),std::ref(velocity_constraint_selection),std::ref(angular_constraint_selection)); + return std::make_tuple(acceleration_constraint_selection,velocity_constraint_selection,angular_constraint_selection); } + auto as_tuple() + { + return std::make_tuple(std::ref(acceleration_constraint_selection),std::ref(velocity_constraint_selection),std::ref(angular_constraint_selection)); + } static KinematicConstraint create_sld_all(::mip::FunctionSelector function) { @@ -3138,7 +3274,6 @@ struct KinematicConstraint { return std::make_tuple(std::ref(acceleration_constraint_selection),std::ref(velocity_constraint_selection),std::ref(angular_constraint_selection)); } - }; }; void insert(Serializer& serializer, const KinematicConstraint& self); @@ -3234,9 +3369,13 @@ struct InitializationConfiguration auto as_tuple() const { - return std::make_tuple(std::ref(wait_for_run_command),std::ref(initial_cond_src),std::ref(auto_heading_alignment_selector),std::ref(initial_heading),std::ref(initial_pitch),std::ref(initial_roll),std::ref(initial_position),std::ref(initial_velocity),std::ref(reference_frame_selector)); + return std::make_tuple(wait_for_run_command,initial_cond_src,auto_heading_alignment_selector,initial_heading,initial_pitch,initial_roll,initial_position,initial_velocity,reference_frame_selector); } + auto as_tuple() + { + return std::make_tuple(std::ref(wait_for_run_command),std::ref(initial_cond_src),std::ref(auto_heading_alignment_selector),std::ref(initial_heading),std::ref(initial_pitch),std::ref(initial_roll),std::ref(initial_position),std::ref(initial_velocity),std::ref(reference_frame_selector)); + } static InitializationConfiguration create_sld_all(::mip::FunctionSelector function) { @@ -3267,7 +3406,6 @@ struct InitializationConfiguration { return std::make_tuple(std::ref(wait_for_run_command),std::ref(initial_cond_src),std::ref(auto_heading_alignment_selector),std::ref(initial_heading),std::ref(initial_pitch),std::ref(initial_roll),std::ref(initial_position),std::ref(initial_velocity),std::ref(reference_frame_selector)); } - }; }; void insert(Serializer& serializer, const InitializationConfiguration& self); @@ -3310,9 +3448,13 @@ struct AdaptiveFilterOptions auto as_tuple() const { - return std::make_tuple(std::ref(level),std::ref(time_limit)); + return std::make_tuple(level,time_limit); } + auto as_tuple() + { + return std::make_tuple(std::ref(level),std::ref(time_limit)); + } static AdaptiveFilterOptions create_sld_all(::mip::FunctionSelector function) { @@ -3336,7 +3478,6 @@ struct AdaptiveFilterOptions { return std::make_tuple(std::ref(level),std::ref(time_limit)); } - }; }; void insert(Serializer& serializer, const AdaptiveFilterOptions& self); @@ -3382,9 +3523,13 @@ struct MultiAntennaOffset auto as_tuple() const { - return std::make_tuple(std::ref(receiver_id),std::ref(antenna_offset)); + return std::make_tuple(receiver_id,antenna_offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_id),std::ref(antenna_offset)); + } static MultiAntennaOffset create_sld_all(::mip::FunctionSelector function) { @@ -3409,7 +3554,6 @@ struct MultiAntennaOffset { return std::make_tuple(std::ref(receiver_id),std::ref(antenna_offset)); } - }; }; void insert(Serializer& serializer, const MultiAntennaOffset& self); @@ -3453,9 +3597,13 @@ struct RelPosConfiguration auto as_tuple() const { - return std::make_tuple(std::ref(source),std::ref(reference_frame_selector),std::ref(reference_coordinates)); + return std::make_tuple(source,reference_frame_selector,reference_coordinates); } + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(reference_frame_selector),std::ref(reference_coordinates)); + } static RelPosConfiguration create_sld_all(::mip::FunctionSelector function) { @@ -3480,7 +3628,6 @@ struct RelPosConfiguration { return std::make_tuple(std::ref(source),std::ref(reference_frame_selector),std::ref(reference_coordinates)); } - }; }; void insert(Serializer& serializer, const RelPosConfiguration& self); @@ -3535,9 +3682,13 @@ struct RefPointLeverArm auto as_tuple() const { - return std::make_tuple(std::ref(ref_point_sel),std::ref(lever_arm_offset)); + return std::make_tuple(ref_point_sel,lever_arm_offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(ref_point_sel),std::ref(lever_arm_offset)); + } static RefPointLeverArm create_sld_all(::mip::FunctionSelector function) { @@ -3561,7 +3712,6 @@ struct RefPointLeverArm { return std::make_tuple(std::ref(ref_point_sel),std::ref(lever_arm_offset)); } - }; }; void insert(Serializer& serializer, const RefPointLeverArm& self); @@ -3601,9 +3751,13 @@ struct SpeedMeasurement auto as_tuple() const { - return std::make_tuple(std::ref(source),std::ref(time_of_week),std::ref(speed),std::ref(speed_uncertainty)); + return std::make_tuple(source,time_of_week,speed,speed_uncertainty); } + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(time_of_week),std::ref(speed),std::ref(speed_uncertainty)); + } typedef void Response; }; void insert(Serializer& serializer, const SpeedMeasurement& self); @@ -3645,9 +3799,13 @@ struct SpeedLeverArm auto as_tuple() const { - return std::make_tuple(std::ref(source),std::ref(lever_arm_offset)); + return std::make_tuple(source,lever_arm_offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(source),std::ref(lever_arm_offset)); + } static SpeedLeverArm create_sld_all(::mip::FunctionSelector function) { @@ -3672,7 +3830,6 @@ struct SpeedLeverArm { return std::make_tuple(std::ref(source),std::ref(lever_arm_offset)); } - }; }; void insert(Serializer& serializer, const SpeedLeverArm& self); @@ -3720,9 +3877,13 @@ struct WheeledVehicleConstraintControl auto as_tuple() const { - return std::make_tuple(std::ref(enable)); + return std::make_tuple(enable); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } static WheeledVehicleConstraintControl create_sld_all(::mip::FunctionSelector function) { @@ -3745,7 +3906,6 @@ struct WheeledVehicleConstraintControl { return std::make_tuple(std::ref(enable)); } - }; }; void insert(Serializer& serializer, const WheeledVehicleConstraintControl& self); @@ -3791,9 +3951,13 @@ struct VerticalGyroConstraintControl auto as_tuple() const { - return std::make_tuple(std::ref(enable)); + return std::make_tuple(enable); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable)); + } static VerticalGyroConstraintControl create_sld_all(::mip::FunctionSelector function) { @@ -3816,7 +3980,6 @@ struct VerticalGyroConstraintControl { return std::make_tuple(std::ref(enable)); } - }; }; void insert(Serializer& serializer, const VerticalGyroConstraintControl& self); @@ -3861,9 +4024,13 @@ struct GnssAntennaCalControl auto as_tuple() const { - return std::make_tuple(std::ref(enable),std::ref(max_offset)); + return std::make_tuple(enable,max_offset); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(max_offset)); + } static GnssAntennaCalControl create_sld_all(::mip::FunctionSelector function) { @@ -3887,7 +4054,6 @@ struct GnssAntennaCalControl { return std::make_tuple(std::ref(enable),std::ref(max_offset)); } - }; }; void insert(Serializer& serializer, const GnssAntennaCalControl& self); @@ -3925,9 +4091,13 @@ struct SetInitialHeading auto as_tuple() const { - return std::make_tuple(std::ref(heading)); + return std::make_tuple(heading); } + auto as_tuple() + { + return std::make_tuple(std::ref(heading)); + } typedef void Response; }; void insert(Serializer& serializer, const SetInitialHeading& self); diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index dc524896d..ee33ec593 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -87,6 +87,10 @@ struct ReceiverInfo return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; @@ -102,7 +106,6 @@ struct ReceiverInfo { return std::make_tuple(std::ref(num_receivers),std::ref(receiver_info)); } - }; }; void insert(Serializer& serializer, const ReceiverInfo& self); @@ -148,9 +151,13 @@ struct SignalConfiguration auto as_tuple() const { - return std::make_tuple(std::ref(gps_enable),std::ref(glonass_enable),std::ref(galileo_enable),std::ref(beidou_enable),std::ref(reserved)); + return std::make_tuple(gps_enable,glonass_enable,galileo_enable,beidou_enable,reserved); } + auto as_tuple() + { + return std::make_tuple(std::ref(gps_enable),std::ref(glonass_enable),std::ref(galileo_enable),std::ref(beidou_enable),std::ref(reserved)); + } static SignalConfiguration create_sld_all(::mip::FunctionSelector function) { @@ -177,7 +184,6 @@ struct SignalConfiguration { return std::make_tuple(std::ref(gps_enable),std::ref(glonass_enable),std::ref(galileo_enable),std::ref(beidou_enable),std::ref(reserved)); } - }; }; void insert(Serializer& serializer, const SignalConfiguration& self); @@ -221,9 +227,13 @@ struct RtkDongleConfiguration auto as_tuple() const { - return std::make_tuple(std::ref(enable),std::ref(reserved)); + return std::make_tuple(enable,reserved); } + auto as_tuple() + { + return std::make_tuple(std::ref(enable),std::ref(reserved)); + } static RtkDongleConfiguration create_sld_all(::mip::FunctionSelector function) { @@ -247,7 +257,6 @@ struct RtkDongleConfiguration { return std::make_tuple(std::ref(enable),std::ref(reserved)); } - }; }; void insert(Serializer& serializer, const RtkDongleConfiguration& self); diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 5d2c049b2..c5f0df4dd 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -211,6 +211,10 @@ struct GetStatusFlags return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -225,7 +229,6 @@ struct GetStatusFlags { return std::make_tuple(std::ref(flags)); } - }; }; void insert(Serializer& serializer, const GetStatusFlags& self); @@ -258,6 +261,10 @@ struct GetImei return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -272,7 +279,6 @@ struct GetImei { return std::make_tuple(std::ref(IMEI)); } - }; }; void insert(Serializer& serializer, const GetImei& self); @@ -305,6 +311,10 @@ struct GetImsi return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -319,7 +329,6 @@ struct GetImsi { return std::make_tuple(std::ref(IMSI)); } - }; }; void insert(Serializer& serializer, const GetImsi& self); @@ -352,6 +361,10 @@ struct GetIccid return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -366,7 +379,6 @@ struct GetIccid { return std::make_tuple(std::ref(ICCID)); } - }; }; void insert(Serializer& serializer, const GetIccid& self); @@ -409,9 +421,13 @@ struct ConnectedDeviceType auto as_tuple() const { - return std::make_tuple(std::ref(devType)); + return std::make_tuple(devType); } + auto as_tuple() + { + return std::make_tuple(std::ref(devType)); + } static ConnectedDeviceType create_sld_all(::mip::FunctionSelector function) { @@ -434,7 +450,6 @@ struct ConnectedDeviceType { return std::make_tuple(std::ref(devType)); } - }; }; void insert(Serializer& serializer, const ConnectedDeviceType& self); @@ -471,6 +486,10 @@ struct GetActCode return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -485,7 +504,6 @@ struct GetActCode { return std::make_tuple(std::ref(ActivationCode)); } - }; }; void insert(Serializer& serializer, const GetActCode& self); @@ -518,6 +536,10 @@ struct GetModemFirmwareVersion return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -532,7 +554,6 @@ struct GetModemFirmwareVersion { return std::make_tuple(std::ref(ModemFirmwareVersion)); } - }; }; void insert(Serializer& serializer, const GetModemFirmwareVersion& self); @@ -566,6 +587,10 @@ struct GetRssi return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -582,7 +607,6 @@ struct GetRssi { return std::make_tuple(std::ref(valid),std::ref(rssi),std::ref(signalQuality)); } - }; }; void insert(Serializer& serializer, const GetRssi& self); @@ -646,9 +670,13 @@ struct ServiceStatus auto as_tuple() const { - return std::make_tuple(std::ref(reserved1),std::ref(reserved2)); + return std::make_tuple(reserved1,reserved2); } + auto as_tuple() + { + return std::make_tuple(std::ref(reserved1),std::ref(reserved2)); + } struct Response { static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; @@ -666,7 +694,6 @@ struct ServiceStatus { return std::make_tuple(std::ref(flags),std::ref(receivedBytes),std::ref(lastBytes),std::ref(lastBytesTime)); } - }; }; void insert(Serializer& serializer, const ServiceStatus& self); @@ -698,9 +725,13 @@ struct ProdEraseStorage auto as_tuple() const { - return std::make_tuple(std::ref(media)); + return std::make_tuple(media); } + auto as_tuple() + { + return std::make_tuple(std::ref(media)); + } typedef void Response; }; void insert(Serializer& serializer, const ProdEraseStorage& self); @@ -731,9 +762,13 @@ struct LedControl auto as_tuple() const { - return std::make_tuple(std::ref(primaryColor),std::ref(altColor),std::ref(act),std::ref(period)); + return std::make_tuple(primaryColor,altColor,act,period); } + auto as_tuple() + { + return std::make_tuple(std::ref(primaryColor),std::ref(altColor),std::ref(act),std::ref(period)); + } typedef void Response; }; void insert(Serializer& serializer, const LedControl& self); @@ -764,6 +799,10 @@ struct ModemHardReset return std::make_tuple(); } + auto as_tuple() + { + return std::make_tuple(); + } typedef void Response; }; void insert(Serializer& serializer, const ModemHardReset& self); diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index 892a7c385..537e0d042 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -88,9 +88,13 @@ struct CommMode auto as_tuple() const { - return std::make_tuple(std::ref(mode)); + return std::make_tuple(mode); } + auto as_tuple() + { + return std::make_tuple(std::ref(mode)); + } static CommMode create_sld_all(::mip::FunctionSelector function) { @@ -113,7 +117,6 @@ struct CommMode { return std::make_tuple(std::ref(mode)); } - }; }; void insert(Serializer& serializer, const CommMode& self); diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 14f5a2815..d55259ec2 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -369,9 +369,13 @@ struct PositionLlh auto as_tuple() const { - return std::make_tuple(std::ref(latitude),std::ref(longitude),std::ref(ellipsoid_height),std::ref(valid_flags)); + return std::make_tuple(latitude,longitude,ellipsoid_height,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(latitude),std::ref(longitude),std::ref(ellipsoid_height),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const PositionLlh& self); void extract(Serializer& serializer, PositionLlh& self); @@ -398,9 +402,13 @@ struct VelocityNed auto as_tuple() const { - return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); + return std::make_tuple(north,east,down,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const VelocityNed& self); void extract(Serializer& serializer, VelocityNed& self); @@ -433,9 +441,13 @@ struct AttitudeQuaternion auto as_tuple() const { - return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3]),std::ref(valid_flags)); + return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AttitudeQuaternion& self); void extract(Serializer& serializer, AttitudeQuaternion& self); @@ -470,9 +482,13 @@ struct AttitudeDcm auto as_tuple() const { - return std::make_tuple(std::ref(dcm[0]),std::ref(dcm[1]),std::ref(dcm[2]),std::ref(dcm[3]),std::ref(dcm[4]),std::ref(dcm[5]),std::ref(dcm[6]),std::ref(dcm[7]),std::ref(dcm[8]),std::ref(valid_flags)); + return std::make_tuple(dcm[0],dcm[1],dcm[2],dcm[3],dcm[4],dcm[5],dcm[6],dcm[7],dcm[8],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(dcm[0]),std::ref(dcm[1]),std::ref(dcm[2]),std::ref(dcm[3]),std::ref(dcm[4]),std::ref(dcm[5]),std::ref(dcm[6]),std::ref(dcm[7]),std::ref(dcm[8]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AttitudeDcm& self); void extract(Serializer& serializer, AttitudeDcm& self); @@ -500,9 +516,13 @@ struct EulerAngles auto as_tuple() const { - return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw),std::ref(valid_flags)); + return std::make_tuple(roll,pitch,yaw,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const EulerAngles& self); void extract(Serializer& serializer, EulerAngles& self); @@ -527,9 +547,13 @@ struct GyroBias auto as_tuple() const { - return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GyroBias& self); void extract(Serializer& serializer, GyroBias& self); @@ -554,9 +578,13 @@ struct AccelBias auto as_tuple() const { - return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AccelBias& self); void extract(Serializer& serializer, AccelBias& self); @@ -583,9 +611,13 @@ struct PositionLlhUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); + return std::make_tuple(north,east,down,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const PositionLlhUncertainty& self); void extract(Serializer& serializer, PositionLlhUncertainty& self); @@ -612,9 +644,13 @@ struct VelocityNedUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); + return std::make_tuple(north,east,down,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(north),std::ref(east),std::ref(down),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const VelocityNedUncertainty& self); void extract(Serializer& serializer, VelocityNedUncertainty& self); @@ -642,9 +678,13 @@ struct EulerAnglesUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw),std::ref(valid_flags)); + return std::make_tuple(roll,pitch,yaw,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const EulerAnglesUncertainty& self); void extract(Serializer& serializer, EulerAnglesUncertainty& self); @@ -669,9 +709,13 @@ struct GyroBiasUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GyroBiasUncertainty& self); void extract(Serializer& serializer, GyroBiasUncertainty& self); @@ -696,9 +740,13 @@ struct AccelBiasUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AccelBiasUncertainty& self); void extract(Serializer& serializer, AccelBiasUncertainty& self); @@ -730,9 +778,13 @@ struct Timestamp auto as_tuple() const { - return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + return std::make_tuple(tow,week_number,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const Timestamp& self); void extract(Serializer& serializer, Timestamp& self); @@ -758,9 +810,13 @@ struct Status auto as_tuple() const { - return std::make_tuple(std::ref(filter_state),std::ref(dynamics_mode),std::ref(status_flags)); + return std::make_tuple(filter_state,dynamics_mode,status_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(filter_state),std::ref(dynamics_mode),std::ref(status_flags)); + } }; void insert(Serializer& serializer, const Status& self); void extract(Serializer& serializer, Status& self); @@ -786,9 +842,13 @@ struct LinearAccel auto as_tuple() const { - return std::make_tuple(std::ref(accel[0]),std::ref(accel[1]),std::ref(accel[2]),std::ref(valid_flags)); + return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(accel[0]),std::ref(accel[1]),std::ref(accel[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const LinearAccel& self); void extract(Serializer& serializer, LinearAccel& self); @@ -813,9 +873,13 @@ struct GravityVector auto as_tuple() const { - return std::make_tuple(std::ref(gravity[0]),std::ref(gravity[1]),std::ref(gravity[2]),std::ref(valid_flags)); + return std::make_tuple(gravity[0],gravity[1],gravity[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(gravity[0]),std::ref(gravity[1]),std::ref(gravity[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GravityVector& self); void extract(Serializer& serializer, GravityVector& self); @@ -840,9 +904,13 @@ struct CompAccel auto as_tuple() const { - return std::make_tuple(std::ref(accel[0]),std::ref(accel[1]),std::ref(accel[2]),std::ref(valid_flags)); + return std::make_tuple(accel[0],accel[1],accel[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(accel[0]),std::ref(accel[1]),std::ref(accel[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const CompAccel& self); void extract(Serializer& serializer, CompAccel& self); @@ -867,9 +935,13 @@ struct CompAngularRate auto as_tuple() const { - return std::make_tuple(std::ref(gyro[0]),std::ref(gyro[1]),std::ref(gyro[2]),std::ref(valid_flags)); + return std::make_tuple(gyro[0],gyro[1],gyro[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(gyro[0]),std::ref(gyro[1]),std::ref(gyro[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const CompAngularRate& self); void extract(Serializer& serializer, CompAngularRate& self); @@ -894,9 +966,13 @@ struct QuaternionAttitudeUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3]),std::ref(valid_flags)); + return std::make_tuple(q[0],q[1],q[2],q[3],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const QuaternionAttitudeUncertainty& self); void extract(Serializer& serializer, QuaternionAttitudeUncertainty& self); @@ -921,9 +997,13 @@ struct Wgs84GravityMag auto as_tuple() const { - return std::make_tuple(std::ref(magnitude),std::ref(valid_flags)); + return std::make_tuple(magnitude,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(magnitude),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const Wgs84GravityMag& self); void extract(Serializer& serializer, Wgs84GravityMag& self); @@ -962,9 +1042,13 @@ struct HeadingUpdateState auto as_tuple() const { - return std::make_tuple(std::ref(heading),std::ref(heading_1sigma),std::ref(source),std::ref(valid_flags)); + return std::make_tuple(heading,heading_1sigma,source,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(heading),std::ref(heading_1sigma),std::ref(source),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const HeadingUpdateState& self); void extract(Serializer& serializer, HeadingUpdateState& self); @@ -994,9 +1078,13 @@ struct MagneticModel auto as_tuple() const { - return std::make_tuple(std::ref(intensity_north),std::ref(intensity_east),std::ref(intensity_down),std::ref(inclination),std::ref(declination),std::ref(valid_flags)); + return std::make_tuple(intensity_north,intensity_east,intensity_down,inclination,declination,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(intensity_north),std::ref(intensity_east),std::ref(intensity_down),std::ref(inclination),std::ref(declination),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagneticModel& self); void extract(Serializer& serializer, MagneticModel& self); @@ -1021,9 +1109,13 @@ struct AccelScaleFactor auto as_tuple() const { - return std::make_tuple(std::ref(scale_factor[0]),std::ref(scale_factor[1]),std::ref(scale_factor[2]),std::ref(valid_flags)); + return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(scale_factor[0]),std::ref(scale_factor[1]),std::ref(scale_factor[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AccelScaleFactor& self); void extract(Serializer& serializer, AccelScaleFactor& self); @@ -1048,9 +1140,13 @@ struct AccelScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(scale_factor_uncert[0]),std::ref(scale_factor_uncert[1]),std::ref(scale_factor_uncert[2]),std::ref(valid_flags)); + return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(scale_factor_uncert[0]),std::ref(scale_factor_uncert[1]),std::ref(scale_factor_uncert[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AccelScaleFactorUncertainty& self); void extract(Serializer& serializer, AccelScaleFactorUncertainty& self); @@ -1075,9 +1171,13 @@ struct GyroScaleFactor auto as_tuple() const { - return std::make_tuple(std::ref(scale_factor[0]),std::ref(scale_factor[1]),std::ref(scale_factor[2]),std::ref(valid_flags)); + return std::make_tuple(scale_factor[0],scale_factor[1],scale_factor[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(scale_factor[0]),std::ref(scale_factor[1]),std::ref(scale_factor[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GyroScaleFactor& self); void extract(Serializer& serializer, GyroScaleFactor& self); @@ -1102,9 +1202,13 @@ struct GyroScaleFactorUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(scale_factor_uncert[0]),std::ref(scale_factor_uncert[1]),std::ref(scale_factor_uncert[2]),std::ref(valid_flags)); + return std::make_tuple(scale_factor_uncert[0],scale_factor_uncert[1],scale_factor_uncert[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(scale_factor_uncert[0]),std::ref(scale_factor_uncert[1]),std::ref(scale_factor_uncert[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GyroScaleFactorUncertainty& self); void extract(Serializer& serializer, GyroScaleFactorUncertainty& self); @@ -1129,9 +1233,13 @@ struct MagBias auto as_tuple() const { - return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); + return std::make_tuple(bias[0],bias[1],bias[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias[0]),std::ref(bias[1]),std::ref(bias[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagBias& self); void extract(Serializer& serializer, MagBias& self); @@ -1156,9 +1264,13 @@ struct MagBiasUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); + return std::make_tuple(bias_uncert[0],bias_uncert[1],bias_uncert[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias_uncert[0]),std::ref(bias_uncert[1]),std::ref(bias_uncert[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagBiasUncertainty& self); void extract(Serializer& serializer, MagBiasUncertainty& self); @@ -1189,9 +1301,13 @@ struct StandardAtmosphere auto as_tuple() const { - return std::make_tuple(std::ref(geometric_altitude),std::ref(geopotential_altitude),std::ref(standard_temperature),std::ref(standard_pressure),std::ref(standard_density),std::ref(valid_flags)); + return std::make_tuple(geometric_altitude,geopotential_altitude,standard_temperature,standard_pressure,standard_density,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(geometric_altitude),std::ref(geopotential_altitude),std::ref(standard_temperature),std::ref(standard_pressure),std::ref(standard_density),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const StandardAtmosphere& self); void extract(Serializer& serializer, StandardAtmosphere& self); @@ -1220,9 +1336,13 @@ struct PressureAltitude auto as_tuple() const { - return std::make_tuple(std::ref(pressure_altitude),std::ref(valid_flags)); + return std::make_tuple(pressure_altitude,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(pressure_altitude),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const PressureAltitude& self); void extract(Serializer& serializer, PressureAltitude& self); @@ -1246,9 +1366,13 @@ struct DensityAltitude auto as_tuple() const { - return std::make_tuple(std::ref(density_altitude),std::ref(valid_flags)); + return std::make_tuple(density_altitude,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(density_altitude),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const DensityAltitude& self); void extract(Serializer& serializer, DensityAltitude& self); @@ -1275,9 +1399,13 @@ struct AntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(std::ref(offset[0]),std::ref(offset[1]),std::ref(offset[2]),std::ref(valid_flags)); + return std::make_tuple(offset[0],offset[1],offset[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(offset[0]),std::ref(offset[1]),std::ref(offset[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AntennaOffsetCorrection& self); void extract(Serializer& serializer, AntennaOffsetCorrection& self); @@ -1302,9 +1430,13 @@ struct AntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(offset_uncert[0]),std::ref(offset_uncert[1]),std::ref(offset_uncert[2]),std::ref(valid_flags)); + return std::make_tuple(offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(offset_uncert[0]),std::ref(offset_uncert[1]),std::ref(offset_uncert[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const AntennaOffsetCorrectionUncertainty& self); void extract(Serializer& serializer, AntennaOffsetCorrectionUncertainty& self); @@ -1332,9 +1464,13 @@ struct MultiAntennaOffsetCorrection auto as_tuple() const { - return std::make_tuple(std::ref(receiver_id),std::ref(offset[0]),std::ref(offset[1]),std::ref(offset[2]),std::ref(valid_flags)); + return std::make_tuple(receiver_id,offset[0],offset[1],offset[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_id),std::ref(offset[0]),std::ref(offset[1]),std::ref(offset[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MultiAntennaOffsetCorrection& self); void extract(Serializer& serializer, MultiAntennaOffsetCorrection& self); @@ -1360,9 +1496,13 @@ struct MultiAntennaOffsetCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(receiver_id),std::ref(offset_uncert[0]),std::ref(offset_uncert[1]),std::ref(offset_uncert[2]),std::ref(valid_flags)); + return std::make_tuple(receiver_id,offset_uncert[0],offset_uncert[1],offset_uncert[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_id),std::ref(offset_uncert[0]),std::ref(offset_uncert[1]),std::ref(offset_uncert[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MultiAntennaOffsetCorrectionUncertainty& self); void extract(Serializer& serializer, MultiAntennaOffsetCorrectionUncertainty& self); @@ -1389,9 +1529,13 @@ struct MagnetometerOffset auto as_tuple() const { - return std::make_tuple(std::ref(hard_iron[0]),std::ref(hard_iron[1]),std::ref(hard_iron[2]),std::ref(valid_flags)); + return std::make_tuple(hard_iron[0],hard_iron[1],hard_iron[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(hard_iron[0]),std::ref(hard_iron[1]),std::ref(hard_iron[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagnetometerOffset& self); void extract(Serializer& serializer, MagnetometerOffset& self); @@ -1418,9 +1562,13 @@ struct MagnetometerMatrix auto as_tuple() const { - return std::make_tuple(std::ref(soft_iron[0]),std::ref(soft_iron[1]),std::ref(soft_iron[2]),std::ref(soft_iron[3]),std::ref(soft_iron[4]),std::ref(soft_iron[5]),std::ref(soft_iron[6]),std::ref(soft_iron[7]),std::ref(soft_iron[8]),std::ref(valid_flags)); + return std::make_tuple(soft_iron[0],soft_iron[1],soft_iron[2],soft_iron[3],soft_iron[4],soft_iron[5],soft_iron[6],soft_iron[7],soft_iron[8],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(soft_iron[0]),std::ref(soft_iron[1]),std::ref(soft_iron[2]),std::ref(soft_iron[3]),std::ref(soft_iron[4]),std::ref(soft_iron[5]),std::ref(soft_iron[6]),std::ref(soft_iron[7]),std::ref(soft_iron[8]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagnetometerMatrix& self); void extract(Serializer& serializer, MagnetometerMatrix& self); @@ -1445,9 +1593,13 @@ struct MagnetometerOffsetUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(hard_iron_uncertainty[0]),std::ref(hard_iron_uncertainty[1]),std::ref(hard_iron_uncertainty[2]),std::ref(valid_flags)); + return std::make_tuple(hard_iron_uncertainty[0],hard_iron_uncertainty[1],hard_iron_uncertainty[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(hard_iron_uncertainty[0]),std::ref(hard_iron_uncertainty[1]),std::ref(hard_iron_uncertainty[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagnetometerOffsetUncertainty& self); void extract(Serializer& serializer, MagnetometerOffsetUncertainty& self); @@ -1472,9 +1624,13 @@ struct MagnetometerMatrixUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(soft_iron_uncertainty[0]),std::ref(soft_iron_uncertainty[1]),std::ref(soft_iron_uncertainty[2]),std::ref(soft_iron_uncertainty[3]),std::ref(soft_iron_uncertainty[4]),std::ref(soft_iron_uncertainty[5]),std::ref(soft_iron_uncertainty[6]),std::ref(soft_iron_uncertainty[7]),std::ref(soft_iron_uncertainty[8]),std::ref(valid_flags)); + return std::make_tuple(soft_iron_uncertainty[0],soft_iron_uncertainty[1],soft_iron_uncertainty[2],soft_iron_uncertainty[3],soft_iron_uncertainty[4],soft_iron_uncertainty[5],soft_iron_uncertainty[6],soft_iron_uncertainty[7],soft_iron_uncertainty[8],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(soft_iron_uncertainty[0]),std::ref(soft_iron_uncertainty[1]),std::ref(soft_iron_uncertainty[2]),std::ref(soft_iron_uncertainty[3]),std::ref(soft_iron_uncertainty[4]),std::ref(soft_iron_uncertainty[5]),std::ref(soft_iron_uncertainty[6]),std::ref(soft_iron_uncertainty[7]),std::ref(soft_iron_uncertainty[8]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagnetometerMatrixUncertainty& self); void extract(Serializer& serializer, MagnetometerMatrixUncertainty& self); @@ -1498,9 +1654,13 @@ struct MagnetometerCovarianceMatrix auto as_tuple() const { - return std::make_tuple(std::ref(covariance[0]),std::ref(covariance[1]),std::ref(covariance[2]),std::ref(covariance[3]),std::ref(covariance[4]),std::ref(covariance[5]),std::ref(covariance[6]),std::ref(covariance[7]),std::ref(covariance[8]),std::ref(valid_flags)); + return std::make_tuple(covariance[0],covariance[1],covariance[2],covariance[3],covariance[4],covariance[5],covariance[6],covariance[7],covariance[8],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(covariance[0]),std::ref(covariance[1]),std::ref(covariance[2]),std::ref(covariance[3]),std::ref(covariance[4]),std::ref(covariance[5]),std::ref(covariance[6]),std::ref(covariance[7]),std::ref(covariance[8]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagnetometerCovarianceMatrix& self); void extract(Serializer& serializer, MagnetometerCovarianceMatrix& self); @@ -1525,9 +1685,13 @@ struct MagnetometerResidualVector auto as_tuple() const { - return std::make_tuple(std::ref(residual[0]),std::ref(residual[1]),std::ref(residual[2]),std::ref(valid_flags)); + return std::make_tuple(residual[0],residual[1],residual[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(residual[0]),std::ref(residual[1]),std::ref(residual[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const MagnetometerResidualVector& self); void extract(Serializer& serializer, MagnetometerResidualVector& self); @@ -1554,9 +1718,13 @@ struct ClockCorrection auto as_tuple() const { - return std::make_tuple(std::ref(receiver_id),std::ref(bias),std::ref(bias_drift),std::ref(valid_flags)); + return std::make_tuple(receiver_id,bias,bias_drift,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_id),std::ref(bias),std::ref(bias_drift),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const ClockCorrection& self); void extract(Serializer& serializer, ClockCorrection& self); @@ -1583,9 +1751,13 @@ struct ClockCorrectionUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(receiver_id),std::ref(bias_uncertainty),std::ref(bias_drift_uncertainty),std::ref(valid_flags)); + return std::make_tuple(receiver_id,bias_uncertainty,bias_drift_uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_id),std::ref(bias_uncertainty),std::ref(bias_drift_uncertainty),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const ClockCorrectionUncertainty& self); void extract(Serializer& serializer, ClockCorrectionUncertainty& self); @@ -1612,9 +1784,13 @@ struct GnssPosAidStatus auto as_tuple() const { - return std::make_tuple(std::ref(receiver_id),std::ref(time_of_week),std::ref(status),std::ref(reserved)); + return std::make_tuple(receiver_id,time_of_week,status,reserved); } + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_id),std::ref(time_of_week),std::ref(status),std::ref(reserved)); + } }; void insert(Serializer& serializer, const GnssPosAidStatus& self); void extract(Serializer& serializer, GnssPosAidStatus& self); @@ -1640,9 +1816,13 @@ struct GnssAttAidStatus auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(status),std::ref(reserved)); + return std::make_tuple(time_of_week,status,reserved); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(status),std::ref(reserved)); + } }; void insert(Serializer& serializer, const GnssAttAidStatus& self); void extract(Serializer& serializer, GnssAttAidStatus& self); @@ -1674,9 +1854,13 @@ struct HeadAidStatus auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(type),std::ref(reserved)); + return std::make_tuple(time_of_week,type,reserved); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(type),std::ref(reserved)); + } }; void insert(Serializer& serializer, const HeadAidStatus& self); void extract(Serializer& serializer, HeadAidStatus& self); @@ -1701,9 +1885,13 @@ struct RelPosNed auto as_tuple() const { - return std::make_tuple(std::ref(relative_position[0]),std::ref(relative_position[1]),std::ref(relative_position[2]),std::ref(valid_flags)); + return std::make_tuple(relative_position[0],relative_position[1],relative_position[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(relative_position[0]),std::ref(relative_position[1]),std::ref(relative_position[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const RelPosNed& self); void extract(Serializer& serializer, RelPosNed& self); @@ -1728,9 +1916,13 @@ struct EcefPos auto as_tuple() const { - return std::make_tuple(std::ref(position_ecef[0]),std::ref(position_ecef[1]),std::ref(position_ecef[2]),std::ref(valid_flags)); + return std::make_tuple(position_ecef[0],position_ecef[1],position_ecef[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(position_ecef[0]),std::ref(position_ecef[1]),std::ref(position_ecef[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); @@ -1755,9 +1947,13 @@ struct EcefVel auto as_tuple() const { - return std::make_tuple(std::ref(velocity_ecef[0]),std::ref(velocity_ecef[1]),std::ref(velocity_ecef[2]),std::ref(valid_flags)); + return std::make_tuple(velocity_ecef[0],velocity_ecef[1],velocity_ecef[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(velocity_ecef[0]),std::ref(velocity_ecef[1]),std::ref(velocity_ecef[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); @@ -1782,9 +1978,13 @@ struct EcefPosUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(pos_uncertainty[0]),std::ref(pos_uncertainty[1]),std::ref(pos_uncertainty[2]),std::ref(valid_flags)); + return std::make_tuple(pos_uncertainty[0],pos_uncertainty[1],pos_uncertainty[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(pos_uncertainty[0]),std::ref(pos_uncertainty[1]),std::ref(pos_uncertainty[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const EcefPosUncertainty& self); void extract(Serializer& serializer, EcefPosUncertainty& self); @@ -1809,9 +2009,13 @@ struct EcefVelUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(vel_uncertainty[0]),std::ref(vel_uncertainty[1]),std::ref(vel_uncertainty[2]),std::ref(valid_flags)); + return std::make_tuple(vel_uncertainty[0],vel_uncertainty[1],vel_uncertainty[2],valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(vel_uncertainty[0]),std::ref(vel_uncertainty[1]),std::ref(vel_uncertainty[2]),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const EcefVelUncertainty& self); void extract(Serializer& serializer, EcefVelUncertainty& self); @@ -1838,9 +2042,13 @@ struct AidingMeasurementSummary auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(source),std::ref(type),std::ref(indicator)); + return std::make_tuple(time_of_week,source,type,indicator); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(source),std::ref(type),std::ref(indicator)); + } }; void insert(Serializer& serializer, const AidingMeasurementSummary& self); void extract(Serializer& serializer, AidingMeasurementSummary& self); @@ -1865,9 +2073,13 @@ struct OdometerScaleFactorError auto as_tuple() const { - return std::make_tuple(std::ref(scale_factor_error),std::ref(valid_flags)); + return std::make_tuple(scale_factor_error,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(scale_factor_error),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const OdometerScaleFactorError& self); void extract(Serializer& serializer, OdometerScaleFactorError& self); @@ -1892,9 +2104,13 @@ struct OdometerScaleFactorErrorUncertainty auto as_tuple() const { - return std::make_tuple(std::ref(scale_factor_error_uncertainty),std::ref(valid_flags)); + return std::make_tuple(scale_factor_error_uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(scale_factor_error_uncertainty),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const OdometerScaleFactorErrorUncertainty& self); void extract(Serializer& serializer, OdometerScaleFactorErrorUncertainty& self); @@ -1961,9 +2177,13 @@ struct GnssDualAntennaStatus auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(heading),std::ref(heading_unc),std::ref(fix_type),std::ref(status_flags),std::ref(valid_flags)); + return std::make_tuple(time_of_week,heading,heading_unc,fix_type,status_flags,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(heading),std::ref(heading_unc),std::ref(fix_type),std::ref(status_flags),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GnssDualAntennaStatus& self); void extract(Serializer& serializer, GnssDualAntennaStatus& self); diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index e8425f0b5..9a52a8644 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -230,9 +230,13 @@ struct PosLlh auto as_tuple() const { - return std::make_tuple(std::ref(latitude),std::ref(longitude),std::ref(ellipsoid_height),std::ref(msl_height),std::ref(horizontal_accuracy),std::ref(vertical_accuracy),std::ref(valid_flags)); + return std::make_tuple(latitude,longitude,ellipsoid_height,msl_height,horizontal_accuracy,vertical_accuracy,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(latitude),std::ref(longitude),std::ref(ellipsoid_height),std::ref(msl_height),std::ref(horizontal_accuracy),std::ref(vertical_accuracy),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const PosLlh& self); void extract(Serializer& serializer, PosLlh& self); @@ -289,9 +293,13 @@ struct PosEcef auto as_tuple() const { - return std::make_tuple(std::ref(x[0]),std::ref(x[1]),std::ref(x[2]),std::ref(x_accuracy),std::ref(valid_flags)); + return std::make_tuple(x[0],x[1],x[2],x_accuracy,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(x[0]),std::ref(x[1]),std::ref(x[2]),std::ref(x_accuracy),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const PosEcef& self); void extract(Serializer& serializer, PosEcef& self); @@ -364,9 +372,13 @@ struct VelNed auto as_tuple() const { - return std::make_tuple(std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(speed),std::ref(ground_speed),std::ref(heading),std::ref(speed_accuracy),std::ref(heading_accuracy),std::ref(valid_flags)); + return std::make_tuple(v[0],v[1],v[2],speed,ground_speed,heading,speed_accuracy,heading_accuracy,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(speed),std::ref(ground_speed),std::ref(heading),std::ref(speed_accuracy),std::ref(heading_accuracy),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const VelNed& self); void extract(Serializer& serializer, VelNed& self); @@ -423,9 +435,13 @@ struct VelEcef auto as_tuple() const { - return std::make_tuple(std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(v_accuracy),std::ref(valid_flags)); + return std::make_tuple(v[0],v[1],v[2],v_accuracy,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(v_accuracy),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const VelEcef& self); void extract(Serializer& serializer, VelEcef& self); @@ -502,9 +518,13 @@ struct Dop auto as_tuple() const { - return std::make_tuple(std::ref(gdop),std::ref(pdop),std::ref(hdop),std::ref(vdop),std::ref(tdop),std::ref(ndop),std::ref(edop),std::ref(valid_flags)); + return std::make_tuple(gdop,pdop,hdop,vdop,tdop,ndop,edop,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(gdop),std::ref(pdop),std::ref(hdop),std::ref(vdop),std::ref(tdop),std::ref(ndop),std::ref(edop),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const Dop& self); void extract(Serializer& serializer, Dop& self); @@ -566,9 +586,13 @@ struct UtcTime auto as_tuple() const { - return std::make_tuple(std::ref(year),std::ref(month),std::ref(day),std::ref(hour),std::ref(min),std::ref(sec),std::ref(msec),std::ref(valid_flags)); + return std::make_tuple(year,month,day,hour,min,sec,msec,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(year),std::ref(month),std::ref(day),std::ref(hour),std::ref(min),std::ref(sec),std::ref(msec),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const UtcTime& self); void extract(Serializer& serializer, UtcTime& self); @@ -625,9 +649,13 @@ struct GpsTime auto as_tuple() const { - return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + return std::make_tuple(tow,week_number,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GpsTime& self); void extract(Serializer& serializer, GpsTime& self); @@ -688,9 +716,13 @@ struct ClockInfo auto as_tuple() const { - return std::make_tuple(std::ref(bias),std::ref(drift),std::ref(accuracy_estimate),std::ref(valid_flags)); + return std::make_tuple(bias,drift,accuracy_estimate,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias),std::ref(drift),std::ref(accuracy_estimate),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const ClockInfo& self); void extract(Serializer& serializer, ClockInfo& self); @@ -790,9 +822,13 @@ struct FixInfo auto as_tuple() const { - return std::make_tuple(std::ref(fix_type),std::ref(num_sv),std::ref(fix_flags),std::ref(valid_flags)); + return std::make_tuple(fix_type,num_sv,fix_flags,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(fix_type),std::ref(num_sv),std::ref(fix_flags),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const FixInfo& self); void extract(Serializer& serializer, FixInfo& self); @@ -895,9 +931,13 @@ struct SvInfo auto as_tuple() const { - return std::make_tuple(std::ref(channel),std::ref(sv_id),std::ref(carrier_noise_ratio),std::ref(azimuth),std::ref(elevation),std::ref(sv_flags),std::ref(valid_flags)); + return std::make_tuple(channel,sv_id,carrier_noise_ratio,azimuth,elevation,sv_flags,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(channel),std::ref(sv_id),std::ref(carrier_noise_ratio),std::ref(azimuth),std::ref(elevation),std::ref(sv_flags),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const SvInfo& self); void extract(Serializer& serializer, SvInfo& self); @@ -981,9 +1021,13 @@ struct HwStatus auto as_tuple() const { - return std::make_tuple(std::ref(receiver_state),std::ref(antenna_state),std::ref(antenna_power),std::ref(valid_flags)); + return std::make_tuple(receiver_state,antenna_state,antenna_power,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(receiver_state),std::ref(antenna_state),std::ref(antenna_power),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const HwStatus& self); void extract(Serializer& serializer, HwStatus& self); @@ -1060,9 +1104,13 @@ struct DgpsInfo auto as_tuple() const { - return std::make_tuple(std::ref(sv_id),std::ref(age),std::ref(range_correction),std::ref(range_rate_correction),std::ref(valid_flags)); + return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(sv_id),std::ref(age),std::ref(range_correction),std::ref(range_rate_correction),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const DgpsInfo& self); void extract(Serializer& serializer, DgpsInfo& self); @@ -1129,9 +1177,13 @@ struct DgpsChannel auto as_tuple() const { - return std::make_tuple(std::ref(sv_id),std::ref(age),std::ref(range_correction),std::ref(range_rate_correction),std::ref(valid_flags)); + return std::make_tuple(sv_id,age,range_correction,range_rate_correction,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(sv_id),std::ref(age),std::ref(range_correction),std::ref(range_rate_correction),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const DgpsChannel& self); void extract(Serializer& serializer, DgpsChannel& self); @@ -1198,9 +1250,13 @@ struct ClockInfo2 auto as_tuple() const { - return std::make_tuple(std::ref(bias),std::ref(drift),std::ref(bias_accuracy_estimate),std::ref(drift_accuracy_estimate),std::ref(valid_flags)); + return std::make_tuple(bias,drift,bias_accuracy_estimate,drift_accuracy_estimate,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(bias),std::ref(drift),std::ref(bias_accuracy_estimate),std::ref(drift_accuracy_estimate),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const ClockInfo2& self); void extract(Serializer& serializer, ClockInfo2& self); @@ -1250,9 +1306,13 @@ struct GpsLeapSeconds auto as_tuple() const { - return std::make_tuple(std::ref(leap_seconds),std::ref(valid_flags)); + return std::make_tuple(leap_seconds,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(leap_seconds),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GpsLeapSeconds& self); void extract(Serializer& serializer, GpsLeapSeconds& self); @@ -1359,9 +1419,13 @@ struct SbasInfo auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(sbas_system),std::ref(sbas_id),std::ref(count),std::ref(sbas_status),std::ref(valid_flags)); + return std::make_tuple(time_of_week,week_number,sbas_system,sbas_id,count,sbas_status,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(sbas_system),std::ref(sbas_id),std::ref(count),std::ref(sbas_status),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const SbasInfo& self); void extract(Serializer& serializer, SbasInfo& self); @@ -1450,9 +1514,13 @@ struct SbasCorrection auto as_tuple() const { - return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(gnss_id),std::ref(sv_id),std::ref(udrei),std::ref(pseudorange_correction),std::ref(iono_correction),std::ref(valid_flags)); + return std::make_tuple(index,count,time_of_week,week_number,gnss_id,sv_id,udrei,pseudorange_correction,iono_correction,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(gnss_id),std::ref(sv_id),std::ref(udrei),std::ref(pseudorange_correction),std::ref(iono_correction),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const SbasCorrection& self); void extract(Serializer& serializer, SbasCorrection& self); @@ -1538,9 +1606,13 @@ struct RfErrorDetection auto as_tuple() const { - return std::make_tuple(std::ref(rf_band),std::ref(jamming_state),std::ref(spoofing_state),std::ref(reserved),std::ref(valid_flags)); + return std::make_tuple(rf_band,jamming_state,spoofing_state,reserved,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(rf_band),std::ref(jamming_state),std::ref(spoofing_state),std::ref(reserved),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const RfErrorDetection& self); void extract(Serializer& serializer, RfErrorDetection& self); @@ -1664,9 +1736,13 @@ struct BaseStationInfo auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(ecef_pos[0]),std::ref(ecef_pos[1]),std::ref(ecef_pos[2]),std::ref(height),std::ref(station_id),std::ref(indicators),std::ref(valid_flags)); + return std::make_tuple(time_of_week,week_number,ecef_pos[0],ecef_pos[1],ecef_pos[2],height,station_id,indicators,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(ecef_pos[0]),std::ref(ecef_pos[1]),std::ref(ecef_pos[2]),std::ref(height),std::ref(station_id),std::ref(indicators),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const BaseStationInfo& self); void extract(Serializer& serializer, BaseStationInfo& self); @@ -1796,9 +1872,13 @@ struct RtkCorrectionsStatus auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(epoch_status),std::ref(dongle_status),std::ref(gps_correction_latency),std::ref(glonass_correction_latency),std::ref(galileo_correction_latency),std::ref(beidou_correction_latency),std::ref(reserved),std::ref(valid_flags)); + return std::make_tuple(time_of_week,week_number,epoch_status,dongle_status,gps_correction_latency,glonass_correction_latency,galileo_correction_latency,beidou_correction_latency,reserved,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(epoch_status),std::ref(dongle_status),std::ref(gps_correction_latency),std::ref(glonass_correction_latency),std::ref(galileo_correction_latency),std::ref(beidou_correction_latency),std::ref(reserved),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const RtkCorrectionsStatus& self); void extract(Serializer& serializer, RtkCorrectionsStatus& self); @@ -1877,9 +1957,13 @@ struct SatelliteStatus auto as_tuple() const { - return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(gnss_id),std::ref(satellite_id),std::ref(elevation),std::ref(azimuth),std::ref(health),std::ref(valid_flags)); + return std::make_tuple(index,count,time_of_week,week_number,gnss_id,satellite_id,elevation,azimuth,health,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(gnss_id),std::ref(satellite_id),std::ref(elevation),std::ref(azimuth),std::ref(health),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const SatelliteStatus& self); void extract(Serializer& serializer, SatelliteStatus& self); @@ -2004,9 +2088,13 @@ struct Raw auto as_tuple() const { - return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(receiver_id),std::ref(tracking_channel),std::ref(gnss_id),std::ref(satellite_id),std::ref(signal_id),std::ref(signal_strength),std::ref(quality),std::ref(pseudorange),std::ref(carrier_phase),std::ref(doppler),std::ref(range_uncert),std::ref(phase_uncert),std::ref(doppler_uncert),std::ref(lock_time),std::ref(valid_flags)); + return std::make_tuple(index,count,time_of_week,week_number,receiver_id,tracking_channel,gnss_id,satellite_id,signal_id,signal_strength,quality,pseudorange,carrier_phase,doppler,range_uncert,phase_uncert,doppler_uncert,lock_time,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(receiver_id),std::ref(tracking_channel),std::ref(gnss_id),std::ref(satellite_id),std::ref(signal_id),std::ref(signal_strength),std::ref(quality),std::ref(pseudorange),std::ref(carrier_phase),std::ref(doppler),std::ref(range_uncert),std::ref(phase_uncert),std::ref(doppler_uncert),std::ref(lock_time),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const Raw& self); void extract(Serializer& serializer, Raw& self); @@ -2094,9 +2182,13 @@ struct GpsEphemeris auto as_tuple() const { - return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(health),std::ref(iodc),std::ref(iode),std::ref(t_oc),std::ref(af0),std::ref(af1),std::ref(af2),std::ref(t_gd),std::ref(ISC_L1CA),std::ref(ISC_L2C),std::ref(t_oe),std::ref(a),std::ref(a_dot),std::ref(mean_anomaly),std::ref(delta_mean_motion),std::ref(delta_mean_motion_dot),std::ref(eccentricity),std::ref(argument_of_perigee),std::ref(omega),std::ref(omega_dot),std::ref(inclination),std::ref(inclination_dot),std::ref(c_ic),std::ref(c_is),std::ref(c_uc),std::ref(c_us),std::ref(c_rc),std::ref(c_rs),std::ref(valid_flags)); + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(health),std::ref(iodc),std::ref(iode),std::ref(t_oc),std::ref(af0),std::ref(af1),std::ref(af2),std::ref(t_gd),std::ref(ISC_L1CA),std::ref(ISC_L2C),std::ref(t_oe),std::ref(a),std::ref(a_dot),std::ref(mean_anomaly),std::ref(delta_mean_motion),std::ref(delta_mean_motion_dot),std::ref(eccentricity),std::ref(argument_of_perigee),std::ref(omega),std::ref(omega_dot),std::ref(inclination),std::ref(inclination_dot),std::ref(c_ic),std::ref(c_is),std::ref(c_uc),std::ref(c_us),std::ref(c_rc),std::ref(c_rs),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GpsEphemeris& self); void extract(Serializer& serializer, GpsEphemeris& self); @@ -2184,9 +2276,13 @@ struct GalileoEphemeris auto as_tuple() const { - return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(health),std::ref(iodc),std::ref(iode),std::ref(t_oc),std::ref(af0),std::ref(af1),std::ref(af2),std::ref(t_gd),std::ref(ISC_L1CA),std::ref(ISC_L2C),std::ref(t_oe),std::ref(a),std::ref(a_dot),std::ref(mean_anomaly),std::ref(delta_mean_motion),std::ref(delta_mean_motion_dot),std::ref(eccentricity),std::ref(argument_of_perigee),std::ref(omega),std::ref(omega_dot),std::ref(inclination),std::ref(inclination_dot),std::ref(c_ic),std::ref(c_is),std::ref(c_uc),std::ref(c_us),std::ref(c_rc),std::ref(c_rs),std::ref(valid_flags)); + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,health,iodc,iode,t_oc,af0,af1,af2,t_gd,ISC_L1CA,ISC_L2C,t_oe,a,a_dot,mean_anomaly,delta_mean_motion,delta_mean_motion_dot,eccentricity,argument_of_perigee,omega,omega_dot,inclination,inclination_dot,c_ic,c_is,c_uc,c_us,c_rc,c_rs,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(health),std::ref(iodc),std::ref(iode),std::ref(t_oc),std::ref(af0),std::ref(af1),std::ref(af2),std::ref(t_gd),std::ref(ISC_L1CA),std::ref(ISC_L2C),std::ref(t_oe),std::ref(a),std::ref(a_dot),std::ref(mean_anomaly),std::ref(delta_mean_motion),std::ref(delta_mean_motion_dot),std::ref(eccentricity),std::ref(argument_of_perigee),std::ref(omega),std::ref(omega_dot),std::ref(inclination),std::ref(inclination_dot),std::ref(c_ic),std::ref(c_is),std::ref(c_uc),std::ref(c_us),std::ref(c_rc),std::ref(c_rs),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GalileoEphemeris& self); void extract(Serializer& serializer, GalileoEphemeris& self); @@ -2262,9 +2358,13 @@ struct GloEphemeris auto as_tuple() const { - return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(freq_number),std::ref(tk),std::ref(tb),std::ref(sat_type),std::ref(gamma),std::ref(tau_n),std::ref(x[0]),std::ref(x[1]),std::ref(x[2]),std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(a[0]),std::ref(a[1]),std::ref(a[2]),std::ref(health),std::ref(P),std::ref(NT),std::ref(delta_tau_n),std::ref(Ft),std::ref(En),std::ref(P1),std::ref(P2),std::ref(P3),std::ref(P4),std::ref(valid_flags)); + return std::make_tuple(index,count,time_of_week,week_number,satellite_id,freq_number,tk,tb,sat_type,gamma,tau_n,x[0],x[1],x[2],v[0],v[1],v[2],a[0],a[1],a[2],health,P,NT,delta_tau_n,Ft,En,P1,P2,P3,P4,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(index),std::ref(count),std::ref(time_of_week),std::ref(week_number),std::ref(satellite_id),std::ref(freq_number),std::ref(tk),std::ref(tb),std::ref(sat_type),std::ref(gamma),std::ref(tau_n),std::ref(x[0]),std::ref(x[1]),std::ref(x[2]),std::ref(v[0]),std::ref(v[1]),std::ref(v[2]),std::ref(a[0]),std::ref(a[1]),std::ref(a[2]),std::ref(health),std::ref(P),std::ref(NT),std::ref(delta_tau_n),std::ref(Ft),std::ref(En),std::ref(P1),std::ref(P2),std::ref(P3),std::ref(P4),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GloEphemeris& self); void extract(Serializer& serializer, GloEphemeris& self); @@ -2329,9 +2429,13 @@ struct GpsIonoCorr auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(alpha),std::ref(beta),std::ref(valid_flags)); + return std::make_tuple(time_of_week,week_number,alpha,beta,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(alpha),std::ref(beta),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GpsIonoCorr& self); void extract(Serializer& serializer, GpsIonoCorr& self); @@ -2396,9 +2500,13 @@ struct GalileoIonoCorr auto as_tuple() const { - return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(alpha[0]),std::ref(alpha[1]),std::ref(alpha[2]),std::ref(disturbance_flags),std::ref(valid_flags)); + return std::make_tuple(time_of_week,week_number,alpha[0],alpha[1],alpha[2],disturbance_flags,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_of_week),std::ref(week_number),std::ref(alpha[0]),std::ref(alpha[1]),std::ref(alpha[2]),std::ref(disturbance_flags),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GalileoIonoCorr& self); void extract(Serializer& serializer, GalileoIonoCorr& self); diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 2b7591329..5e3142f72 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -87,9 +87,13 @@ struct RawAccel auto as_tuple() const { - return std::make_tuple(std::ref(raw_accel[0]),std::ref(raw_accel[1]),std::ref(raw_accel[2])); + return std::make_tuple(raw_accel[0],raw_accel[1],raw_accel[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(raw_accel[0]),std::ref(raw_accel[1]),std::ref(raw_accel[2])); + } }; void insert(Serializer& serializer, const RawAccel& self); void extract(Serializer& serializer, RawAccel& self); @@ -114,9 +118,13 @@ struct RawGyro auto as_tuple() const { - return std::make_tuple(std::ref(raw_gyro[0]),std::ref(raw_gyro[1]),std::ref(raw_gyro[2])); + return std::make_tuple(raw_gyro[0],raw_gyro[1],raw_gyro[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(raw_gyro[0]),std::ref(raw_gyro[1]),std::ref(raw_gyro[2])); + } }; void insert(Serializer& serializer, const RawGyro& self); void extract(Serializer& serializer, RawGyro& self); @@ -141,9 +149,13 @@ struct RawMag auto as_tuple() const { - return std::make_tuple(std::ref(raw_mag[0]),std::ref(raw_mag[1]),std::ref(raw_mag[2])); + return std::make_tuple(raw_mag[0],raw_mag[1],raw_mag[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(raw_mag[0]),std::ref(raw_mag[1]),std::ref(raw_mag[2])); + } }; void insert(Serializer& serializer, const RawMag& self); void extract(Serializer& serializer, RawMag& self); @@ -168,9 +180,13 @@ struct RawPressure auto as_tuple() const { - return std::make_tuple(std::ref(raw_pressure)); + return std::make_tuple(raw_pressure); } + auto as_tuple() + { + return std::make_tuple(std::ref(raw_pressure)); + } }; void insert(Serializer& serializer, const RawPressure& self); void extract(Serializer& serializer, RawPressure& self); @@ -195,9 +211,13 @@ struct ScaledAccel auto as_tuple() const { - return std::make_tuple(std::ref(scaled_accel[0]),std::ref(scaled_accel[1]),std::ref(scaled_accel[2])); + return std::make_tuple(scaled_accel[0],scaled_accel[1],scaled_accel[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(scaled_accel[0]),std::ref(scaled_accel[1]),std::ref(scaled_accel[2])); + } }; void insert(Serializer& serializer, const ScaledAccel& self); void extract(Serializer& serializer, ScaledAccel& self); @@ -222,9 +242,13 @@ struct ScaledGyro auto as_tuple() const { - return std::make_tuple(std::ref(scaled_gyro[0]),std::ref(scaled_gyro[1]),std::ref(scaled_gyro[2])); + return std::make_tuple(scaled_gyro[0],scaled_gyro[1],scaled_gyro[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(scaled_gyro[0]),std::ref(scaled_gyro[1]),std::ref(scaled_gyro[2])); + } }; void insert(Serializer& serializer, const ScaledGyro& self); void extract(Serializer& serializer, ScaledGyro& self); @@ -249,9 +273,13 @@ struct ScaledMag auto as_tuple() const { - return std::make_tuple(std::ref(scaled_mag[0]),std::ref(scaled_mag[1]),std::ref(scaled_mag[2])); + return std::make_tuple(scaled_mag[0],scaled_mag[1],scaled_mag[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(scaled_mag[0]),std::ref(scaled_mag[1]),std::ref(scaled_mag[2])); + } }; void insert(Serializer& serializer, const ScaledMag& self); void extract(Serializer& serializer, ScaledMag& self); @@ -275,9 +303,13 @@ struct ScaledPressure auto as_tuple() const { - return std::make_tuple(std::ref(scaled_pressure)); + return std::make_tuple(scaled_pressure); } + auto as_tuple() + { + return std::make_tuple(std::ref(scaled_pressure)); + } }; void insert(Serializer& serializer, const ScaledPressure& self); void extract(Serializer& serializer, ScaledPressure& self); @@ -302,9 +334,13 @@ struct DeltaTheta auto as_tuple() const { - return std::make_tuple(std::ref(delta_theta[0]),std::ref(delta_theta[1]),std::ref(delta_theta[2])); + return std::make_tuple(delta_theta[0],delta_theta[1],delta_theta[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(delta_theta[0]),std::ref(delta_theta[1]),std::ref(delta_theta[2])); + } }; void insert(Serializer& serializer, const DeltaTheta& self); void extract(Serializer& serializer, DeltaTheta& self); @@ -329,9 +365,13 @@ struct DeltaVelocity auto as_tuple() const { - return std::make_tuple(std::ref(delta_velocity[0]),std::ref(delta_velocity[1]),std::ref(delta_velocity[2])); + return std::make_tuple(delta_velocity[0],delta_velocity[1],delta_velocity[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(delta_velocity[0]),std::ref(delta_velocity[1]),std::ref(delta_velocity[2])); + } }; void insert(Serializer& serializer, const DeltaVelocity& self); void extract(Serializer& serializer, DeltaVelocity& self); @@ -365,9 +405,13 @@ struct CompOrientationMatrix auto as_tuple() const { - return std::make_tuple(std::ref(m[0]),std::ref(m[1]),std::ref(m[2]),std::ref(m[3]),std::ref(m[4]),std::ref(m[5]),std::ref(m[6]),std::ref(m[7]),std::ref(m[8])); + return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } + auto as_tuple() + { + return std::make_tuple(std::ref(m[0]),std::ref(m[1]),std::ref(m[2]),std::ref(m[3]),std::ref(m[4]),std::ref(m[5]),std::ref(m[6]),std::ref(m[7]),std::ref(m[8])); + } }; void insert(Serializer& serializer, const CompOrientationMatrix& self); void extract(Serializer& serializer, CompOrientationMatrix& self); @@ -399,9 +443,13 @@ struct CompQuaternion auto as_tuple() const { - return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3])); + return std::make_tuple(q[0],q[1],q[2],q[3]); } + auto as_tuple() + { + return std::make_tuple(std::ref(q[0]),std::ref(q[1]),std::ref(q[2]),std::ref(q[3])); + } }; void insert(Serializer& serializer, const CompQuaternion& self); void extract(Serializer& serializer, CompQuaternion& self); @@ -428,9 +476,13 @@ struct CompEulerAngles auto as_tuple() const { - return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + return std::make_tuple(roll,pitch,yaw); } + auto as_tuple() + { + return std::make_tuple(std::ref(roll),std::ref(pitch),std::ref(yaw)); + } }; void insert(Serializer& serializer, const CompEulerAngles& self); void extract(Serializer& serializer, CompEulerAngles& self); @@ -454,9 +506,13 @@ struct CompOrientationUpdateMatrix auto as_tuple() const { - return std::make_tuple(std::ref(m[0]),std::ref(m[1]),std::ref(m[2]),std::ref(m[3]),std::ref(m[4]),std::ref(m[5]),std::ref(m[6]),std::ref(m[7]),std::ref(m[8])); + return std::make_tuple(m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); } + auto as_tuple() + { + return std::make_tuple(std::ref(m[0]),std::ref(m[1]),std::ref(m[2]),std::ref(m[3]),std::ref(m[4]),std::ref(m[5]),std::ref(m[6]),std::ref(m[7]),std::ref(m[8])); + } }; void insert(Serializer& serializer, const CompOrientationUpdateMatrix& self); void extract(Serializer& serializer, CompOrientationUpdateMatrix& self); @@ -480,9 +536,13 @@ struct OrientationRawTemp auto as_tuple() const { - return std::make_tuple(std::ref(raw_temp)); + return std::make_tuple(raw_temp); } + auto as_tuple() + { + return std::make_tuple(std::ref(raw_temp)); + } }; void insert(Serializer& serializer, const OrientationRawTemp& self); void extract(Serializer& serializer, OrientationRawTemp& self); @@ -506,9 +566,13 @@ struct InternalTimestamp auto as_tuple() const { - return std::make_tuple(std::ref(counts)); + return std::make_tuple(counts); } + auto as_tuple() + { + return std::make_tuple(std::ref(counts)); + } }; void insert(Serializer& serializer, const InternalTimestamp& self); void extract(Serializer& serializer, InternalTimestamp& self); @@ -533,9 +597,13 @@ struct PpsTimestamp auto as_tuple() const { - return std::make_tuple(std::ref(seconds),std::ref(useconds)); + return std::make_tuple(seconds,useconds); } + auto as_tuple() + { + return std::make_tuple(std::ref(seconds),std::ref(useconds)); + } }; void insert(Serializer& serializer, const PpsTimestamp& self); void extract(Serializer& serializer, PpsTimestamp& self); @@ -604,9 +672,13 @@ struct GpsTimestamp auto as_tuple() const { - return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + return std::make_tuple(tow,week_number,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GpsTimestamp& self); void extract(Serializer& serializer, GpsTimestamp& self); @@ -636,9 +708,13 @@ struct TemperatureAbs auto as_tuple() const { - return std::make_tuple(std::ref(min_temp),std::ref(max_temp),std::ref(mean_temp)); + return std::make_tuple(min_temp,max_temp,mean_temp); } + auto as_tuple() + { + return std::make_tuple(std::ref(min_temp),std::ref(max_temp),std::ref(mean_temp)); + } }; void insert(Serializer& serializer, const TemperatureAbs& self); void extract(Serializer& serializer, TemperatureAbs& self); @@ -668,9 +744,13 @@ struct UpVector auto as_tuple() const { - return std::make_tuple(std::ref(up[0]),std::ref(up[1]),std::ref(up[2])); + return std::make_tuple(up[0],up[1],up[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(up[0]),std::ref(up[1]),std::ref(up[2])); + } }; void insert(Serializer& serializer, const UpVector& self); void extract(Serializer& serializer, UpVector& self); @@ -697,9 +777,13 @@ struct NorthVector auto as_tuple() const { - return std::make_tuple(std::ref(north[0]),std::ref(north[1]),std::ref(north[2])); + return std::make_tuple(north[0],north[1],north[2]); } + auto as_tuple() + { + return std::make_tuple(std::ref(north[0]),std::ref(north[1]),std::ref(north[2])); + } }; void insert(Serializer& serializer, const NorthVector& self); void extract(Serializer& serializer, NorthVector& self); @@ -774,9 +858,13 @@ struct OverrangeStatus auto as_tuple() const { - return std::make_tuple(std::ref(status)); + return std::make_tuple(status); } + auto as_tuple() + { + return std::make_tuple(std::ref(status)); + } }; void insert(Serializer& serializer, const OverrangeStatus& self); void extract(Serializer& serializer, OverrangeStatus& self); @@ -801,9 +889,13 @@ struct OdometerData auto as_tuple() const { - return std::make_tuple(std::ref(speed),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(speed,uncertainty,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(speed),std::ref(uncertainty),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const OdometerData& self); void extract(Serializer& serializer, OdometerData& self); diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 1927ae2cd..0f26f11fb 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -74,9 +74,13 @@ struct EventSource auto as_tuple() const { - return std::make_tuple(std::ref(trigger_id)); + return std::make_tuple(trigger_id); } + auto as_tuple() + { + return std::make_tuple(std::ref(trigger_id)); + } }; void insert(Serializer& serializer, const EventSource& self); void extract(Serializer& serializer, EventSource& self); @@ -103,9 +107,13 @@ struct Ticks auto as_tuple() const { - return std::make_tuple(std::ref(ticks)); + return std::make_tuple(ticks); } + auto as_tuple() + { + return std::make_tuple(std::ref(ticks)); + } }; void insert(Serializer& serializer, const Ticks& self); void extract(Serializer& serializer, Ticks& self); @@ -133,9 +141,13 @@ struct DeltaTicks auto as_tuple() const { - return std::make_tuple(std::ref(ticks)); + return std::make_tuple(ticks); } + auto as_tuple() + { + return std::make_tuple(std::ref(ticks)); + } }; void insert(Serializer& serializer, const DeltaTicks& self); void extract(Serializer& serializer, DeltaTicks& self); @@ -195,9 +207,13 @@ struct GpsTimestamp auto as_tuple() const { - return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + return std::make_tuple(tow,week_number,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(tow),std::ref(week_number),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const GpsTimestamp& self); void extract(Serializer& serializer, GpsTimestamp& self); @@ -230,9 +246,13 @@ struct DeltaTime auto as_tuple() const { - return std::make_tuple(std::ref(seconds)); + return std::make_tuple(seconds); } + auto as_tuple() + { + return std::make_tuple(std::ref(seconds)); + } }; void insert(Serializer& serializer, const DeltaTime& self); void extract(Serializer& serializer, DeltaTime& self); @@ -263,9 +283,13 @@ struct ReferenceTimestamp auto as_tuple() const { - return std::make_tuple(std::ref(nanoseconds)); + return std::make_tuple(nanoseconds); } + auto as_tuple() + { + return std::make_tuple(std::ref(nanoseconds)); + } }; void insert(Serializer& serializer, const ReferenceTimestamp& self); void extract(Serializer& serializer, ReferenceTimestamp& self); @@ -298,9 +322,13 @@ struct ReferenceTimeDelta auto as_tuple() const { - return std::make_tuple(std::ref(dt_nanos)); + return std::make_tuple(dt_nanos); } + auto as_tuple() + { + return std::make_tuple(std::ref(dt_nanos)); + } }; void insert(Serializer& serializer, const ReferenceTimeDelta& self); void extract(Serializer& serializer, ReferenceTimeDelta& self); @@ -358,9 +386,13 @@ struct ExternalTimestamp auto as_tuple() const { - return std::make_tuple(std::ref(nanoseconds),std::ref(valid_flags)); + return std::make_tuple(nanoseconds,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(nanoseconds),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const ExternalTimestamp& self); void extract(Serializer& serializer, ExternalTimestamp& self); @@ -422,9 +454,13 @@ struct ExternalTimeDelta auto as_tuple() const { - return std::make_tuple(std::ref(dt_nanos),std::ref(valid_flags)); + return std::make_tuple(dt_nanos,valid_flags); } + auto as_tuple() + { + return std::make_tuple(std::ref(dt_nanos),std::ref(valid_flags)); + } }; void insert(Serializer& serializer, const ExternalTimeDelta& self); void extract(Serializer& serializer, ExternalTimeDelta& self); diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 7568b3754..65982acab 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -82,9 +82,13 @@ struct BuiltInTest auto as_tuple() const { - return std::make_tuple(std::ref(result)); + return std::make_tuple(result); } + auto as_tuple() + { + return std::make_tuple(std::ref(result)); + } }; void insert(Serializer& serializer, const BuiltInTest& self); void extract(Serializer& serializer, BuiltInTest& self); @@ -109,9 +113,13 @@ struct TimeSyncStatus auto as_tuple() const { - return std::make_tuple(std::ref(time_sync),std::ref(last_pps_rcvd)); + return std::make_tuple(time_sync,last_pps_rcvd); } + auto as_tuple() + { + return std::make_tuple(std::ref(time_sync),std::ref(last_pps_rcvd)); + } }; void insert(Serializer& serializer, const TimeSyncStatus& self); void extract(Serializer& serializer, TimeSyncStatus& self); @@ -153,9 +161,13 @@ struct GpioState auto as_tuple() const { - return std::make_tuple(std::ref(states)); + return std::make_tuple(states); } + auto as_tuple() + { + return std::make_tuple(std::ref(states)); + } }; void insert(Serializer& serializer, const GpioState& self); void extract(Serializer& serializer, GpioState& self); @@ -181,9 +193,13 @@ struct GpioAnalogValue auto as_tuple() const { - return std::make_tuple(std::ref(gpio_id),std::ref(value)); + return std::make_tuple(gpio_id,value); } + auto as_tuple() + { + return std::make_tuple(std::ref(gpio_id),std::ref(value)); + } }; void insert(Serializer& serializer, const GpioAnalogValue& self); void extract(Serializer& serializer, GpioAnalogValue& self); From 6c35ae9e6e2034514b0351a8b9bca6d04a207010 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 11:07:11 -0400 Subject: [PATCH 102/252] Fix mip vector class to be more interchangeable with C arrays. --- src/mip/definitions/common.h | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index ec283653a..70bca3720 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -7,7 +7,6 @@ #ifdef __cplusplus -#include #include #include @@ -86,12 +85,12 @@ inline void extract(Serializer& serializer, DescriptorRate& self) { return C::ex /// integration with code using plain arrays. /// template -struct Vector : public std::array +struct Vector { Vector() {} template - Vector(U value) { this->fill(value); } + Vector(U value) { fill(value); } template Vector(const U (&ptr)[N]) { copyFrom(ptr, N); } @@ -102,17 +101,33 @@ struct Vector : public std::array template explicit Vector(const U* ptr, size_t n) { copyFrom(ptr, n); } + template + Vector(std::initializer_list values) : Vector(values, std::make_index_sequence()) {} + Vector(const Vector&) = default; Vector& operator=(const Vector&) = default; template Vector& operator=(const Vector& other) { copyFrom(other, N); } - operator const T*() const { return this->data(); } - operator T*() { return this->data(); } + typedef T Array[N]; + operator Array&() { return m_data; } + operator const Array&() const { return m_data; } + + Array& data() { return m_data; } + const Array& data() const { return m_data; } template - void copyFrom(const U* ptr, size_t n) { if(n>N) n=N; for(size_t i=0; i + void copyFrom(const U* ptr, size_t n) { if(n>N) n=N; for(size_t i=0; i + Vector(std::initializer_list values, std::index_sequence) : m_data{ *(values.begin()+Is)... } {} + + T m_data[N]; }; using Vector3f = Vector; From 72b7fd9c1a368a5a8fa0e01a151e11088b4886e0 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 11:34:52 -0400 Subject: [PATCH 103/252] Generate MIP definitions from branches/dev@55244. --- src/mip/definitions/commands_3dm.c | 65 ++++---- src/mip/definitions/commands_3dm.cpp | 39 +++-- src/mip/definitions/commands_3dm.h | 26 ++-- src/mip/definitions/commands_3dm.hpp | 26 ++-- src/mip/definitions/commands_aiding.c | 53 ++++--- src/mip/definitions/commands_aiding.cpp | 27 +++- src/mip/definitions/commands_aiding.h | 14 +- src/mip/definitions/commands_aiding.hpp | 14 +- src/mip/definitions/commands_filter.c | 199 ++++++++++++++---------- src/mip/definitions/commands_filter.cpp | 113 +++++++++----- src/mip/definitions/commands_filter.h | 70 ++++----- src/mip/definitions/commands_filter.hpp | 70 ++++----- 12 files changed, 427 insertions(+), 289 deletions(-) diff --git a/src/mip/definitions/commands_3dm.c b/src/mip/definitions/commands_3dm.c index 3947bb398..8e5708e96 100644 --- a/src/mip/definitions/commands_3dm.c +++ b/src/mip/definitions/commands_3dm.c @@ -3791,7 +3791,7 @@ void extract_mip_3dm_accel_bias_response(mip_serializer* serializer, mip_3dm_acc } -mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, mip_vector3f bias) +mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, const float* bias) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3799,14 +3799,15 @@ mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, mip_vector insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &bias[i]); + insert_float(&serializer, bias[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, mip_vector3f bias_out) +mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, float* bias_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3824,8 +3825,9 @@ mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, mip_vector3 mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(bias_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &bias_out[i]); + extract_float(&deserializer, &bias_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -3904,7 +3906,7 @@ void extract_mip_3dm_gyro_bias_response(mip_serializer* serializer, mip_3dm_gyro } -mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, mip_vector3f bias) +mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, const float* bias) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3912,14 +3914,15 @@ mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, mip_vector3 insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &bias[i]); + insert_float(&serializer, bias[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, mip_vector3f bias_out) +mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, float* bias_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3937,8 +3940,9 @@ mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, mip_vector3f mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(bias_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &bias_out[i]); + extract_float(&deserializer, &bias_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4005,7 +4009,7 @@ void extract_mip_3dm_capture_gyro_bias_response(mip_serializer* serializer, mip_ } -mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, mip_vector3f bias_out) +mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, float* bias_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4023,8 +4027,9 @@ mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(bias_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &bias_out[i]); + extract_float(&deserializer, &bias_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4067,7 +4072,7 @@ void extract_mip_3dm_mag_hard_iron_offset_response(mip_serializer* serializer, m } -mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset) +mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, const float* offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4075,14 +4080,15 @@ mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &offset[i]); + insert_float(&serializer, offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset_out) +mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, float* offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4100,8 +4106,9 @@ mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, m mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &offset_out[i]); + extract_float(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4180,7 +4187,7 @@ void extract_mip_3dm_mag_soft_iron_matrix_response(mip_serializer* serializer, m } -mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset) +mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, const float* offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4188,14 +4195,15 @@ mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(offset || (9 == 0)); for(unsigned int i=0; i < 9; i++) - insert_mip_matrix3f(&serializer, &offset[i]); + insert_float(&serializer, offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset_out) +mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, float* offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4213,8 +4221,9 @@ mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, m mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(offset_out || (9 == 0)); for(unsigned int i=0; i < 9; i++) - extract_mip_matrix3f(&deserializer, &offset_out[i]); + extract_float(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4535,7 +4544,7 @@ void extract_mip_3dm_sensor_2_vehicle_transform_quaternion_response(mip_serializ } -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q) +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, const float* q) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4543,14 +4552,15 @@ mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_in insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(q || (4 == 0)); for(unsigned int i=0; i < 4; i++) - insert_mip_quatf(&serializer, &q[i]); + insert_float(&serializer, q[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q_out) +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, float* q_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4568,8 +4578,9 @@ mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_int mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(q_out || (4 == 0)); for(unsigned int i=0; i < 4; i++) - extract_mip_quatf(&deserializer, &q_out[i]); + extract_float(&deserializer, &q_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4648,7 +4659,7 @@ void extract_mip_3dm_sensor_2_vehicle_transform_dcm_response(mip_serializer* ser } -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm) +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, const float* dcm) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4656,14 +4667,15 @@ mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) - insert_mip_matrix3f(&serializer, &dcm[i]); + insert_float(&serializer, dcm[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm_out) +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, float* dcm_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4681,8 +4693,9 @@ mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(dcm_out || (9 == 0)); for(unsigned int i=0; i < 9; i++) - extract_mip_matrix3f(&deserializer, &dcm_out[i]); + extract_float(&deserializer, &dcm_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index 2d9ced80d..563119dcc 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -3313,12 +3313,13 @@ void extract(Serializer& serializer, AccelBias::Response& self) } -CmdResult writeAccelBias(C::mip_interface& device, Vector3f bias) +CmdResult writeAccelBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, bias[i]); @@ -3326,7 +3327,7 @@ CmdResult writeAccelBias(C::mip_interface& device, Vector3f bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBias(C::mip_interface& device, Vector3f biasOut) +CmdResult readAccelBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3341,6 +3342,7 @@ CmdResult readAccelBias(C::mip_interface& device, Vector3f biasOut) { Serializer deserializer(buffer, responseLength); + assert(biasOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, biasOut[i]); @@ -3415,12 +3417,13 @@ void extract(Serializer& serializer, GyroBias::Response& self) } -CmdResult writeGyroBias(C::mip_interface& device, Vector3f bias) +CmdResult writeGyroBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(bias || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, bias[i]); @@ -3428,7 +3431,7 @@ CmdResult writeGyroBias(C::mip_interface& device, Vector3f bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBias(C::mip_interface& device, Vector3f biasOut) +CmdResult readGyroBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3443,6 +3446,7 @@ CmdResult readGyroBias(C::mip_interface& device, Vector3f biasOut) { Serializer deserializer(buffer, responseLength); + assert(biasOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, biasOut[i]); @@ -3505,7 +3509,7 @@ void extract(Serializer& serializer, CaptureGyroBias::Response& self) } -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, Vector3f biasOut) +CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3521,6 +3525,7 @@ CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, Ve { Serializer deserializer(buffer, responseLength); + assert(biasOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, biasOut[i]); @@ -3565,12 +3570,13 @@ void extract(Serializer& serializer, MagHardIronOffset::Response& self) } -CmdResult writeMagHardIronOffset(C::mip_interface& device, Vector3f offset) +CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, offset[i]); @@ -3578,7 +3584,7 @@ CmdResult writeMagHardIronOffset(C::mip_interface& device, Vector3f offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagHardIronOffset(C::mip_interface& device, Vector3f offsetOut) +CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3593,6 +3599,7 @@ CmdResult readMagHardIronOffset(C::mip_interface& device, Vector3f offsetOut) { Serializer deserializer(buffer, responseLength); + assert(offsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, offsetOut[i]); @@ -3667,12 +3674,13 @@ void extract(Serializer& serializer, MagSoftIronMatrix::Response& self) } -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, Matrix3f offset) +CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(offset || (9 == 0)); for(unsigned int i=0; i < 9; i++) insert(serializer, offset[i]); @@ -3680,7 +3688,7 @@ CmdResult writeMagSoftIronMatrix(C::mip_interface& device, Matrix3f offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagSoftIronMatrix(C::mip_interface& device, Matrix3f offsetOut) +CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3695,6 +3703,7 @@ CmdResult readMagSoftIronMatrix(C::mip_interface& device, Matrix3f offsetOut) { Serializer deserializer(buffer, responseLength); + assert(offsetOut || (9 == 0)); for(unsigned int i=0; i < 9; i++) extract(deserializer, offsetOut[i]); @@ -3989,12 +3998,13 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response } -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf q) +CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(q || (4 == 0)); for(unsigned int i=0; i < 4; i++) insert(serializer, q[i]); @@ -4002,7 +4012,7 @@ CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf qOut) +CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4017,6 +4027,7 @@ CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf { Serializer deserializer(buffer, responseLength); + assert(qOut || (4 == 0)); for(unsigned int i=0; i < 4; i++) extract(deserializer, qOut[i]); @@ -4091,12 +4102,13 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self) } -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcm) +CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) insert(serializer, dcm[i]); @@ -4104,7 +4116,7 @@ CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcm return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcmOut) +CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4119,6 +4131,7 @@ CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcmO { Serializer deserializer(buffer, responseLength); + assert(dcmOut || (9 == 0)); for(unsigned int i=0; i < 9; i++) extract(deserializer, dcmOut[i]); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index 03953d412..ea75a2a6d 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -1725,8 +1725,8 @@ typedef struct mip_3dm_accel_bias_response mip_3dm_accel_bias_response; void insert_mip_3dm_accel_bias_response(struct mip_serializer* serializer, const mip_3dm_accel_bias_response* self); void extract_mip_3dm_accel_bias_response(struct mip_serializer* serializer, mip_3dm_accel_bias_response* self); -mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, mip_vector3f bias); -mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, mip_vector3f bias_out); +mip_cmd_result mip_3dm_write_accel_bias(struct mip_interface* device, const float* bias); +mip_cmd_result mip_3dm_read_accel_bias(struct mip_interface* device, float* bias_out); mip_cmd_result mip_3dm_save_accel_bias(struct mip_interface* device); mip_cmd_result mip_3dm_load_accel_bias(struct mip_interface* device); mip_cmd_result mip_3dm_default_accel_bias(struct mip_interface* device); @@ -1760,8 +1760,8 @@ typedef struct mip_3dm_gyro_bias_response mip_3dm_gyro_bias_response; void insert_mip_3dm_gyro_bias_response(struct mip_serializer* serializer, const mip_3dm_gyro_bias_response* self); void extract_mip_3dm_gyro_bias_response(struct mip_serializer* serializer, mip_3dm_gyro_bias_response* self); -mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, mip_vector3f bias); -mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, mip_vector3f bias_out); +mip_cmd_result mip_3dm_write_gyro_bias(struct mip_interface* device, const float* bias); +mip_cmd_result mip_3dm_read_gyro_bias(struct mip_interface* device, float* bias_out); mip_cmd_result mip_3dm_save_gyro_bias(struct mip_interface* device); mip_cmd_result mip_3dm_load_gyro_bias(struct mip_interface* device); mip_cmd_result mip_3dm_default_gyro_bias(struct mip_interface* device); @@ -1797,7 +1797,7 @@ typedef struct mip_3dm_capture_gyro_bias_response mip_3dm_capture_gyro_bias_resp void insert_mip_3dm_capture_gyro_bias_response(struct mip_serializer* serializer, const mip_3dm_capture_gyro_bias_response* self); void extract_mip_3dm_capture_gyro_bias_response(struct mip_serializer* serializer, mip_3dm_capture_gyro_bias_response* self); -mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, mip_vector3f bias_out); +mip_cmd_result mip_3dm_capture_gyro_bias(struct mip_interface* device, uint16_t averaging_time_ms, float* bias_out); ///@} /// @@ -1832,8 +1832,8 @@ typedef struct mip_3dm_mag_hard_iron_offset_response mip_3dm_mag_hard_iron_offse void insert_mip_3dm_mag_hard_iron_offset_response(struct mip_serializer* serializer, const mip_3dm_mag_hard_iron_offset_response* self); void extract_mip_3dm_mag_hard_iron_offset_response(struct mip_serializer* serializer, mip_3dm_mag_hard_iron_offset_response* self); -mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset); -mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, mip_vector3f offset_out); +mip_cmd_result mip_3dm_write_mag_hard_iron_offset(struct mip_interface* device, const float* offset); +mip_cmd_result mip_3dm_read_mag_hard_iron_offset(struct mip_interface* device, float* offset_out); mip_cmd_result mip_3dm_save_mag_hard_iron_offset(struct mip_interface* device); mip_cmd_result mip_3dm_load_mag_hard_iron_offset(struct mip_interface* device); mip_cmd_result mip_3dm_default_mag_hard_iron_offset(struct mip_interface* device); @@ -1875,8 +1875,8 @@ typedef struct mip_3dm_mag_soft_iron_matrix_response mip_3dm_mag_soft_iron_matri void insert_mip_3dm_mag_soft_iron_matrix_response(struct mip_serializer* serializer, const mip_3dm_mag_soft_iron_matrix_response* self); void extract_mip_3dm_mag_soft_iron_matrix_response(struct mip_serializer* serializer, mip_3dm_mag_soft_iron_matrix_response* self); -mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset); -mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, mip_matrix3f offset_out); +mip_cmd_result mip_3dm_write_mag_soft_iron_matrix(struct mip_interface* device, const float* offset); +mip_cmd_result mip_3dm_read_mag_soft_iron_matrix(struct mip_interface* device, float* offset_out); mip_cmd_result mip_3dm_save_mag_soft_iron_matrix(struct mip_interface* device); mip_cmd_result mip_3dm_load_mag_soft_iron_matrix(struct mip_interface* device); mip_cmd_result mip_3dm_default_mag_soft_iron_matrix(struct mip_interface* device); @@ -2034,8 +2034,8 @@ typedef struct mip_3dm_sensor_2_vehicle_transform_quaternion_response mip_3dm_se void insert_mip_3dm_sensor_2_vehicle_transform_quaternion_response(struct mip_serializer* serializer, const mip_3dm_sensor_2_vehicle_transform_quaternion_response* self); void extract_mip_3dm_sensor_2_vehicle_transform_quaternion_response(struct mip_serializer* serializer, mip_3dm_sensor_2_vehicle_transform_quaternion_response* self); -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q); -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, mip_quatf q_out); +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, const float* q); +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_quaternion(struct mip_interface* device, float* q_out); mip_cmd_result mip_3dm_save_sensor_2_vehicle_transform_quaternion(struct mip_interface* device); mip_cmd_result mip_3dm_load_sensor_2_vehicle_transform_quaternion(struct mip_interface* device); mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_quaternion(struct mip_interface* device); @@ -2097,8 +2097,8 @@ typedef struct mip_3dm_sensor_2_vehicle_transform_dcm_response mip_3dm_sensor_2_ void insert_mip_3dm_sensor_2_vehicle_transform_dcm_response(struct mip_serializer* serializer, const mip_3dm_sensor_2_vehicle_transform_dcm_response* self); void extract_mip_3dm_sensor_2_vehicle_transform_dcm_response(struct mip_serializer* serializer, mip_3dm_sensor_2_vehicle_transform_dcm_response* self); -mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm); -mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, mip_matrix3f dcm_out); +mip_cmd_result mip_3dm_write_sensor_2_vehicle_transform_dcm(struct mip_interface* device, const float* dcm); +mip_cmd_result mip_3dm_read_sensor_2_vehicle_transform_dcm(struct mip_interface* device, float* dcm_out); mip_cmd_result mip_3dm_save_sensor_2_vehicle_transform_dcm(struct mip_interface* device); mip_cmd_result mip_3dm_load_sensor_2_vehicle_transform_dcm(struct mip_interface* device); mip_cmd_result mip_3dm_default_sensor_2_vehicle_transform_dcm(struct mip_interface* device); diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 93e639253..76bafd152 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -2834,8 +2834,8 @@ void extract(Serializer& serializer, AccelBias& self); void insert(Serializer& serializer, const AccelBias::Response& self); void extract(Serializer& serializer, AccelBias::Response& self); -CmdResult writeAccelBias(C::mip_interface& device, Vector3f bias); -CmdResult readAccelBias(C::mip_interface& device, Vector3f biasOut); +CmdResult writeAccelBias(C::mip_interface& device, const float* bias); +CmdResult readAccelBias(C::mip_interface& device, float* biasOut); CmdResult saveAccelBias(C::mip_interface& device); CmdResult loadAccelBias(C::mip_interface& device); CmdResult defaultAccelBias(C::mip_interface& device); @@ -2906,8 +2906,8 @@ void extract(Serializer& serializer, GyroBias& self); void insert(Serializer& serializer, const GyroBias::Response& self); void extract(Serializer& serializer, GyroBias::Response& self); -CmdResult writeGyroBias(C::mip_interface& device, Vector3f bias); -CmdResult readGyroBias(C::mip_interface& device, Vector3f biasOut); +CmdResult writeGyroBias(C::mip_interface& device, const float* bias); +CmdResult readGyroBias(C::mip_interface& device, float* biasOut); CmdResult saveGyroBias(C::mip_interface& device); CmdResult loadGyroBias(C::mip_interface& device); CmdResult defaultGyroBias(C::mip_interface& device); @@ -2967,7 +2967,7 @@ void extract(Serializer& serializer, CaptureGyroBias& self); void insert(Serializer& serializer, const CaptureGyroBias::Response& self); void extract(Serializer& serializer, CaptureGyroBias::Response& self); -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, Vector3f biasOut); +CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); ///@} /// @@ -3039,8 +3039,8 @@ void extract(Serializer& serializer, MagHardIronOffset& self); void insert(Serializer& serializer, const MagHardIronOffset::Response& self); void extract(Serializer& serializer, MagHardIronOffset::Response& self); -CmdResult writeMagHardIronOffset(C::mip_interface& device, Vector3f offset); -CmdResult readMagHardIronOffset(C::mip_interface& device, Vector3f offsetOut); +CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); +CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); CmdResult saveMagHardIronOffset(C::mip_interface& device); CmdResult loadMagHardIronOffset(C::mip_interface& device); CmdResult defaultMagHardIronOffset(C::mip_interface& device); @@ -3119,8 +3119,8 @@ void extract(Serializer& serializer, MagSoftIronMatrix& self); void insert(Serializer& serializer, const MagSoftIronMatrix::Response& self); void extract(Serializer& serializer, MagSoftIronMatrix::Response& self); -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, Matrix3f offset); -CmdResult readMagSoftIronMatrix(C::mip_interface& device, Matrix3f offsetOut); +CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); +CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); CmdResult saveMagSoftIronMatrix(C::mip_interface& device); CmdResult loadMagSoftIronMatrix(C::mip_interface& device); CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); @@ -3389,8 +3389,8 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion& self); void insert(Serializer& serializer, const Sensor2VehicleTransformQuaternion::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response& self); -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf q); -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, Quatf qOut); +CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); +CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); @@ -3489,8 +3489,8 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm& self); void insert(Serializer& serializer, const Sensor2VehicleTransformDcm::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self); -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcm); -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, Matrix3f dcmOut); +CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); +CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device); CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device); CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 8623aa0a0..b4333e05b 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -253,7 +253,7 @@ void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* se *self = tmp; } -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, mip_vector3f translation, mip_quatf rotation) +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -265,17 +265,19 @@ mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, ui insert_mip_aiding_reference_frame_command_format(&serializer, format); + assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &translation[i]); + insert_float(&serializer, translation[i]); + assert(rotation || (4 == 0)); for(unsigned int i=0; i < 4; i++) - insert_mip_quatf(&serializer, &rotation[i]); + insert_float(&serializer, rotation[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, mip_vector3f translation_out, mip_quatf rotation_out) +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -300,11 +302,13 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin assert(format_out); extract_mip_aiding_reference_frame_command_format(&deserializer, format_out); + assert(translation_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &translation_out[i]); + extract_float(&deserializer, &translation_out[i]); + assert(rotation_out || (4 == 0)); for(unsigned int i=0; i < 4; i++) - extract_mip_quatf(&deserializer, &rotation_out[i]); + extract_float(&deserializer, &rotation_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -514,7 +518,7 @@ void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* seri *self = tmp; } -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3d position, mip_vector3f uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -525,11 +529,13 @@ mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* insert_u8(&serializer, sensor_id); + assert(position || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3d(&serializer, &position[i]); + insert_double(&serializer, position[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &uncertainty[i]); + insert_float(&serializer, uncertainty[i]); insert_mip_aiding_ecef_pos_command_valid_flags(&serializer, valid_flags); @@ -585,7 +591,7 @@ void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, mip_vector3f uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -602,8 +608,9 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* insert_double(&serializer, height); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &uncertainty[i]); + insert_float(&serializer, uncertainty[i]); insert_mip_aiding_llh_pos_command_valid_flags(&serializer, valid_flags); @@ -653,7 +660,7 @@ void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* seri *self = tmp; } -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -664,11 +671,13 @@ mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* insert_u8(&serializer, sensor_id); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &velocity[i]); + insert_float(&serializer, velocity[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &uncertainty[i]); + insert_float(&serializer, uncertainty[i]); insert_mip_aiding_ecef_vel_command_valid_flags(&serializer, valid_flags); @@ -718,7 +727,7 @@ void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -729,11 +738,13 @@ mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* insert_u8(&serializer, sensor_id); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &velocity[i]); + insert_float(&serializer, velocity[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &uncertainty[i]); + insert_float(&serializer, uncertainty[i]); insert_mip_aiding_ned_vel_command_valid_flags(&serializer, valid_flags); @@ -783,7 +794,7 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct *self = tmp; } -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -794,11 +805,13 @@ mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* dev insert_u8(&serializer, sensor_id); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &velocity[i]); + insert_float(&serializer, velocity[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &uncertainty[i]); + insert_float(&serializer, uncertainty[i]); insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(&serializer, valid_flags); diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 2140d56ca..1c08df6e2 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -222,7 +222,7 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) } -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, Vector3f translation, Quatf rotation) +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -232,9 +232,11 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen insert(serializer, format); + assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, translation[i]); + assert(rotation || (4 == 0)); for(unsigned int i=0; i < 4; i++) insert(serializer, rotation[i]); @@ -242,7 +244,7 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, Vector3f translationOut, Quatf rotationOut) +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -264,9 +266,11 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc assert(formatOut); extract(deserializer, *formatOut); + assert(translationOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, translationOut[i]); + assert(rotationOut || (4 == 0)); for(unsigned int i=0; i < 4; i++) extract(deserializer, rotationOut[i]); @@ -439,7 +443,7 @@ void extract(Serializer& serializer, EcefPos& self) } -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3d position, Vector3f uncertainty, EcefPos::ValidFlags validFlags) +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -448,9 +452,11 @@ CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, insert(serializer, sensorId); + assert(position || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, position[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -497,7 +503,7 @@ void extract(Serializer& serializer, LlhPos& self) } -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, Vector3f uncertainty, LlhPos::ValidFlags validFlags) +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -512,6 +518,7 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d insert(serializer, height); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -552,7 +559,7 @@ void extract(Serializer& serializer, EcefVel& self) } -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, EcefVel::ValidFlags validFlags) +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -561,9 +568,11 @@ CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, insert(serializer, sensorId); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -604,7 +613,7 @@ void extract(Serializer& serializer, NedVel& self) } -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, NedVel::ValidFlags validFlags) +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -613,9 +622,11 @@ CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, V insert(serializer, sensorId); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); @@ -656,7 +667,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -665,9 +676,11 @@ CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, insert(serializer, sensorId); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); + assert(uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, uncertainty[i]); diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index 6e08c1b8b..b8ac265d9 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -155,8 +155,8 @@ typedef struct mip_aiding_reference_frame_response mip_aiding_reference_frame_re void insert_mip_aiding_reference_frame_response(struct mip_serializer* serializer, const mip_aiding_reference_frame_response* self); void extract_mip_aiding_reference_frame_response(struct mip_serializer* serializer, mip_aiding_reference_frame_response* self); -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, mip_vector3f translation, mip_quatf rotation); -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, mip_vector3f translation_out, mip_quatf rotation_out); +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation); +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out); mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id); @@ -232,7 +232,7 @@ void extract_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self); void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3d position, mip_vector3f uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); ///@} /// @@ -268,7 +268,7 @@ void extract_mip_aiding_llh_pos_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self); void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self); -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, mip_vector3f uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); ///@} /// @@ -301,7 +301,7 @@ void extract_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self); void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); ///@} /// @@ -334,7 +334,7 @@ void extract_mip_aiding_ned_vel_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self); void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self); -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); ///@} /// @@ -368,7 +368,7 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializ void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self); void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self); -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, mip_vector3f velocity, mip_vector3f uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index f68d44d9f..4355c820d 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -226,8 +226,8 @@ void extract(Serializer& serializer, ReferenceFrame& self); void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, Vector3f translation, Quatf rotation); -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, Vector3f translationOut, Quatf rotationOut); +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation); +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut); CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); @@ -375,7 +375,7 @@ struct EcefPos void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3d position, Vector3f uncertainty, EcefPos::ValidFlags validFlags); +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); ///@} /// @@ -447,7 +447,7 @@ struct LlhPos void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, Vector3f uncertainty, LlhPos::ValidFlags validFlags); +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); ///@} /// @@ -516,7 +516,7 @@ struct EcefVel void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, EcefVel::ValidFlags validFlags); +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); ///@} /// @@ -585,7 +585,7 @@ struct NedVel void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, NedVel::ValidFlags validFlags); +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); ///@} /// @@ -655,7 +655,7 @@ struct VehicleFixedFrameVelocity void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, Vector3f velocity, Vector3f uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); ///@} /// diff --git a/src/mip/definitions/commands_filter.c b/src/mip/definitions/commands_filter.c index df9fb53d1..7cc6aed8f 100644 --- a/src/mip/definitions/commands_filter.c +++ b/src/mip/definitions/commands_filter.c @@ -263,7 +263,7 @@ void extract_mip_filter_external_gnss_update_command(mip_serializer* serializer, } -mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, mip_vector3f velocity, mip_vector3f pos_uncertainty, mip_vector3f vel_uncertainty) +mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, const float* velocity, const float* pos_uncertainty, const float* vel_uncertainty) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -279,14 +279,17 @@ mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, dou insert_double(&serializer, height); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &velocity[i]); + insert_float(&serializer, velocity[i]); + assert(pos_uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &pos_uncertainty[i]); + insert_float(&serializer, pos_uncertainty[i]); + assert(vel_uncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &vel_uncertainty[i]); + insert_float(&serializer, vel_uncertainty[i]); assert(mip_serializer_is_ok(&serializer)); @@ -782,7 +785,7 @@ void extract_mip_filter_sensor_to_vehicle_rotation_dcm_response(mip_serializer* } -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm) +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, const float* dcm) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -790,14 +793,15 @@ mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interf insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) - insert_mip_matrix3f(&serializer, &dcm[i]); + insert_float(&serializer, dcm[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm_out) +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, float* dcm_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -815,8 +819,9 @@ mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interfa mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(dcm_out || (9 == 0)); for(unsigned int i=0; i < 9; i++) - extract_mip_matrix3f(&deserializer, &dcm_out[i]); + extract_float(&deserializer, &dcm_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -895,7 +900,7 @@ void extract_mip_filter_sensor_to_vehicle_rotation_quaternion_response(mip_seria } -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat) +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, const float* quat) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -903,14 +908,15 @@ mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(quat || (4 == 0)); for(unsigned int i=0; i < 4; i++) - insert_mip_quatf(&serializer, &quat[i]); + insert_float(&serializer, quat[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat_out) +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, float* quat_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -928,8 +934,9 @@ mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_ mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(quat_out || (4 == 0)); for(unsigned int i=0; i < 4; i++) - extract_mip_quatf(&deserializer, &quat_out[i]); + extract_float(&deserializer, &quat_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1008,7 +1015,7 @@ void extract_mip_filter_sensor_to_vehicle_offset_response(mip_serializer* serial } -mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset) +mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, const float* offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1016,14 +1023,15 @@ mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* d insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &offset[i]); + insert_float(&serializer, offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset_out) +mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, float* offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1041,8 +1049,9 @@ mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* de mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &offset_out[i]); + extract_float(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1121,7 +1130,7 @@ void extract_mip_filter_antenna_offset_response(mip_serializer* serializer, mip_ } -mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, mip_vector3f offset) +mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, const float* offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1129,14 +1138,15 @@ mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, mip insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &offset[i]); + insert_float(&serializer, offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, mip_vector3f offset_out) +mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, float* offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1154,8 +1164,9 @@ mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, mip_ mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &offset_out[i]); + extract_float(&deserializer, &offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1580,7 +1591,7 @@ void extract_mip_filter_accel_noise_response(mip_serializer* serializer, mip_fil } -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, mip_vector3f noise) +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1588,14 +1599,15 @@ mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, mip_ve insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, mip_vector3f noise_out) +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1613,8 +1625,9 @@ mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, mip_vec mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1693,7 +1706,7 @@ void extract_mip_filter_gyro_noise_response(mip_serializer* serializer, mip_filt } -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, mip_vector3f noise) +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1701,14 +1714,15 @@ mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, mip_vec insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, mip_vector3f noise_out) +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1726,8 +1740,9 @@ mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, mip_vect mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1818,7 +1833,7 @@ void extract_mip_filter_accel_bias_model_response(mip_serializer* serializer, mi } -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise) +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1826,17 +1841,19 @@ mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, m insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &beta[i]); + insert_float(&serializer, beta[i]); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out) +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1854,11 +1871,13 @@ mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, mi mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(beta_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &beta_out[i]); + extract_float(&deserializer, &beta_out[i]); + assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -1949,7 +1968,7 @@ void extract_mip_filter_gyro_bias_model_response(mip_serializer* serializer, mip } -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise) +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1957,17 +1976,19 @@ mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, mi insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &beta[i]); + insert_float(&serializer, beta[i]); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out) +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1985,11 +2006,13 @@ mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, mip mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(beta_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &beta_out[i]); + extract_float(&deserializer, &beta_out[i]); + assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2591,7 +2614,7 @@ void extract_mip_filter_gravity_noise_response(mip_serializer* serializer, mip_f } -mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, mip_vector3f noise) +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2599,14 +2622,15 @@ mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, mip_ insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, mip_vector3f noise_out) +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2624,8 +2648,9 @@ mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, mip_v mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2812,7 +2837,7 @@ void extract_mip_filter_hard_iron_offset_noise_response(mip_serializer* serializ } -mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise) +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2820,14 +2845,15 @@ mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* dev insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise_out) +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2845,8 +2871,9 @@ mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* devi mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -2925,7 +2952,7 @@ void extract_mip_filter_soft_iron_matrix_noise_response(mip_serializer* serializ } -mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise) +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2933,14 +2960,15 @@ mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* dev insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(noise || (9 == 0)); for(unsigned int i=0; i < 9; i++) - insert_mip_matrix3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise_out) +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -2958,8 +2986,9 @@ mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* devi mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(noise_out || (9 == 0)); for(unsigned int i=0; i < 9; i++) - extract_mip_matrix3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -3038,7 +3067,7 @@ void extract_mip_filter_mag_noise_response(mip_serializer* serializer, mip_filte } -mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, mip_vector3f noise) +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, const float* noise) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3046,14 +3075,15 @@ mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, mip_vect insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &noise[i]); + insert_float(&serializer, noise[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, mip_vector3f noise_out) +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, float* noise_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -3071,8 +3101,9 @@ mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, mip_vecto mip_serializer deserializer; mip_serializer_init_insertion(&deserializer, buffer, responseLength); + assert(noise_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &noise_out[i]); + extract_float(&deserializer, &noise_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4560,7 +4591,7 @@ void extract_mip_filter_initialization_configuration_command_initial_condition_s *self = tmp; } -mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, mip_vector3f initial_position, mip_vector3f initial_velocity, mip_filter_reference_frame reference_frame_selector) +mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, const float* initial_position, const float* initial_velocity, mip_filter_reference_frame reference_frame_selector) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4580,11 +4611,13 @@ mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interfac insert_float(&serializer, initial_roll); + assert(initial_position || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &initial_position[i]); + insert_float(&serializer, initial_position[i]); + assert(initial_velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &initial_velocity[i]); + insert_float(&serializer, initial_velocity[i]); insert_mip_filter_reference_frame(&serializer, reference_frame_selector); @@ -4592,7 +4625,7 @@ mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interfac return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, mip_vector3f initial_position_out, mip_vector3f initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out) +mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, float* initial_position_out, float* initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4628,11 +4661,13 @@ mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface assert(initial_roll_out); extract_float(&deserializer, initial_roll_out); + assert(initial_position_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &initial_position_out[i]); + extract_float(&deserializer, &initial_position_out[i]); + assert(initial_velocity_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &initial_velocity_out[i]); + extract_float(&deserializer, &initial_velocity_out[i]); assert(reference_frame_selector_out); extract_mip_filter_reference_frame(&deserializer, reference_frame_selector_out); @@ -4843,7 +4878,7 @@ void extract_mip_filter_multi_antenna_offset_response(mip_serializer* serializer } -mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset) +mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, const float* antenna_offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4853,14 +4888,15 @@ mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* devic insert_u8(&serializer, receiver_id); + assert(antenna_offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &antenna_offset[i]); + insert_float(&serializer, antenna_offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset_out) +mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, float* antenna_offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4882,8 +4918,9 @@ mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device extract_u8(&deserializer, &receiver_id); + assert(antenna_offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &antenna_offset_out[i]); + extract_float(&deserializer, &antenna_offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -4984,7 +5021,7 @@ void extract_mip_filter_rel_pos_configuration_response(mip_serializer* serialize } -mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, mip_vector3d reference_coordinates) +mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, const double* reference_coordinates) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -4996,14 +5033,15 @@ mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* devi insert_mip_filter_reference_frame(&serializer, reference_frame_selector); + assert(reference_coordinates || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3d(&serializer, &reference_coordinates[i]); + insert_double(&serializer, reference_coordinates[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, mip_vector3d reference_coordinates_out) +mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, double* reference_coordinates_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -5027,8 +5065,9 @@ mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* devic assert(reference_frame_selector_out); extract_mip_filter_reference_frame(&deserializer, reference_frame_selector_out); + assert(reference_coordinates_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3d(&deserializer, &reference_coordinates_out[i]); + extract_double(&deserializer, &reference_coordinates_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -5126,7 +5165,7 @@ void extract_mip_filter_ref_point_lever_arm_command_reference_point_selector(str *self = tmp; } -mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, mip_vector3f lever_arm_offset) +mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, const float* lever_arm_offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -5136,14 +5175,15 @@ mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device insert_mip_filter_ref_point_lever_arm_command_reference_point_selector(&serializer, ref_point_sel); + assert(lever_arm_offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &lever_arm_offset[i]); + insert_float(&serializer, lever_arm_offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, mip_vector3f lever_arm_offset_out) +mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, float* lever_arm_offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -5164,8 +5204,9 @@ mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, assert(ref_point_sel_out); extract_mip_filter_ref_point_lever_arm_command_reference_point_selector(&deserializer, ref_point_sel_out); + assert(lever_arm_offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &lever_arm_offset_out[i]); + extract_float(&deserializer, &lever_arm_offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; @@ -5293,7 +5334,7 @@ void extract_mip_filter_speed_lever_arm_response(mip_serializer* serializer, mip } -mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset) +mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, const float* lever_arm_offset) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -5303,14 +5344,15 @@ mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, ui insert_u8(&serializer, source); + assert(lever_arm_offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) - insert_mip_vector3f(&serializer, &lever_arm_offset[i]); + insert_float(&serializer, lever_arm_offset[i]); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset_out) +mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, float* lever_arm_offset_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -5332,8 +5374,9 @@ mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uin extract_u8(&deserializer, &source); + assert(lever_arm_offset_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) - extract_mip_vector3f(&deserializer, &lever_arm_offset_out[i]); + extract_float(&deserializer, &lever_arm_offset_out[i]); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 16ea819b2..c88cb0eb8 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -220,7 +220,7 @@ void extract(Serializer& serializer, ExternalGnssUpdate& self) } -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, Vector3f velocity, Vector3f posUncertainty, Vector3f velUncertainty) +CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -235,12 +235,15 @@ CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t insert(serializer, height); + assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velocity[i]); + assert(posUncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, posUncertainty[i]); + assert(velUncertainty || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, velUncertainty[i]); @@ -681,12 +684,13 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self) } -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcm) +CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(dcm || (9 == 0)); for(unsigned int i=0; i < 9; i++) insert(serializer, dcm[i]); @@ -694,7 +698,7 @@ CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcm return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcmOut) +CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -709,6 +713,7 @@ CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcmO { Serializer deserializer(buffer, responseLength); + assert(dcmOut || (9 == 0)); for(unsigned int i=0; i < 9; i++) extract(deserializer, dcmOut[i]); @@ -783,12 +788,13 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response } -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quat) +CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(quat || (4 == 0)); for(unsigned int i=0; i < 4; i++) insert(serializer, quat[i]); @@ -796,7 +802,7 @@ CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quatOut) +CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -811,6 +817,7 @@ CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf { Serializer deserializer(buffer, responseLength); + assert(quatOut || (4 == 0)); for(unsigned int i=0; i < 4; i++) extract(deserializer, quatOut[i]); @@ -885,12 +892,13 @@ void extract(Serializer& serializer, SensorToVehicleOffset::Response& self) } -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, Vector3f offset) +CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, offset[i]); @@ -898,7 +906,7 @@ CmdResult writeSensorToVehicleOffset(C::mip_interface& device, Vector3f offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleOffset(C::mip_interface& device, Vector3f offsetOut) +CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -913,6 +921,7 @@ CmdResult readSensorToVehicleOffset(C::mip_interface& device, Vector3f offsetOut { Serializer deserializer(buffer, responseLength); + assert(offsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, offsetOut[i]); @@ -987,12 +996,13 @@ void extract(Serializer& serializer, AntennaOffset::Response& self) } -CmdResult writeAntennaOffset(C::mip_interface& device, Vector3f offset) +CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(offset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, offset[i]); @@ -1000,7 +1010,7 @@ CmdResult writeAntennaOffset(C::mip_interface& device, Vector3f offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAntennaOffset(C::mip_interface& device, Vector3f offsetOut) +CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1015,6 +1025,7 @@ CmdResult readAntennaOffset(C::mip_interface& device, Vector3f offsetOut) { Serializer deserializer(buffer, responseLength); + assert(offsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, offsetOut[i]); @@ -1380,12 +1391,13 @@ void extract(Serializer& serializer, AccelNoise::Response& self) } -CmdResult writeAccelNoise(C::mip_interface& device, Vector3f noise) +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1393,7 +1405,7 @@ CmdResult writeAccelNoise(C::mip_interface& device, Vector3f noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelNoise(C::mip_interface& device, Vector3f noiseOut) +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1408,6 +1420,7 @@ CmdResult readAccelNoise(C::mip_interface& device, Vector3f noiseOut) { Serializer deserializer(buffer, responseLength); + assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -1482,12 +1495,13 @@ void extract(Serializer& serializer, GyroNoise::Response& self) } -CmdResult writeGyroNoise(C::mip_interface& device, Vector3f noise) +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1495,7 +1509,7 @@ CmdResult writeGyroNoise(C::mip_interface& device, Vector3f noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroNoise(C::mip_interface& device, Vector3f noiseOut) +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1510,6 +1524,7 @@ CmdResult readGyroNoise(C::mip_interface& device, Vector3f noiseOut) { Serializer deserializer(buffer, responseLength); + assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -1596,15 +1611,17 @@ void extract(Serializer& serializer, AccelBiasModel::Response& self) } -CmdResult writeAccelBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise) +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, beta[i]); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1612,7 +1629,7 @@ CmdResult writeAccelBiasModel(C::mip_interface& device, Vector3f beta, Vector3f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut) +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1627,9 +1644,11 @@ CmdResult readAccelBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3 { Serializer deserializer(buffer, responseLength); + assert(betaOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, betaOut[i]); + assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -1716,15 +1735,17 @@ void extract(Serializer& serializer, GyroBiasModel::Response& self) } -CmdResult writeGyroBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise) +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(beta || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, beta[i]); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -1732,7 +1753,7 @@ CmdResult writeGyroBiasModel(C::mip_interface& device, Vector3f beta, Vector3f n return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut) +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1747,9 +1768,11 @@ CmdResult readGyroBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f { Serializer deserializer(buffer, responseLength); + assert(betaOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, betaOut[i]); + assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -2299,12 +2322,13 @@ void extract(Serializer& serializer, GravityNoise::Response& self) } -CmdResult writeGravityNoise(C::mip_interface& device, Vector3f noise) +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -2312,7 +2336,7 @@ CmdResult writeGravityNoise(C::mip_interface& device, Vector3f noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGravityNoise(C::mip_interface& device, Vector3f noiseOut) +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2327,6 +2351,7 @@ CmdResult readGravityNoise(C::mip_interface& device, Vector3f noiseOut) { Serializer deserializer(buffer, responseLength); + assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -2498,12 +2523,13 @@ void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) } -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, Vector3f noise) +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -2511,7 +2537,7 @@ CmdResult writeHardIronOffsetNoise(C::mip_interface& device, Vector3f noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readHardIronOffsetNoise(C::mip_interface& device, Vector3f noiseOut) +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2526,6 +2552,7 @@ CmdResult readHardIronOffsetNoise(C::mip_interface& device, Vector3f noiseOut) { Serializer deserializer(buffer, responseLength); + assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -2600,12 +2627,13 @@ void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self) } -CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noise) +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(noise || (9 == 0)); for(unsigned int i=0; i < 9; i++) insert(serializer, noise[i]); @@ -2613,7 +2641,7 @@ CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noiseOut) +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2628,6 +2656,7 @@ CmdResult readSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noiseOut) { Serializer deserializer(buffer, responseLength); + assert(noiseOut || (9 == 0)); for(unsigned int i=0; i < 9; i++) extract(deserializer, noiseOut[i]); @@ -2702,12 +2731,13 @@ void extract(Serializer& serializer, MagNoise::Response& self) } -CmdResult writeMagNoise(C::mip_interface& device, Vector3f noise) +CmdResult writeMagNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, FunctionSelector::WRITE); + assert(noise || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, noise[i]); @@ -2715,7 +2745,7 @@ CmdResult writeMagNoise(C::mip_interface& device, Vector3f noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagNoise(C::mip_interface& device, Vector3f noiseOut) +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2730,6 +2760,7 @@ CmdResult readMagNoise(C::mip_interface& device, Vector3f noiseOut) { Serializer deserializer(buffer, responseLength); + assert(noiseOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, noiseOut[i]); @@ -4092,7 +4123,7 @@ void extract(Serializer& serializer, InitializationConfiguration::Response& self } -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, Vector3f initialPosition, Vector3f initialVelocity, FilterReferenceFrame referenceFrameSelector) +CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4110,9 +4141,11 @@ CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t wai insert(serializer, initialRoll); + assert(initialPosition || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, initialPosition[i]); + assert(initialVelocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, initialVelocity[i]); @@ -4122,7 +4155,7 @@ CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t wai return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, Vector3f initialPositionOut, Vector3f initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) +CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4155,9 +4188,11 @@ CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* wai assert(initialRollOut); extract(deserializer, *initialRollOut); + assert(initialPositionOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, initialPositionOut[i]); + assert(initialVelocityOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, initialVelocityOut[i]); @@ -4353,7 +4388,7 @@ void extract(Serializer& serializer, MultiAntennaOffset::Response& self) } -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffset) +CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4361,6 +4396,7 @@ CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, insert(serializer, FunctionSelector::WRITE); insert(serializer, receiverId); + assert(antennaOffset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, antennaOffset[i]); @@ -4368,7 +4404,7 @@ CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffsetOut) +CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4387,6 +4423,7 @@ CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, V extract(deserializer, receiverId); + assert(antennaOffsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, antennaOffsetOut[i]); @@ -4483,7 +4520,7 @@ void extract(Serializer& serializer, RelPosConfiguration::Response& self) } -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, Vector3d referenceCoordinates) +CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4493,6 +4530,7 @@ CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, Fil insert(serializer, referenceFrameSelector); + assert(referenceCoordinates || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, referenceCoordinates[i]); @@ -4500,7 +4538,7 @@ CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, Fil return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, Vector3d referenceCoordinatesOut) +CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4521,6 +4559,7 @@ CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, assert(referenceFrameSelectorOut); extract(deserializer, *referenceFrameSelectorOut); + assert(referenceCoordinatesOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, referenceCoordinatesOut[i]); @@ -4603,7 +4642,7 @@ void extract(Serializer& serializer, RefPointLeverArm::Response& self) } -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, Vector3f leverArmOffset) +CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4611,6 +4650,7 @@ CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refe insert(serializer, FunctionSelector::WRITE); insert(serializer, refPointSel); + assert(leverArmOffset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, leverArmOffset[i]); @@ -4618,7 +4658,7 @@ CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refe return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, Vector3f leverArmOffsetOut) +CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4636,6 +4676,7 @@ CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refer assert(refPointSelOut); extract(deserializer, *refPointSelOut); + assert(leverArmOffsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, leverArmOffsetOut[i]); @@ -4758,7 +4799,7 @@ void extract(Serializer& serializer, SpeedLeverArm::Response& self) } -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffset) +CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4766,6 +4807,7 @@ CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f insert(serializer, FunctionSelector::WRITE); insert(serializer, source); + assert(leverArmOffset || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, leverArmOffset[i]); @@ -4773,7 +4815,7 @@ CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffsetOut) +CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4792,6 +4834,7 @@ CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f l extract(deserializer, source); + assert(leverArmOffsetOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, leverArmOffsetOut[i]); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 2ca7ed16f..bb8f91c9f 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -312,7 +312,7 @@ typedef struct mip_filter_external_gnss_update_command mip_filter_external_gnss_ void insert_mip_filter_external_gnss_update_command(struct mip_serializer* serializer, const mip_filter_external_gnss_update_command* self); void extract_mip_filter_external_gnss_update_command(struct mip_serializer* serializer, mip_filter_external_gnss_update_command* self); -mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, mip_vector3f velocity, mip_vector3f pos_uncertainty, mip_vector3f vel_uncertainty); +mip_cmd_result mip_filter_external_gnss_update(struct mip_interface* device, double gps_time, uint16_t gps_week, double latitude, double longitude, double height, const float* velocity, const float* pos_uncertainty, const float* vel_uncertainty); ///@} /// @@ -589,8 +589,8 @@ typedef struct mip_filter_sensor_to_vehicle_rotation_dcm_response mip_filter_sen void insert_mip_filter_sensor_to_vehicle_rotation_dcm_response(struct mip_serializer* serializer, const mip_filter_sensor_to_vehicle_rotation_dcm_response* self); void extract_mip_filter_sensor_to_vehicle_rotation_dcm_response(struct mip_serializer* serializer, mip_filter_sensor_to_vehicle_rotation_dcm_response* self); -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm); -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, mip_matrix3f dcm_out); +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, const float* dcm); +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_dcm(struct mip_interface* device, float* dcm_out); mip_cmd_result mip_filter_save_sensor_to_vehicle_rotation_dcm(struct mip_interface* device); mip_cmd_result mip_filter_load_sensor_to_vehicle_rotation_dcm(struct mip_interface* device); mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_dcm(struct mip_interface* device); @@ -649,8 +649,8 @@ typedef struct mip_filter_sensor_to_vehicle_rotation_quaternion_response mip_fil void insert_mip_filter_sensor_to_vehicle_rotation_quaternion_response(struct mip_serializer* serializer, const mip_filter_sensor_to_vehicle_rotation_quaternion_response* self); void extract_mip_filter_sensor_to_vehicle_rotation_quaternion_response(struct mip_serializer* serializer, mip_filter_sensor_to_vehicle_rotation_quaternion_response* self); -mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat); -mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, mip_quatf quat_out); +mip_cmd_result mip_filter_write_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, const float* quat); +mip_cmd_result mip_filter_read_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device, float* quat_out); mip_cmd_result mip_filter_save_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device); mip_cmd_result mip_filter_load_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device); mip_cmd_result mip_filter_default_sensor_to_vehicle_rotation_quaternion(struct mip_interface* device); @@ -690,8 +690,8 @@ typedef struct mip_filter_sensor_to_vehicle_offset_response mip_filter_sensor_to void insert_mip_filter_sensor_to_vehicle_offset_response(struct mip_serializer* serializer, const mip_filter_sensor_to_vehicle_offset_response* self); void extract_mip_filter_sensor_to_vehicle_offset_response(struct mip_serializer* serializer, mip_filter_sensor_to_vehicle_offset_response* self); -mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset); -mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, mip_vector3f offset_out); +mip_cmd_result mip_filter_write_sensor_to_vehicle_offset(struct mip_interface* device, const float* offset); +mip_cmd_result mip_filter_read_sensor_to_vehicle_offset(struct mip_interface* device, float* offset_out); mip_cmd_result mip_filter_save_sensor_to_vehicle_offset(struct mip_interface* device); mip_cmd_result mip_filter_load_sensor_to_vehicle_offset(struct mip_interface* device); mip_cmd_result mip_filter_default_sensor_to_vehicle_offset(struct mip_interface* device); @@ -728,8 +728,8 @@ typedef struct mip_filter_antenna_offset_response mip_filter_antenna_offset_resp void insert_mip_filter_antenna_offset_response(struct mip_serializer* serializer, const mip_filter_antenna_offset_response* self); void extract_mip_filter_antenna_offset_response(struct mip_serializer* serializer, mip_filter_antenna_offset_response* self); -mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, mip_vector3f offset); -mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, mip_vector3f offset_out); +mip_cmd_result mip_filter_write_antenna_offset(struct mip_interface* device, const float* offset); +mip_cmd_result mip_filter_read_antenna_offset(struct mip_interface* device, float* offset_out); mip_cmd_result mip_filter_save_antenna_offset(struct mip_interface* device); mip_cmd_result mip_filter_load_antenna_offset(struct mip_interface* device); mip_cmd_result mip_filter_default_antenna_offset(struct mip_interface* device); @@ -911,8 +911,8 @@ typedef struct mip_filter_accel_noise_response mip_filter_accel_noise_response; void insert_mip_filter_accel_noise_response(struct mip_serializer* serializer, const mip_filter_accel_noise_response* self); void extract_mip_filter_accel_noise_response(struct mip_serializer* serializer, mip_filter_accel_noise_response* self); -mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, mip_vector3f noise); -mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, mip_vector3f noise_out); +mip_cmd_result mip_filter_write_accel_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_accel_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_noise(struct mip_interface* device); @@ -950,8 +950,8 @@ typedef struct mip_filter_gyro_noise_response mip_filter_gyro_noise_response; void insert_mip_filter_gyro_noise_response(struct mip_serializer* serializer, const mip_filter_gyro_noise_response* self); void extract_mip_filter_gyro_noise_response(struct mip_serializer* serializer, mip_filter_gyro_noise_response* self); -mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, mip_vector3f noise); -mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, mip_vector3f noise_out); +mip_cmd_result mip_filter_write_gyro_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_gyro_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_noise(struct mip_interface* device); @@ -988,8 +988,8 @@ typedef struct mip_filter_accel_bias_model_response mip_filter_accel_bias_model_ void insert_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, const mip_filter_accel_bias_model_response* self); void extract_mip_filter_accel_bias_model_response(struct mip_serializer* serializer, mip_filter_accel_bias_model_response* self); -mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise); -mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out); +mip_cmd_result mip_filter_write_accel_bias_model(struct mip_interface* device, const float* beta, const float* noise); +mip_cmd_result mip_filter_read_accel_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); mip_cmd_result mip_filter_save_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_accel_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_accel_bias_model(struct mip_interface* device); @@ -1026,8 +1026,8 @@ typedef struct mip_filter_gyro_bias_model_response mip_filter_gyro_bias_model_re void insert_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, const mip_filter_gyro_bias_model_response* self); void extract_mip_filter_gyro_bias_model_response(struct mip_serializer* serializer, mip_filter_gyro_bias_model_response* self); -mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, mip_vector3f beta, mip_vector3f noise); -mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, mip_vector3f beta_out, mip_vector3f noise_out); +mip_cmd_result mip_filter_write_gyro_bias_model(struct mip_interface* device, const float* beta, const float* noise); +mip_cmd_result mip_filter_read_gyro_bias_model(struct mip_interface* device, float* beta_out, float* noise_out); mip_cmd_result mip_filter_save_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_load_gyro_bias_model(struct mip_interface* device); mip_cmd_result mip_filter_default_gyro_bias_model(struct mip_interface* device); @@ -1264,8 +1264,8 @@ typedef struct mip_filter_gravity_noise_response mip_filter_gravity_noise_respon void insert_mip_filter_gravity_noise_response(struct mip_serializer* serializer, const mip_filter_gravity_noise_response* self); void extract_mip_filter_gravity_noise_response(struct mip_serializer* serializer, mip_filter_gravity_noise_response* self); -mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, mip_vector3f noise); -mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, mip_vector3f noise_out); +mip_cmd_result mip_filter_write_gravity_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_gravity_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_gravity_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_gravity_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_gravity_noise(struct mip_interface* device); @@ -1342,8 +1342,8 @@ typedef struct mip_filter_hard_iron_offset_noise_response mip_filter_hard_iron_o void insert_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, const mip_filter_hard_iron_offset_noise_response* self); void extract_mip_filter_hard_iron_offset_noise_response(struct mip_serializer* serializer, mip_filter_hard_iron_offset_noise_response* self); -mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise); -mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, mip_vector3f noise_out); +mip_cmd_result mip_filter_write_hard_iron_offset_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_hard_iron_offset_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_hard_iron_offset_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_hard_iron_offset_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_hard_iron_offset_noise(struct mip_interface* device); @@ -1381,8 +1381,8 @@ typedef struct mip_filter_soft_iron_matrix_noise_response mip_filter_soft_iron_m void insert_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, const mip_filter_soft_iron_matrix_noise_response* self); void extract_mip_filter_soft_iron_matrix_noise_response(struct mip_serializer* serializer, mip_filter_soft_iron_matrix_noise_response* self); -mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise); -mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, mip_matrix3f noise_out); +mip_cmd_result mip_filter_write_soft_iron_matrix_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_soft_iron_matrix_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_soft_iron_matrix_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_soft_iron_matrix_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_soft_iron_matrix_noise(struct mip_interface* device); @@ -1420,8 +1420,8 @@ typedef struct mip_filter_mag_noise_response mip_filter_mag_noise_response; void insert_mip_filter_mag_noise_response(struct mip_serializer* serializer, const mip_filter_mag_noise_response* self); void extract_mip_filter_mag_noise_response(struct mip_serializer* serializer, mip_filter_mag_noise_response* self); -mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, mip_vector3f noise); -mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, mip_vector3f noise_out); +mip_cmd_result mip_filter_write_mag_noise(struct mip_interface* device, const float* noise); +mip_cmd_result mip_filter_read_mag_noise(struct mip_interface* device, float* noise_out); mip_cmd_result mip_filter_save_mag_noise(struct mip_interface* device); mip_cmd_result mip_filter_load_mag_noise(struct mip_interface* device); mip_cmd_result mip_filter_default_mag_noise(struct mip_interface* device); @@ -1913,8 +1913,8 @@ typedef struct mip_filter_initialization_configuration_response mip_filter_initi void insert_mip_filter_initialization_configuration_response(struct mip_serializer* serializer, const mip_filter_initialization_configuration_response* self); void extract_mip_filter_initialization_configuration_response(struct mip_serializer* serializer, mip_filter_initialization_configuration_response* self); -mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, mip_vector3f initial_position, mip_vector3f initial_velocity, mip_filter_reference_frame reference_frame_selector); -mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, mip_vector3f initial_position_out, mip_vector3f initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out); +mip_cmd_result mip_filter_write_initialization_configuration(struct mip_interface* device, uint8_t wait_for_run_command, mip_filter_initialization_configuration_command_initial_condition_source initial_cond_src, mip_filter_initialization_configuration_command_alignment_selector auto_heading_alignment_selector, float initial_heading, float initial_pitch, float initial_roll, const float* initial_position, const float* initial_velocity, mip_filter_reference_frame reference_frame_selector); +mip_cmd_result mip_filter_read_initialization_configuration(struct mip_interface* device, uint8_t* wait_for_run_command_out, mip_filter_initialization_configuration_command_initial_condition_source* initial_cond_src_out, mip_filter_initialization_configuration_command_alignment_selector* auto_heading_alignment_selector_out, float* initial_heading_out, float* initial_pitch_out, float* initial_roll_out, float* initial_position_out, float* initial_velocity_out, mip_filter_reference_frame* reference_frame_selector_out); mip_cmd_result mip_filter_save_initialization_configuration(struct mip_interface* device); mip_cmd_result mip_filter_load_initialization_configuration(struct mip_interface* device); mip_cmd_result mip_filter_default_initialization_configuration(struct mip_interface* device); @@ -1986,8 +1986,8 @@ typedef struct mip_filter_multi_antenna_offset_response mip_filter_multi_antenna void insert_mip_filter_multi_antenna_offset_response(struct mip_serializer* serializer, const mip_filter_multi_antenna_offset_response* self); void extract_mip_filter_multi_antenna_offset_response(struct mip_serializer* serializer, mip_filter_multi_antenna_offset_response* self); -mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset); -mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, mip_vector3f antenna_offset_out); +mip_cmd_result mip_filter_write_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, const float* antenna_offset); +mip_cmd_result mip_filter_read_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id, float* antenna_offset_out); mip_cmd_result mip_filter_save_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id); mip_cmd_result mip_filter_load_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id); mip_cmd_result mip_filter_default_multi_antenna_offset(struct mip_interface* device, uint8_t receiver_id); @@ -2023,8 +2023,8 @@ typedef struct mip_filter_rel_pos_configuration_response mip_filter_rel_pos_conf void insert_mip_filter_rel_pos_configuration_response(struct mip_serializer* serializer, const mip_filter_rel_pos_configuration_response* self); void extract_mip_filter_rel_pos_configuration_response(struct mip_serializer* serializer, mip_filter_rel_pos_configuration_response* self); -mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, mip_vector3d reference_coordinates); -mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, mip_vector3d reference_coordinates_out); +mip_cmd_result mip_filter_write_rel_pos_configuration(struct mip_interface* device, uint8_t source, mip_filter_reference_frame reference_frame_selector, const double* reference_coordinates); +mip_cmd_result mip_filter_read_rel_pos_configuration(struct mip_interface* device, uint8_t* source_out, mip_filter_reference_frame* reference_frame_selector_out, double* reference_coordinates_out); mip_cmd_result mip_filter_save_rel_pos_configuration(struct mip_interface* device); mip_cmd_result mip_filter_load_rel_pos_configuration(struct mip_interface* device); mip_cmd_result mip_filter_default_rel_pos_configuration(struct mip_interface* device); @@ -2071,8 +2071,8 @@ typedef struct mip_filter_ref_point_lever_arm_response mip_filter_ref_point_leve void insert_mip_filter_ref_point_lever_arm_response(struct mip_serializer* serializer, const mip_filter_ref_point_lever_arm_response* self); void extract_mip_filter_ref_point_lever_arm_response(struct mip_serializer* serializer, mip_filter_ref_point_lever_arm_response* self); -mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, mip_vector3f lever_arm_offset); -mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, mip_vector3f lever_arm_offset_out); +mip_cmd_result mip_filter_write_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector ref_point_sel, const float* lever_arm_offset); +mip_cmd_result mip_filter_read_ref_point_lever_arm(struct mip_interface* device, mip_filter_ref_point_lever_arm_command_reference_point_selector* ref_point_sel_out, float* lever_arm_offset_out); mip_cmd_result mip_filter_save_ref_point_lever_arm(struct mip_interface* device); mip_cmd_result mip_filter_load_ref_point_lever_arm(struct mip_interface* device); mip_cmd_result mip_filter_default_ref_point_lever_arm(struct mip_interface* device); @@ -2136,8 +2136,8 @@ typedef struct mip_filter_speed_lever_arm_response mip_filter_speed_lever_arm_re void insert_mip_filter_speed_lever_arm_response(struct mip_serializer* serializer, const mip_filter_speed_lever_arm_response* self); void extract_mip_filter_speed_lever_arm_response(struct mip_serializer* serializer, mip_filter_speed_lever_arm_response* self); -mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset); -mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, mip_vector3f lever_arm_offset_out); +mip_cmd_result mip_filter_write_speed_lever_arm(struct mip_interface* device, uint8_t source, const float* lever_arm_offset); +mip_cmd_result mip_filter_read_speed_lever_arm(struct mip_interface* device, uint8_t source, float* lever_arm_offset_out); mip_cmd_result mip_filter_save_speed_lever_arm(struct mip_interface* device, uint8_t source); mip_cmd_result mip_filter_load_speed_lever_arm(struct mip_interface* device, uint8_t source); mip_cmd_result mip_filter_default_speed_lever_arm(struct mip_interface* device, uint8_t source); diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 28e88a9aa..9e43c1a1c 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -427,7 +427,7 @@ struct ExternalGnssUpdate void insert(Serializer& serializer, const ExternalGnssUpdate& self); void extract(Serializer& serializer, ExternalGnssUpdate& self); -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, Vector3f velocity, Vector3f posUncertainty, Vector3f velUncertainty); +CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); ///@} /// @@ -902,8 +902,8 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm& self); void insert(Serializer& serializer, const SensorToVehicleRotationDcm::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self); -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcm); -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, Matrix3f dcmOut); +CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); +CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device); CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device); CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); @@ -999,8 +999,8 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion& self); void insert(Serializer& serializer, const SensorToVehicleRotationQuaternion::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response& self); -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quat); -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, Quatf quatOut); +CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); +CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); @@ -1077,8 +1077,8 @@ void extract(Serializer& serializer, SensorToVehicleOffset& self); void insert(Serializer& serializer, const SensorToVehicleOffset::Response& self); void extract(Serializer& serializer, SensorToVehicleOffset::Response& self); -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, Vector3f offset); -CmdResult readSensorToVehicleOffset(C::mip_interface& device, Vector3f offsetOut); +CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); +CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); CmdResult saveSensorToVehicleOffset(C::mip_interface& device); CmdResult loadSensorToVehicleOffset(C::mip_interface& device); CmdResult defaultSensorToVehicleOffset(C::mip_interface& device); @@ -1152,8 +1152,8 @@ void extract(Serializer& serializer, AntennaOffset& self); void insert(Serializer& serializer, const AntennaOffset::Response& self); void extract(Serializer& serializer, AntennaOffset::Response& self); -CmdResult writeAntennaOffset(C::mip_interface& device, Vector3f offset); -CmdResult readAntennaOffset(C::mip_interface& device, Vector3f offsetOut); +CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset); +CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut); CmdResult saveAntennaOffset(C::mip_interface& device); CmdResult loadAntennaOffset(C::mip_interface& device); CmdResult defaultAntennaOffset(C::mip_interface& device); @@ -1481,8 +1481,8 @@ void extract(Serializer& serializer, AccelNoise& self); void insert(Serializer& serializer, const AccelNoise::Response& self); void extract(Serializer& serializer, AccelNoise::Response& self); -CmdResult writeAccelNoise(C::mip_interface& device, Vector3f noise); -CmdResult readAccelNoise(C::mip_interface& device, Vector3f noiseOut); +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise); +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut); CmdResult saveAccelNoise(C::mip_interface& device); CmdResult loadAccelNoise(C::mip_interface& device); CmdResult defaultAccelNoise(C::mip_interface& device); @@ -1557,8 +1557,8 @@ void extract(Serializer& serializer, GyroNoise& self); void insert(Serializer& serializer, const GyroNoise::Response& self); void extract(Serializer& serializer, GyroNoise::Response& self); -CmdResult writeGyroNoise(C::mip_interface& device, Vector3f noise); -CmdResult readGyroNoise(C::mip_interface& device, Vector3f noiseOut); +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise); +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut); CmdResult saveGyroNoise(C::mip_interface& device); CmdResult loadGyroNoise(C::mip_interface& device); CmdResult defaultGyroNoise(C::mip_interface& device); @@ -1632,8 +1632,8 @@ void extract(Serializer& serializer, AccelBiasModel& self); void insert(Serializer& serializer, const AccelBiasModel::Response& self); void extract(Serializer& serializer, AccelBiasModel::Response& self); -CmdResult writeAccelBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise); -CmdResult readAccelBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut); +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); CmdResult saveAccelBiasModel(C::mip_interface& device); CmdResult loadAccelBiasModel(C::mip_interface& device); CmdResult defaultAccelBiasModel(C::mip_interface& device); @@ -1707,8 +1707,8 @@ void extract(Serializer& serializer, GyroBiasModel& self); void insert(Serializer& serializer, const GyroBiasModel::Response& self); void extract(Serializer& serializer, GyroBiasModel::Response& self); -CmdResult writeGyroBiasModel(C::mip_interface& device, Vector3f beta, Vector3f noise); -CmdResult readGyroBiasModel(C::mip_interface& device, Vector3f betaOut, Vector3f noiseOut); +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); CmdResult saveGyroBiasModel(C::mip_interface& device); CmdResult loadGyroBiasModel(C::mip_interface& device); CmdResult defaultGyroBiasModel(C::mip_interface& device); @@ -2202,8 +2202,8 @@ void extract(Serializer& serializer, GravityNoise& self); void insert(Serializer& serializer, const GravityNoise::Response& self); void extract(Serializer& serializer, GravityNoise::Response& self); -CmdResult writeGravityNoise(C::mip_interface& device, Vector3f noise); -CmdResult readGravityNoise(C::mip_interface& device, Vector3f noiseOut); +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise); +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut); CmdResult saveGravityNoise(C::mip_interface& device); CmdResult loadGravityNoise(C::mip_interface& device); CmdResult defaultGravityNoise(C::mip_interface& device); @@ -2354,8 +2354,8 @@ void extract(Serializer& serializer, HardIronOffsetNoise& self); void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, Vector3f noise); -CmdResult readHardIronOffsetNoise(C::mip_interface& device, Vector3f noiseOut); +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); CmdResult saveHardIronOffsetNoise(C::mip_interface& device); CmdResult loadHardIronOffsetNoise(C::mip_interface& device); CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); @@ -2430,8 +2430,8 @@ void extract(Serializer& serializer, SoftIronMatrixNoise& self); void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self); void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self); -CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noise); -CmdResult readSoftIronMatrixNoise(C::mip_interface& device, Matrix3f noiseOut); +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); CmdResult saveSoftIronMatrixNoise(C::mip_interface& device); CmdResult loadSoftIronMatrixNoise(C::mip_interface& device); CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); @@ -2506,8 +2506,8 @@ void extract(Serializer& serializer, MagNoise& self); void insert(Serializer& serializer, const MagNoise::Response& self); void extract(Serializer& serializer, MagNoise::Response& self); -CmdResult writeMagNoise(C::mip_interface& device, Vector3f noise); -CmdResult readMagNoise(C::mip_interface& device, Vector3f noiseOut); +CmdResult writeMagNoise(C::mip_interface& device, const float* noise); +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut); CmdResult saveMagNoise(C::mip_interface& device); CmdResult loadMagNoise(C::mip_interface& device); CmdResult defaultMagNoise(C::mip_interface& device); @@ -3414,8 +3414,8 @@ void extract(Serializer& serializer, InitializationConfiguration& self); void insert(Serializer& serializer, const InitializationConfiguration::Response& self); void extract(Serializer& serializer, InitializationConfiguration::Response& self); -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, Vector3f initialPosition, Vector3f initialVelocity, FilterReferenceFrame referenceFrameSelector); -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, Vector3f initialPositionOut, Vector3f initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); +CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); +CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); CmdResult saveInitializationConfiguration(C::mip_interface& device); CmdResult loadInitializationConfiguration(C::mip_interface& device); CmdResult defaultInitializationConfiguration(C::mip_interface& device); @@ -3562,8 +3562,8 @@ void extract(Serializer& serializer, MultiAntennaOffset& self); void insert(Serializer& serializer, const MultiAntennaOffset::Response& self); void extract(Serializer& serializer, MultiAntennaOffset::Response& self); -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffset); -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, Vector3f antennaOffsetOut); +CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); +CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); @@ -3636,8 +3636,8 @@ void extract(Serializer& serializer, RelPosConfiguration& self); void insert(Serializer& serializer, const RelPosConfiguration::Response& self); void extract(Serializer& serializer, RelPosConfiguration::Response& self); -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, Vector3d referenceCoordinates); -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, Vector3d referenceCoordinatesOut); +CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); +CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); CmdResult saveRelPosConfiguration(C::mip_interface& device); CmdResult loadRelPosConfiguration(C::mip_interface& device); CmdResult defaultRelPosConfiguration(C::mip_interface& device); @@ -3720,8 +3720,8 @@ void extract(Serializer& serializer, RefPointLeverArm& self); void insert(Serializer& serializer, const RefPointLeverArm::Response& self); void extract(Serializer& serializer, RefPointLeverArm::Response& self); -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, Vector3f leverArmOffset); -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, Vector3f leverArmOffsetOut); +CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); +CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); CmdResult saveRefPointLeverArm(C::mip_interface& device); CmdResult loadRefPointLeverArm(C::mip_interface& device); CmdResult defaultRefPointLeverArm(C::mip_interface& device); @@ -3838,8 +3838,8 @@ void extract(Serializer& serializer, SpeedLeverArm& self); void insert(Serializer& serializer, const SpeedLeverArm::Response& self); void extract(Serializer& serializer, SpeedLeverArm::Response& self); -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffset); -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, Vector3f leverArmOffsetOut); +CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); +CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); From 733ba4d90da5facd3979e0a880a50c6e1346f5f4 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 11:55:20 -0400 Subject: [PATCH 104/252] Add missing include. --- src/mip/definitions/common.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 70bca3720..fc53ce499 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -9,6 +9,7 @@ #include #include +#include namespace mip { namespace C { From 9e5d6960fb67c785c33e44b0503cf3e1f829e3ce Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 12:12:49 -0400 Subject: [PATCH 105/252] Remove c++14 dependency (std::index_sequence). --- src/mip/definitions/common.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index fc53ce499..1114ff5f1 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -102,8 +102,8 @@ struct Vector template explicit Vector(const U* ptr, size_t n) { copyFrom(ptr, n); } - template - Vector(std::initializer_list values) : Vector(values, std::make_index_sequence()) {} + template + Vector(U u, V v, Rest... rest) : m_data{u, v, rest...} {} Vector(const Vector&) = default; Vector& operator=(const Vector&) = default; @@ -125,9 +125,6 @@ struct Vector void copyFrom(const U* ptr, size_t n) { if(n>N) n=N; for(size_t i=0; i - Vector(std::initializer_list values, std::index_sequence) : m_data{ *(values.begin()+Is)... } {} - T m_data[N]; }; From c76ba07c1384f2b819eac57612f6aef10fe58159 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 13:10:39 -0400 Subject: [PATCH 106/252] Attempt to work around bug in MSVC 2017. --- src/mip/definitions/common.h | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 1114ff5f1..d9d8426db 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -88,39 +88,72 @@ inline void extract(Serializer& serializer, DescriptorRate& self) { return C::ex template struct Vector { + /// Default constructor, no initialization. Vector() {} + /// Set all elements to this value (typically 0). + ///@param value template Vector(U value) { fill(value); } + /// Construct from a C array of known size. + ///@param ptr template Vector(const U (&ptr)[N]) { copyFrom(ptr, N); } + /// Construct from a C array of different size (smaller or larger vector). + ///@param ptr template explicit Vector(const U (&ptr)[M]) { static_assert(M>=N, "Input array is too small"); copyFrom(ptr, M); } + /// Construct from a pointer and size. + ///@param ptr Pointer to data to copy. Can be NULL if n==0. + ///@param n Number of elements to copy. Clamped to N. template explicit Vector(const U* ptr, size_t n) { copyFrom(ptr, n); } + /// Construct from individual elements or a braced init list. + ///@param u The first value (typically X). + ///@param v The value value (typically Y). + ///@param reset Additional optional values (typically none, Z, or Z and W). template Vector(U u, V v, Rest... rest) : m_data{u, v, rest...} {} + /// Copy constructor. Vector(const Vector&) = default; + + /// Assignment operator. Vector& operator=(const Vector&) = default; + /// Assignment operator from different type (e.g. float to double). template Vector& operator=(const Vector& other) { copyFrom(other, N); } + typedef T Array[N]; + /// Implicitly convert to a C-style array (rather than a pointer) so size information is preserved. operator Array&() { return m_data; } + +#if _MSC_VER < 1920 + // MSVC 2017 has a bug which causes operator[] to be ambiguous. + // See https://stackoverflow.com/questions/48250560/msvc-error-c2593-when-overloading-const-and-non-const-conversion-operator-return + operator const T*() const { return m_data; } +#else + // Normally an array ref is returned so that the size information is preserved. operator const Array&() const { return m_data; } +#endif + /// Explicitly convert to a C-style array. Array& data() { return m_data; } const Array& data() const { return m_data; } + /// Fill all elements with a given value. template void fill(U value) { for(size_t i=0; i void copyFrom(const U* ptr, size_t n) { if(n>N) n=N; for(size_t i=0; i Date: Fri, 11 Aug 2023 13:19:59 -0400 Subject: [PATCH 107/252] Attempt to work around bug in MSVC (still exists in 2019). --- src/mip/definitions/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index d9d8426db..9d3a34fcb 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -134,7 +134,7 @@ struct Vector /// Implicitly convert to a C-style array (rather than a pointer) so size information is preserved. operator Array&() { return m_data; } -#if _MSC_VER < 1920 +#if _MSC_VER < 1930 // MSVC 2017 has a bug which causes operator[] to be ambiguous. // See https://stackoverflow.com/questions/48250560/msvc-error-c2593-when-overloading-const-and-non-const-conversion-operator-return operator const T*() const { return m_data; } From c29dea6ca41cfadc57a60e831e5b259242cc1ada Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 13:25:11 -0400 Subject: [PATCH 108/252] Fix missing return statement. --- src/mip/definitions/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 9d3a34fcb..a4cbc7f08 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -127,7 +127,7 @@ struct Vector /// Assignment operator from different type (e.g. float to double). template - Vector& operator=(const Vector& other) { copyFrom(other, N); } + Vector& operator=(const Vector& other) { copyFrom(other, N); return *this; } typedef T Array[N]; From 624555bcadcd9a5a4015236716ec82eee8df0ecf Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 13:28:11 -0400 Subject: [PATCH 109/252] Fix missing return value and wrong parameter type. --- src/mip/mip.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 137634c6f..dac63581c 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -322,11 +322,11 @@ class SizedPacketBuf : public PacketRef ///@brief Copy constructor (required to put packets into std::vector in some cases). template - explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; + explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; ///@brief Assignment operator, copies data from another buffer to this one. template - SizedPacketBuf& operator=(const SizedPacketBuf& other) { copyFrom(other); } + SizedPacketBuf& operator=(const SizedPacketBuf& other) { copyFrom(other); return *this; } ///@brief Create a packet containing just the given field. /// From 87031d9c4f5c4008eeff02021fdb9a28c4842fae Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 11 Aug 2023 13:31:18 -0400 Subject: [PATCH 110/252] Attempt 2 to work around bug in MSVC. --- src/mip/definitions/common.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index a4cbc7f08..1f70357e5 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -131,15 +131,15 @@ struct Vector typedef T Array[N]; - /// Implicitly convert to a C-style array (rather than a pointer) so size information is preserved. - operator Array&() { return m_data; } #if _MSC_VER < 1930 // MSVC 2017 has a bug which causes operator[] to be ambiguous. // See https://stackoverflow.com/questions/48250560/msvc-error-c2593-when-overloading-const-and-non-const-conversion-operator-return + operator T*() { return m_data; } operator const T*() const { return m_data; } #else - // Normally an array ref is returned so that the size information is preserved. + /// Implicitly convert to a C-style array (rather than a pointer) so size information is preserved. + operator Array&() { return m_data; } operator const Array&() const { return m_data; } #endif From ce83b2c6479da63ba0d92a8542c7d186a067e5ad Mon Sep 17 00:00:00 2001 From: msclissa Date: Thu, 17 Aug 2023 16:16:25 -0400 Subject: [PATCH 111/252] Update CMakeLists.txt --- CMakeLists.txt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 24bf60d35..9e71c2bd7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,10 +22,11 @@ include_directories( ) if(WITH_INTERNAL) - set(MIP_INTERNAL_DIR "int" CACHE PATH "") - if(MIP_INTERNAL_DIR) - add_subdirectory("${MIP_INTERNAL_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/mip-internal") + if(NOT DEFINED MIP_INTERNAL_DIR) + set(MIP_INTERNAL_DIR "int" CACHE PATH "") endif() + + add_subdirectory("${MIP_INTERNAL_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/mip-internal") endif() set(MIP_DIR "${SRC_DIR}/mip") From 54cde6f3e18301702080a67da8144de7cffaaf20 Mon Sep 17 00:00:00 2001 From: msclissa Date: Thu, 17 Aug 2023 17:07:21 -0400 Subject: [PATCH 112/252] add Version interpreter from InertialConnect --- CMakeLists.txt | 1 + src/mip/utils/version.hpp | 74 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 75 insertions(+) create mode 100644 src/mip/utils/version.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e71c2bd7..ba4a22825 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,6 +103,7 @@ set(UTILS_SOURCES "${UTILS_DIR}/byte_ring.h" "${UTILS_DIR}/serialization.c" "${UTILS_DIR}/serialization.h" + "${UTILS_DIR}/version.hpp" ) # diff --git a/src/mip/utils/version.hpp b/src/mip/utils/version.hpp new file mode 100644 index 000000000..12a704992 --- /dev/null +++ b/src/mip/utils/version.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include + +//////////////////////////////////////////////////////////////////////////////// +///@defgroup mip_serialization MIP Serialization +/// +///@brief Serialization Functions for reading and writing to byte buffers. +///@{ +///@defgroup mip_serialization_c MIP Serialization [C] +///@defgroup mip_serialization_cpp MIP Serialization [CPP] +///@} + +#ifdef __cplusplus +#include + +namespace mip { +namespace C { +extern "C" { +#endif // __cplusplus + + + +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup version_cpp +/// +///@brief Version interpretation in C++. +/// +///@{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief Structure used interpretting version strings. +/// +/// +struct Version +{ + Version(uint16_t device_value=0) : m_value(device_value) {} + Version(uint8_t major_, uint8_t minor_, uint8_t patch_) : m_value(major_*100+minor_*10+patch_) { assert(major_<10 && minor_<10 && patch_<100); } + + uint8_t major() const { return m_value / 100; } + uint8_t minor() const { return (m_value / 10) % 10;} + uint8_t patch() const { return m_value % 100; } + uint16_t bcd() const { return m_value; } + + bool isDevVersion() const { return major() == 0; } + bool isReleaseVersion() const { return major() > 0; } + bool isSpecialVersion() const { return major() > 1; } + + bool operator==(Version other) const { return m_value == other.m_value; } + bool operator!=(Version other) const { return m_value != other.m_value; } + bool operator<=(Version other) const { return m_value <= other.m_value; } + bool operator>=(Version other) const { return m_value <= other.m_value; } + bool operator< (Version other) const { return m_value < other.m_value; } + bool operator> (Version other) const { return m_value < other.m_value; } + + static constexpr size_t STRING_BUFFER_SIZE = 7+1; // Including null + void toString(char* buffer) const { std::snprintf(buffer, STRING_BUFFER_SIZE, "%u.%u.%02u", major(), minor(), patch()); } + std::string toString() const { char buffer[STRING_BUFFER_SIZE]; toString(buffer); return std::string(buffer); } + +private: + uint16_t m_value = 0; +}; + + +///@} +///@} +//////////////////////////////////////////////////////////////////////////////// + +} // namespace mip +#endif // __cplusplus + +//////////////////////////////////////////////////////////////////////////////// \ No newline at end of file From 46bb7507b582fb2ad126d26681b8122eca99b31c Mon Sep 17 00:00:00 2001 From: msclissa Date: Thu, 17 Aug 2023 17:25:09 -0400 Subject: [PATCH 113/252] remove Version from C namespace --- src/mip/utils/version.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/mip/utils/version.hpp b/src/mip/utils/version.hpp index 12a704992..a4e7b86b0 100644 --- a/src/mip/utils/version.hpp +++ b/src/mip/utils/version.hpp @@ -17,8 +17,6 @@ #include namespace mip { -namespace C { -extern "C" { #endif // __cplusplus @@ -68,6 +66,7 @@ struct Version ///@} //////////////////////////////////////////////////////////////////////////////// +#ifdef __cplusplus } // namespace mip #endif // __cplusplus From 7bade63edceb8418a254fec499be878075efb3d2 Mon Sep 17 00:00:00 2001 From: msclissa Date: Fri, 18 Aug 2023 17:51:29 -0400 Subject: [PATCH 114/252] add model enum and info, improvements to version.hpp --- CMakeLists.txt | 2 + src/mip/mip_models.c | 82 +++++++++++++++++++++++++++++++++ src/mip/mip_models.h | 55 ++++++++++++++++++++++ src/mip/utils/version.hpp | 97 ++++++++++++++++++++++++--------------- 4 files changed, 198 insertions(+), 38 deletions(-) create mode 100644 src/mip/mip_models.c create mode 100644 src/mip/mip_models.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ba4a22825..ed7b07ecc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,6 +118,8 @@ set(MIP_SOURCES "${MIP_DIR}/mip_dispatch.h" "${MIP_DIR}/mip_field.c" "${MIP_DIR}/mip_field.h" + "${MIP_DIR}/mip_models.c" + "${MIP_DIR}/mip_models.h" "${MIP_DIR}/mip_offsets.h" "${MIP_DIR}/mip_packet.c" "${MIP_DIR}/mip_packet.h" diff --git a/src/mip/mip_models.c b/src/mip/mip_models.c new file mode 100644 index 000000000..c8e9d6b81 --- /dev/null +++ b/src/mip/mip_models.c @@ -0,0 +1,82 @@ +#include "mip_models.h" +#include +#include +#include + +#ifdef __cplusplus +namespace mip { +#endif // __cplusplus + + enum Model getModelFromSerial(const char* serial) + { + char model_str[16]; + bool found = false; + // The model number is just the portion of the serial before the dot + // serial number field is 16 chars + for (uint8_t i = 0; i < 16; i++) + { + model_str[i] = serial[i]; + if (serial[i] == '.') + { + found = true; + break; + } + } + + if (!found) + { + return MODEL_UNKNOWN; + } + + return atoi(model_str); + } + +#ifdef __cplusplus + + std::string getModelNameFromSerial(const char* serial) + { + enum Model model = getModelFromSerial(serial); + switch (model) + { + case MODEL_3DM_DH3 : return "3DM-DH3"; + case MODEL_3DM_GX3_15 : return "3DM-GX3-15"; + case MODEL_3DM_GX3_25 : return "3DM-GX3-25"; + case MODEL_3DM_GX3_35 : return "3DM-GX3-35"; + case MODEL_3DM_GX3_45 : return "3DM-GX3-45"; + case MODEL_3DM_RQ1_45_LT: return "3DM-RQ1-45_LT"; + case MODEL_3DM_GX4_15 : return "3DM-GX4-15"; + case MODEL_3DM_GX4_25 : return "3DM-GX4-25"; + case MODEL_3DM_GX4_45 : return "3DM-GX4-45"; + case MODEL_3DM_RQ1_45_ST: return "3DM-RQ1-45_ST"; + case MODEL_3DM_GX5_10 : return "3DM-GX5-10"; + case MODEL_3DM_GX5_15 : return "3DM-GX5-15"; + case MODEL_3DM_GX5_25 : return "3DM-GX5-25"; + case MODEL_3DM_GX5_35 : return "3DM-GX5-35"; + case MODEL_3DM_GX5_45 : return "3DM-GX5-45"; + case MODEL_3DM_CV5_10 : return "3DM-CV5-10"; + case MODEL_3DM_CV5_15 : return "3DM-CV5-15"; + case MODEL_3DM_CV5_25 : return "3DM-CV5-25"; + case MODEL_3DM_CV5_45 : return "3DM-CV5-45"; + case MODEL_3DM_GQ4_45 : return "3DM-GQ4-45"; + case MODEL_3DM_CX5_45 : return "3DM-CX5-45"; + case MODEL_3DM_CX5_35 : return "3DM-CX5-35"; + case MODEL_3DM_CX5_25 : return "3DM-CX5-25"; + case MODEL_3DM_CX5_15 : return "3DM-CX5-15"; + case MODEL_3DM_CX5_10 : return "3DM-CX5-10"; + case MODEL_3DM_CL5_15 : return "3DM-CL5-15"; + case MODEL_3DM_CL5_25 : return "3DM-CL5-25"; + case MODEL_3DM_GQ7 : return "3DM-GQ7"; + case MODEL_3DM_RTK : return "3DM-RTK"; + case MODEL_3DM_CV7_AHRS : return "3DM-CV7-AHRS"; + case MODEL_3DM_CV7_AR : return "3DM-CV7-AR"; + case MODEL_3DM_GV7_AHRS : return "3DM-GV7-AHRS"; + case MODEL_3DM_GV7_AR : return "3DM-GV7-AR"; + case MODEL_3DM_GV7_INS : return "3DM-GV7-INS"; + case MODEL_3DM_CV7_INS : return "3DM-CV7-INS"; + + default: + case MODEL_UNKNOWN : return ""; + } + } +} // namespace mip +#endif // __cplusplus \ No newline at end of file diff --git a/src/mip/mip_models.h b/src/mip/mip_models.h new file mode 100644 index 000000000..1faa2d3dd --- /dev/null +++ b/src/mip/mip_models.h @@ -0,0 +1,55 @@ +#pragma once + +#ifdef __cplusplus +#include +#include + +namespace mip { +#endif // __cplusplus + + enum Model + { + MODEL_UNKNOWN = 0, + MODEL_3DM_DH3 = 6219, // 3DM-DH3 + MODEL_3DM_GX3_15 = 6227, // 3DM-GX3-15 + MODEL_3DM_GX3_25 = 6223, // 3DM-GX3-25 + MODEL_3DM_GX3_35 = 6225, // 3DM-GX3-35 + MODEL_3DM_GX3_45 = 6228, // 3DM-GX3-45 + MODEL_3DM_RQ1_45_LT = 6232, // 3DM-RQ1-45-LT + MODEL_3DM_GX4_15 = 6233, // 3DM-GX4-15 + MODEL_3DM_GX4_25 = 6234, // 3DM-GX4-25 + MODEL_3DM_GX4_45 = 6236, // 3DM-GX4-45 + MODEL_3DM_RQ1_45_ST = 6239, // 3DM-RQ1-45-ST + MODEL_3DM_GX5_10 = 6255, // 3DM-GX5-10 + MODEL_3DM_GX5_15 = 6254, // 3DM-GX5-15 + MODEL_3DM_GX5_25 = 6253, // 3DM-GX5-25 + MODEL_3DM_GX5_35 = 6252, // 3DM-GX5-35 + MODEL_3DM_GX5_45 = 6251, // 3DM-GX5-45 + MODEL_3DM_CV5_10 = 6259, // 3DM-CV5-10 + MODEL_3DM_CV5_15 = 6258, // 3DM-CV5-15 + MODEL_3DM_CV5_25 = 6257, // 3DM-CV5-25 + MODEL_3DM_CV5_45 = 6256, // 3DM-CV5-45 + MODEL_3DM_GQ4_45 = 6250, // 3DM-GQ4-45 + MODEL_3DM_CX5_45 = 6271, // 3DM-CX5-45 + MODEL_3DM_CX5_35 = 6272, // 3DM-CX5-35 + MODEL_3DM_CX5_25 = 6273, // 3DM-CX5-25 + MODEL_3DM_CX5_15 = 6274, // 3DM-CX5-15 + MODEL_3DM_CX5_10 = 6275, // 3DM-CX5-10 + MODEL_3DM_CL5_15 = 6280, // 3DM-CL5-15 + MODEL_3DM_CL5_25 = 6281, // 3DM-CL5-25 + MODEL_3DM_GQ7 = 6284, // 3DM-GQ7 + MODEL_3DM_RTK = 6285, // 3DM-RTK + MODEL_3DM_CV7_AHRS = 6286, // 3DM-CV7-AHRS + MODEL_3DM_CV7_AR = 6287, // 3DM-CV7-AR + MODEL_3DM_GV7_AHRS = 6288, // 3DM-GV7-AHRS + MODEL_3DM_GV7_AR = 6289, // 3DM-GV7-AR + MODEL_3DM_GV7_INS = 6290, // 3DM-GV7-INS + MODEL_3DM_CV7_INS = 6291 // 3DM-CV7-INS + }; + + enum Model getModelFromSerial(const char* serial); + +#ifdef __cplusplus + std::string getModelNameFromSerial(const char* serial); +} // namespace mip +#endif // __cplusplus \ No newline at end of file diff --git a/src/mip/utils/version.hpp b/src/mip/utils/version.hpp index a4e7b86b0..97860b41f 100644 --- a/src/mip/utils/version.hpp +++ b/src/mip/utils/version.hpp @@ -3,6 +3,7 @@ #include #include #include +#include //////////////////////////////////////////////////////////////////////////////// ///@defgroup mip_serialization MIP Serialization @@ -17,56 +18,76 @@ #include namespace mip { -#endif // __cplusplus + //////////////////////////////////////////////////////////////////////////////// + ///@addtogroup version_cpp + /// + ///@brief Version interpretation in C++. + /// + ///@{ + //////////////////////////////////////////////////////////////////////////////// + ///@brief Structure used interpretting version strings. + /// + /// + struct Version + { + Version(uint16_t device_value=0) : m_value(device_value) {} + Version(uint8_t major_, uint8_t minor_, uint8_t patch_) : m_value(fromParts(major_, minor_, patch_)) { assert(major_<10 && minor_<10 && patch_<100); } -//////////////////////////////////////////////////////////////////////////////// -///@addtogroup version_cpp -/// -///@brief Version interpretation in C++. -/// -///@{ + uint8_t major() const { return m_value / 100; } + uint8_t minor() const { return (m_value / 10) % 10;} + uint8_t patch() const { return m_value % 100; } + uint16_t bcd() const { return m_value; } -//////////////////////////////////////////////////////////////////////////////// -///@brief Structure used interpretting version strings. -/// -/// -struct Version -{ - Version(uint16_t device_value=0) : m_value(device_value) {} - Version(uint8_t major_, uint8_t minor_, uint8_t patch_) : m_value(major_*100+minor_*10+patch_) { assert(major_<10 && minor_<10 && patch_<100); } + bool isDevVersion() const { return major() == 0; } + bool isReleaseVersion() const { return major() > 0; } + bool isSpecialVersion() const { return major() > 1; } - uint8_t major() const { return m_value / 100; } - uint8_t minor() const { return (m_value / 10) % 10;} - uint8_t patch() const { return m_value % 100; } - uint16_t bcd() const { return m_value; } + bool operator==(Version other) const { return m_value == other.m_value; } + bool operator!=(Version other) const { return m_value != other.m_value; } + bool operator<=(Version other) const { return m_value <= other.m_value; } + bool operator>=(Version other) const { return m_value <= other.m_value; } + bool operator< (Version other) const { return m_value < other.m_value; } + bool operator> (Version other) const { return m_value < other.m_value; } - bool isDevVersion() const { return major() == 0; } - bool isReleaseVersion() const { return major() > 0; } - bool isSpecialVersion() const { return major() > 1; } + static constexpr size_t STRING_BUFFER_SIZE = 7+1; // Including null + void toString(char* buffer) const { std::snprintf(buffer, STRING_BUFFER_SIZE, "%u.%u.%02u", major(), minor(), patch()); } + std::string toString() const { char buffer[STRING_BUFFER_SIZE]; toString(buffer); return std::string(buffer); } - bool operator==(Version other) const { return m_value == other.m_value; } - bool operator!=(Version other) const { return m_value != other.m_value; } - bool operator<=(Version other) const { return m_value <= other.m_value; } - bool operator>=(Version other) const { return m_value <= other.m_value; } - bool operator< (Version other) const { return m_value < other.m_value; } - bool operator> (Version other) const { return m_value < other.m_value; } + bool fromString(std::string str) + { + // Pattern to extract only numbers Major.Minor.Patch. Anything else before or after is ignored + const std::regex pattern(R"((\d+)\.?(\d+)?\.?(\d+)?)"); - static constexpr size_t STRING_BUFFER_SIZE = 7+1; // Including null - void toString(char* buffer) const { std::snprintf(buffer, STRING_BUFFER_SIZE, "%u.%u.%02u", major(), minor(), patch()); } - std::string toString() const { char buffer[STRING_BUFFER_SIZE]; toString(buffer); return std::string(buffer); } + std::smatch match; -private: - uint16_t m_value = 0; -}; + // Pattern will always find a Major number if it exists + if (std::regex_search(str, match, pattern)) + { + uint8_t major = std::stoi(match[1]); + // Minor is optionally found + uint8_t minor = match[2].matched ? std::stoi(match[2]) : 0; -///@} -///@} -//////////////////////////////////////////////////////////////////////////////// + // Patch is optionally found + uint8_t patch = match[3].matched ? std::stoi(match[3]) : 0; + + m_value = fromParts(major, minor, patch); + + return true; + } + + return false; + } + + private: + uint16_t m_value = 0; + + private: + static uint16_t fromParts(uint8_t major, uint8_t minor, uint8_t patch) { return major * 100 + minor * 10 + patch; } + }; -#ifdef __cplusplus } // namespace mip #endif // __cplusplus From a85f1968776f68b05bba5b1dad05f0caf8f1e2c3 Mon Sep 17 00:00:00 2001 From: msclissa Date: Mon, 21 Aug 2023 17:37:02 -0400 Subject: [PATCH 115/252] add function to get mode enum from model string --- src/mip/mip_models.c | 15 ++++++++++++--- src/mip/mip_models.h | 1 + 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/mip/mip_models.c b/src/mip/mip_models.c index c8e9d6b81..7e674e626 100644 --- a/src/mip/mip_models.c +++ b/src/mip/mip_models.c @@ -11,12 +11,13 @@ namespace mip { { char model_str[16]; bool found = false; - // The model number is just the portion of the serial before the dot - // serial number field is 16 chars + // The model number is just the portion of the serial or model string before the dot or dash + // serial and model number fields are 16 chars for (uint8_t i = 0; i < 16; i++) { model_str[i] = serial[i]; - if (serial[i] == '.') + if (serial[i] == '.' + || serial[i] == '-') { found = true; break; @@ -31,6 +32,14 @@ namespace mip { return atoi(model_str); } + enum Model getModelFromModelString(const char* model) + { + // same logic can be used on either serial or model number string + // serial: model.deviceid + // model: model-specifier + return getModelFromSerial(model); + } + #ifdef __cplusplus std::string getModelNameFromSerial(const char* serial) diff --git a/src/mip/mip_models.h b/src/mip/mip_models.h index 1faa2d3dd..a069e1da4 100644 --- a/src/mip/mip_models.h +++ b/src/mip/mip_models.h @@ -48,6 +48,7 @@ namespace mip { }; enum Model getModelFromSerial(const char* serial); + enum Model getModelFromModelString(const char* model); #ifdef __cplusplus std::string getModelNameFromSerial(const char* serial); From 61b36b189df525fb3a111a0e3fce8dd17154b636 Mon Sep 17 00:00:00 2001 From: msclissa Date: Fri, 25 Aug 2023 13:54:12 -0400 Subject: [PATCH 116/252] add ScopeHelper from MSCL --- CMakeLists.txt | 2 ++ src/mip/extras/scope_helper.cpp | 26 +++++++++++++++++++ src/mip/extras/scope_helper.hpp | 45 +++++++++++++++++++++++++++++++++ 3 files changed, 73 insertions(+) create mode 100644 src/mip/extras/scope_helper.cpp create mode 100644 src/mip/extras/scope_helper.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ed7b07ecc..bfcde4910 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -199,6 +199,8 @@ if(MIP_USE_EXTRAS) set(MIP_EXTRA_SOURCES "${MIP_EXTRAS_DIR}/recording_connection.hpp" "${MIP_EXTRAS_DIR}/recording_connection.cpp" + "${MIP_EXTRAS_DIR}/scope_helper.hpp" + "${MIP_EXTRAS_DIR}/scope_helper.cpp" ) endif() diff --git a/src/mip/extras/scope_helper.cpp b/src/mip/extras/scope_helper.cpp new file mode 100644 index 000000000..75c574262 --- /dev/null +++ b/src/mip/extras/scope_helper.cpp @@ -0,0 +1,26 @@ +#include "scope_helper.hpp" + +namespace mip +{ + namespace extras + { + ScopeHelper::ScopeHelper(std::function scopeFunction) : + m_outOfScopeFunction(scopeFunction), + m_canceled(false) + { + } + + ScopeHelper::~ScopeHelper() + { + if (!m_canceled) + { + m_outOfScopeFunction(); + } + } + + void ScopeHelper::cancel() + { + m_canceled = true; + } + } //namespace extras +} //namespace mip \ No newline at end of file diff --git a/src/mip/extras/scope_helper.hpp b/src/mip/extras/scope_helper.hpp new file mode 100644 index 000000000..ab2386a8d --- /dev/null +++ b/src/mip/extras/scope_helper.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include + +namespace mip +{ + namespace extras + { + //Class: ScopeHelper + // Class that allows a function to be run when this object goes out of scope. + class ScopeHelper + { + private: + //Variable: m_outOfScopeFunction + // The function to run when the ScopeHelper goes out of scope. + std::function m_outOfScopeFunction; + + //Variable: m_canceled + // Whether the scope function has been canceled or not. + bool m_canceled; + + ScopeHelper(); //default constructor disabled + ScopeHelper(const ScopeHelper&); //copy constructor disabled + ScopeHelper& operator=(const ScopeHelper&); //assignment operator disabled + + public: + //Constructor: ScopeHelper + // Creates a ScopeHelper object. + // + //Parameters: + // scopeFunction - The function to run when the ScopeHelper object is destroyed. + ScopeHelper(std::function scopeFunction); + + //Destructor: ScopeHelper + // Runs whatever function was assigned in the creation of the ScopeHelper. + ~ScopeHelper(); + + //Function: cancel + // Sets a flag that indicates the originally set function should NOT be run when + // the ScopeHelper is destroyed. + void cancel(); + }; + } //namespace extras +} //namespace mip \ No newline at end of file From 21f9798c683f09559fce583324a393f8fb022d66 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 25 Aug 2023 17:25:16 -0400 Subject: [PATCH 117/252] Make CompositeDescriptor constexpr. --- src/mip/definitions/descriptors.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index e95ca1294..1b26a2978 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -68,17 +68,17 @@ struct CompositeDescriptor uint8_t descriptorSet; ///< MIP descriptor set. uint8_t fieldDescriptor; ///< MIP field descriptor. - CompositeDescriptor(uint8_t descSet, uint8_t fieldDesc) : descriptorSet(descSet), fieldDescriptor(fieldDesc) {} - CompositeDescriptor(uint16_t combo) : descriptorSet(combo >> 8), fieldDescriptor(combo & 0xFF) {} + constexpr CompositeDescriptor(uint8_t descSet, uint8_t fieldDesc) : descriptorSet(descSet), fieldDescriptor(fieldDesc) {} + constexpr CompositeDescriptor(uint16_t combo) : descriptorSet(combo >> 8), fieldDescriptor(combo & 0xFF) {} - CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } + constexpr CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } - uint16_t as_u16() const { return (uint16_t(descriptorSet) << 8) | fieldDescriptor; } + constexpr uint16_t as_u16() const { return (uint16_t(descriptorSet) << 8) | fieldDescriptor; } // operator uint16_t() const { return as_u16(); } - bool operator==(const CompositeDescriptor& other) const { return other.descriptorSet == descriptorSet && other.fieldDescriptor == fieldDescriptor; } - bool operator<(const CompositeDescriptor& other) const { return descriptorSet < other.descriptorSet || (!(descriptorSet > other.descriptorSet) && (fieldDescriptor < other.fieldDescriptor)); } + constexpr bool operator==(const CompositeDescriptor& other) const { return other.descriptorSet == descriptorSet && other.fieldDescriptor == fieldDescriptor; } + constexpr bool operator<(const CompositeDescriptor& other) const { return as_u16() < other.as_u16(); } }; From cc58ff8069504060cd3b53848dd3d821cb8cc0ee Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 25 Aug 2023 17:26:56 -0400 Subject: [PATCH 118/252] Generate MIP definitions from branches/dev@55320. --- src/mip/definitions/commands_3dm.hpp | 1187 +++++++++++--------- src/mip/definitions/commands_aiding.hpp | 146 +-- src/mip/definitions/commands_base.hpp | 187 ++-- src/mip/definitions/commands_filter.hpp | 1369 ++++++++++++----------- src/mip/definitions/commands_gnss.hpp | 96 +- src/mip/definitions/commands_rtk.hpp | 217 ++-- src/mip/definitions/commands_system.hpp | 38 +- src/mip/definitions/data_filter.hpp | 275 +++-- src/mip/definitions/data_gnss.hpp | 149 ++- src/mip/definitions/data_sensor.hpp | 115 +- src/mip/definitions/data_shared.hpp | 47 +- src/mip/definitions/data_system.hpp | 20 +- 12 files changed, 2101 insertions(+), 1745 deletions(-) diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 76bafd152..caedca9e3 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -210,11 +210,12 @@ struct PollImuMessage uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. DescriptorRate descriptors[83]; ///< Descriptor list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -253,11 +254,12 @@ struct PollGnssMessage uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. DescriptorRate descriptors[83]; ///< Descriptor list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -296,11 +298,12 @@ struct PollFilterMessage uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. DescriptorRate descriptors[83]; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -334,17 +337,18 @@ struct ImuMessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -365,11 +369,12 @@ struct ImuMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. @@ -408,17 +413,18 @@ struct GpsMessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -439,11 +445,12 @@ struct GpsMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. @@ -482,17 +489,18 @@ struct FilterMessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) DescriptorRate descriptors[82]; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -513,11 +521,12 @@ struct FilterMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) DescriptorRate descriptors[82]; @@ -554,12 +563,13 @@ CmdResult defaultFilterMessageFormat(C::mip_interface& device); struct ImuGetBaseRate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -572,11 +582,12 @@ struct ImuGetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] @@ -608,12 +619,13 @@ CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); struct GpsGetBaseRate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -626,11 +638,12 @@ struct GpsGetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] @@ -662,12 +675,13 @@ CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); struct FilterGetBaseRate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -680,11 +694,12 @@ struct FilterGetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] @@ -724,11 +739,12 @@ struct PollData uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. uint8_t descriptors[82] = {0}; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; auto as_tuple() const { @@ -758,12 +774,13 @@ struct GetBaseRate { uint8_t desc_set = 0; ///< This is the data descriptor set. It must be a supported descriptor. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -776,11 +793,12 @@ struct GetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; ///< Echoes the parameter in the command. uint16_t rate = 0; ///< Base rate in Hz (0 = variable, unknown, or user-defined rate. Data will be sent when received). @@ -816,17 +834,18 @@ struct MessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) DescriptorRate descriptors[82]; ///< List of descriptors and decimations. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -848,11 +867,12 @@ struct MessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; uint8_t desc_set = 0; ///< Echoes the descriptor set from the command. uint8_t num_descriptors = 0; ///< Number of descriptors in the list. DescriptorRate descriptors[82]; ///< List of descriptors and decimations. @@ -896,11 +916,12 @@ struct NmeaPollData uint8_t count = 0; ///< Number of format entries (limited by payload size) NmeaMessage format_entries[40]; ///< List of format entries. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -932,17 +953,18 @@ struct NmeaMessageFormat uint8_t count = 0; ///< Number of format entries (limited by payload size) NmeaMessage format_entries[40]; ///< List of format entries. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -963,11 +985,12 @@ struct NmeaMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of format entries (limited by payload size) NmeaMessage format_entries[40]; ///< List of format entries. @@ -1006,16 +1029,17 @@ struct DeviceSettings { FunctionSelector function = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x0000; - static const uint32_t READ_PARAMS = 0x0000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x0000; + static constexpr const uint32_t READ_PARAMS = 0x0000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1070,17 +1094,18 @@ struct UartBaudrate FunctionSelector function = static_cast(0); uint32_t baud = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1101,11 +1126,12 @@ struct UartBaudrate struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint32_t baud = 0; @@ -1150,11 +1176,12 @@ struct FactoryStreaming Action action = static_cast(0); uint8_t reserved = 0; ///< Reserved. Set to 0x00. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1187,25 +1214,26 @@ CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action ac struct DatastreamControl { - static const uint8_t LEGACY_IMU_STREAM = 0x01; - static const uint8_t LEGACY_GNSS_STREAM = 0x02; - static const uint8_t LEGACY_FILTER_STREAM = 0x03; - static const uint8_t ALL_STREAMS = 0x00; + static constexpr const uint8_t LEGACY_IMU_STREAM = 0x01; + static constexpr const uint8_t LEGACY_GNSS_STREAM = 0x02; + static constexpr const uint8_t LEGACY_FILTER_STREAM = 0x03; + static constexpr const uint8_t ALL_STREAMS = 0x00; FunctionSelector function = static_cast(0); uint8_t desc_set = 0; ///< The descriptor set of the stream to control. When function is SAVE, LOAD, or DEFAULT, can be ALL_STREAMS(0) to apply to all descriptor sets. On Generation 5 products, this must be one of the above legacy constants. bool enable = 0; ///< True or false to enable or disable the stream. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1227,11 +1255,12 @@ struct DatastreamControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; bool enabled = 0; @@ -1330,17 +1359,18 @@ struct ConstellationSettings uint8_t config_count = 0; Settings settings[42]; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -1361,11 +1391,12 @@ struct ConstellationSettings struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; uint16_t max_channels_available = 0; ///< Maximum channels available uint16_t max_channels_use = 0; ///< Maximum channels to use uint8_t config_count = 0; ///< Number of constellation configurations @@ -1443,17 +1474,18 @@ struct GnssSbasSettings uint8_t num_included_prns = 0; ///< Number of SBAS PRNs to include in search (0 = include all) uint16_t included_prns[39] = {0}; ///< List of specific SBAS PRNs to search for - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; auto as_tuple() const { @@ -1474,11 +1506,12 @@ struct GnssSbasSettings struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; uint8_t enable_sbas = 0; ///< 0 - SBAS Disabled, 1 - SBAS enabled SBASOptions sbas_options; ///< SBAS options, see definition uint8_t num_included_prns = 0; ///< Number of SBAS PRNs to include in search (0 = include all) @@ -1533,17 +1566,18 @@ struct GnssAssistedFix AssistedFixOption option = static_cast(0); ///< Assisted fix options uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1564,11 +1598,12 @@ struct GnssAssistedFix struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AssistedFixOption option = static_cast(0); ///< Assisted fix options uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) @@ -1609,17 +1644,18 @@ struct GnssTimeAssistance uint16_t week_number = 0; ///< GPS Weeks since 1980 [weeks] float accuracy = 0; ///< Accuracy of time information [seconds] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1640,11 +1676,12 @@ struct GnssTimeAssistance struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; double tow = 0; ///< GPS Time of week [seconds] uint16_t week_number = 0; ///< GPS Weeks since 1980 [weeks] float accuracy = 0; ///< Accuracy of time information [seconds] @@ -1697,17 +1734,18 @@ struct ImuLowpassFilter uint16_t frequency = 0; ///< -3dB cutoff frequency in Hz. Will not affect filtering if 'manual' is false. uint8_t reserved = 0; ///< Reserved, set to 0x00. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1729,11 +1767,12 @@ struct ImuLowpassFilter struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t target_descriptor = 0; bool enable = 0; ///< True if the filter is currently enabled. bool manual = 0; ///< True if the filter cutoff was manually configured. @@ -1781,17 +1820,18 @@ struct PpsSource FunctionSelector function = static_cast(0); Source source = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1812,11 +1852,12 @@ struct PpsSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); @@ -1933,17 +1974,18 @@ struct GpioConfig Behavior behavior = static_cast(0); ///< Select an appropriate value from the enumeration based on the selected feature (e.g. for PPS, select one of the values prefixed with PPS_.) PinMode pin_mode; ///< GPIO configuration. May be restricted depending on device, pin, feature, and behavior. See device user manual. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1965,11 +2007,12 @@ struct GpioConfig struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t pin = 0; ///< GPIO pin number counting from 1. For save, load, and default function selectors, this can be 0 to select all pins. Feature feature = static_cast(0); ///< Determines how the pin will be used. Behavior behavior = static_cast(0); ///< Select an appropriate value from the enumeration based on the selected feature (e.g. for PPS, select one of the values prefixed with PPS_.) @@ -2023,17 +2066,18 @@ struct GpioState uint8_t pin = 0; ///< GPIO pin number counting from 1. Cannot be 0. bool state = 0; ///< The pin state. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2054,11 +2098,12 @@ struct GpioState struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t pin = 0; ///< GPIO pin number counting from 1. Cannot be 0. bool state = 0; ///< The pin state. @@ -2100,17 +2145,18 @@ struct Odometer float scaling = 0; ///< Encoder pulses per meter of distance traveled [pulses/m]. Distance traveled is computed using the formula d = p / N * 2R * pi, where d is distance, p is the number of pulses received, N is the encoder resolution, and R is the wheel radius. By simplifying all of the parameters into one, the formula d = p / S is obtained, where s is the odometer scaling factor passed to this command. S is equivalent to N / (2R * pi) and has units of pulses / meter. N is in units of "A" pulses per revolution and R is in meters. Make this value negative if the odometer is mounted so that it rotates backwards. float uncertainty = 0; ///< Uncertainty in encoder counts to distance translation (1-sigma value) [m/m]. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2131,11 +2177,12 @@ struct Odometer struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Mode mode = static_cast(0); ///< Mode setting. float scaling = 0; ///< Encoder pulses per meter of distance traveled [pulses/m]. Distance traveled is computed using the formula d = p / N * 2R * pi, where d is distance, p is the number of pulses received, N is the encoder resolution, and R is the wheel radius. By simplifying all of the parameters into one, the formula d = p / S is obtained, where s is the odometer scaling factor passed to this command. S is equivalent to N / (2R * pi) and has units of pulses / meter. N is in units of "A" pulses per revolution and R is in meters. Make this value negative if the odometer is mounted so that it rotates backwards. float uncertainty = 0; ///< Uncertainty in encoder counts to distance translation (1-sigma value) [m/m]. @@ -2199,12 +2246,13 @@ struct GetEventSupport }; Query query = static_cast(0); ///< What type of information to retrieve. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2217,11 +2265,12 @@ struct GetEventSupport } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; Query query = static_cast(0); ///< Query type specified in the command. uint8_t max_instances = 0; ///< Number of slots available. The 'instance' number for the configuration or control commands must be between 1 and this value. uint8_t num_entries = 0; ///< Number of supported types. @@ -2276,17 +2325,18 @@ struct EventControl uint8_t instance = 0; ///< Trigger instance to affect. 0 can be used to apply the mode to all configured triggers, except when the function selector is READ. Mode mode = static_cast(0); ///< How to change the trigger state. Except when instance is 0, the corresponding trigger must be configured, i.e. not have type 0. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2308,11 +2358,12 @@ struct EventControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Trigger instance to affect. 0 can be used to apply the mode to all configured triggers, except when the function selector is READ. Mode mode = static_cast(0); ///< How to change the trigger state. Except when instance is 0, the corresponding trigger must be configured, i.e. not have type 0. @@ -2384,12 +2435,13 @@ struct GetEventTriggerStatus uint8_t requested_count = 0; ///< Number of entries requested. If 0, requests all trigger slots. uint8_t requested_instances[20] = {0}; ///< List of trigger instances to query. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -2402,11 +2454,12 @@ struct GetEventTriggerStatus } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of entries requested. If requested_count was 0, this is the number of supported trigger slots. Entry triggers[20]; ///< A list of the configured triggers. Entries are in the order requested, or in increasing order if count was 0. @@ -2446,12 +2499,13 @@ struct GetEventActionStatus uint8_t requested_count = 0; ///< Number of entries requested. If 0, requests all action slots. uint8_t requested_instances[20] = {0}; ///< List of action instances to query. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -2464,11 +2518,12 @@ struct GetEventActionStatus } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of entries requested. If requested_count was 0, this is the number of supported action slots. Entry actions[20]; ///< A list of the configured actions. Entries are in the order requested, or in increasing order if count was 0. @@ -2540,19 +2595,19 @@ struct EventTrigger }; struct CombinationParams { - static const uint16_t LOGIC_NEVER = 0x0000; - static const uint16_t LOGIC_ALWAYS = 0xFFFF; - static const uint16_t LOGIC_NONE = 0x0001; - static const uint16_t LOGIC_OR = 0xFFFE; - static const uint16_t LOGIC_NAND = 0x7FFF; - static const uint16_t LOGIC_XOR_ONE = 0x0116; - static const uint16_t LOGIC_ONLY_A = 0x0002; - static const uint16_t LOGIC_ONLY_B = 0x0004; - static const uint16_t LOGIC_ONLY_C = 0x0010; - static const uint16_t LOGIC_ONLY_D = 0x0100; - static const uint16_t LOGIC_AND_AB = 0x8888; - static const uint16_t LOGIC_AB_OR_C = 0xF8F8; - static const uint16_t LOGIC_AND = 0x8000; + static constexpr const uint16_t LOGIC_NEVER = 0x0000; + static constexpr const uint16_t LOGIC_ALWAYS = 0xFFFF; + static constexpr const uint16_t LOGIC_NONE = 0x0001; + static constexpr const uint16_t LOGIC_OR = 0xFFFE; + static constexpr const uint16_t LOGIC_NAND = 0x7FFF; + static constexpr const uint16_t LOGIC_XOR_ONE = 0x0116; + static constexpr const uint16_t LOGIC_ONLY_A = 0x0002; + static constexpr const uint16_t LOGIC_ONLY_B = 0x0004; + static constexpr const uint16_t LOGIC_ONLY_C = 0x0010; + static constexpr const uint16_t LOGIC_ONLY_D = 0x0100; + static constexpr const uint16_t LOGIC_AND_AB = 0x8888; + static constexpr const uint16_t LOGIC_AB_OR_C = 0xF8F8; + static constexpr const uint16_t LOGIC_AND = 0x8000; uint16_t logic_table = 0; ///< The last column of a truth table describing the output given the state of each input. uint8_t input_triggers[4] = {0}; ///< List of trigger IDs for inputs. Use 0 for unused inputs. @@ -2578,17 +2633,18 @@ struct EventTrigger Type type = static_cast(0); ///< Type of trigger to configure. Parameters parameters; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2610,11 +2666,12 @@ struct EventTrigger struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Trigger number. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all instances. Type type = static_cast(0); ///< Type of trigger to configure. Parameters parameters; @@ -2701,17 +2758,18 @@ struct EventAction Type type = static_cast(0); ///< Type of action to configure. Parameters parameters; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2733,11 +2791,12 @@ struct EventAction struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Action number. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all instances. uint8_t trigger = 0; ///< Trigger ID number. Type type = static_cast(0); ///< Type of action to configure. @@ -2783,17 +2842,18 @@ struct AccelBias FunctionSelector function = static_cast(0); Vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2814,11 +2874,12 @@ struct AccelBias struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] @@ -2855,17 +2916,18 @@ struct GyroBias FunctionSelector function = static_cast(0); Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2886,11 +2948,12 @@ struct GyroBias struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] @@ -2929,12 +2992,13 @@ struct CaptureGyroBias { uint16_t averaging_time_ms = 0; ///< Averaging time [milliseconds] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2947,11 +3011,12 @@ struct CaptureGyroBias } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] @@ -2988,17 +3053,18 @@ struct MagHardIronOffset FunctionSelector function = static_cast(0); Vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3019,11 +3085,12 @@ struct MagHardIronOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] @@ -3068,17 +3135,18 @@ struct MagSoftIronMatrix FunctionSelector function = static_cast(0); Matrix3f offset; ///< soft iron matrix [dimensionless] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3099,11 +3167,12 @@ struct MagSoftIronMatrix struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f offset; ///< soft iron matrix [dimensionless] @@ -3138,17 +3207,18 @@ struct ConingScullingEnable FunctionSelector function = static_cast(0); bool enable = 0; ///< If true, coning and sculling compensation is enabled. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3169,11 +3239,12 @@ struct ConingScullingEnable struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< If true, coning and sculling compensation is enabled. @@ -3234,17 +3305,18 @@ struct Sensor2VehicleTransformEuler float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3265,11 +3337,12 @@ struct Sensor2VehicleTransformEuler struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] @@ -3338,17 +3411,18 @@ struct Sensor2VehicleTransformQuaternion FunctionSelector function = static_cast(0); Quatf q; ///< Unit length quaternion representing transform [w, i, j, k] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3369,11 +3443,12 @@ struct Sensor2VehicleTransformQuaternion struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Quatf q; ///< Unit length quaternion representing transform [w, i, j, k] @@ -3438,17 +3513,18 @@ struct Sensor2VehicleTransformDcm FunctionSelector function = static_cast(0); Matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3469,11 +3545,12 @@ struct Sensor2VehicleTransformDcm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order @@ -3515,17 +3592,18 @@ struct ComplementaryFilter float pitch_roll_time_constant = 0; ///< Time constant associated with the pitch/roll corrections [s] float heading_time_constant = 0; ///< Time constant associated with the heading corrections [s] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3546,11 +3624,12 @@ struct ComplementaryFilter struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool pitch_roll_enable = 0; ///< Enable Pitch/Roll corrections bool heading_enable = 0; ///< Enable Heading corrections (only available on devices with magnetometer) float pitch_roll_time_constant = 0; ///< Time constant associated with the pitch/roll corrections [s] @@ -3596,17 +3675,18 @@ struct SensorRange SensorRangeType sensor = static_cast(0); ///< Which type of sensor will get the new range value. uint8_t setting = 0; ///< Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine this value. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3628,11 +3708,12 @@ struct SensorRange struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; SensorRangeType sensor = static_cast(0); ///< Which type of sensor will get the new range value. uint8_t setting = 0; ///< Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine this value. @@ -3676,12 +3757,13 @@ struct CalibratedSensorRanges }; SensorRangeType sensor = static_cast(0); ///< The sensor to query. Cannot be ALL. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3694,11 +3776,12 @@ struct CalibratedSensorRanges } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; SensorRangeType sensor = static_cast(0); ///< The sensor type from the command. uint8_t num_ranges = 0; ///< Number of supported ranges. Entry ranges[50]; ///< List of possible range settings. @@ -3751,17 +3834,18 @@ struct LowpassFilter bool manual = 0; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. float frequency = 0; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8003; - static const uint32_t SAVE_PARAMS = 0x8003; - static const uint32_t LOAD_PARAMS = 0x8003; - static const uint32_t DEFAULT_PARAMS = 0x8003; - static const uint32_t ECHOED_PARAMS = 0x0003; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8003; + static constexpr const uint32_t SAVE_PARAMS = 0x8003; + static constexpr const uint32_t LOAD_PARAMS = 0x8003; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8003; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3784,11 +3868,12 @@ struct LowpassFilter struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0003; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; ///< Descriptor set of the quantity to be filtered. uint8_t field_desc = 0; ///< Field descriptor of the quantity to be filtered. bool enable = 0; ///< The filter will be enabled if this is true. diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 4355c820d..e7cf75c2c 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -92,17 +92,18 @@ struct SensorFrameMapping uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; - - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -123,11 +124,12 @@ struct SensorFrameMapping struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. @@ -171,17 +173,18 @@ struct ReferenceFrame Vector3f translation; ///< Translation X, Y, and Z. Quatf rotation; ///< Depends on the format parameter. Unused values are ignored. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -203,11 +206,12 @@ struct ReferenceFrame struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. Vector3f translation; ///< Translation X, Y, and Z. @@ -251,17 +255,18 @@ struct AidingEchoControl FunctionSelector function = static_cast(0); Mode mode = static_cast(0); ///< Controls data echoing. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -282,11 +287,12 @@ struct AidingEchoControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Mode mode = static_cast(0); ///< Controls data echoing. @@ -355,11 +361,12 @@ struct EcefPos Vector3f uncertainty; ///< ECEF position uncertainty [m]. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -427,11 +434,12 @@ struct LlhPos Vector3f uncertainty; ///< NED position uncertainty. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -496,11 +504,12 @@ struct EcefVel Vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -565,11 +574,12 @@ struct NedVel Vector3f uncertainty; ///< NED velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -635,11 +645,12 @@ struct VehicleFixedFrameVelocity Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -672,11 +683,12 @@ struct TrueHeading float uncertainty = 0; uint16_t valid_flags = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index e75c8fd70..5665a4753 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -199,11 +199,12 @@ struct CommandedTestBitsGq7 : Bitfield struct Ping { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -236,11 +237,12 @@ CmdResult ping(C::mip_interface& device); struct SetIdle { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -269,12 +271,13 @@ CmdResult setIdle(C::mip_interface& device); struct GetDeviceInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -287,11 +290,12 @@ struct GetDeviceInfo } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; BaseDeviceInfo device_info; @@ -323,12 +327,13 @@ CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) struct GetDeviceDescriptors { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -341,11 +346,12 @@ struct GetDeviceDescriptors } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; uint16_t descriptors[253] = {0}; uint8_t descriptors_count = 0; @@ -380,12 +386,13 @@ CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOu struct BuiltInTest { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -398,11 +405,12 @@ struct BuiltInTest } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint32_t result = 0; @@ -433,11 +441,12 @@ CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut); struct Resume { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -469,12 +478,13 @@ CmdResult resume(C::mip_interface& device); struct GetExtendedDescriptors { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -487,11 +497,12 @@ struct GetExtendedDescriptors } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; uint16_t descriptors[253] = {0}; uint8_t descriptors_count = 0; @@ -523,12 +534,13 @@ CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptors struct ContinuousBit { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -541,11 +553,12 @@ struct ContinuousBit } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. @@ -586,22 +599,23 @@ CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut); struct CommSpeed { - static const uint32_t ALL_PORTS = 0; + static constexpr const uint32_t ALL_PORTS = 0; FunctionSelector function = static_cast(0); uint8_t port = 0; ///< Port ID number, starting with 1. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all ports. See the device user manual for details. uint32_t baud = 0; ///< Port baud rate. Must be a supported rate. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -623,11 +637,12 @@ struct CommSpeed struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t port = 0; ///< Port ID number, starting with 1. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all ports. See the device user manual for details. uint32_t baud = 0; ///< Port baud rate. Must be a supported rate. @@ -673,16 +688,17 @@ struct GpsTimeUpdate FieldId field_id = static_cast(0); ///< Determines how to interpret value. uint32_t value = 0; ///< Week number or time of week, depending on the field_id. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x0000; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x0000; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -721,11 +737,12 @@ CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fi struct SoftReset { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 9e43c1a1c..1451f409c 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -192,11 +192,12 @@ enum class FilterAdaptiveMeasurement : uint8_t struct Reset { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -239,11 +240,12 @@ struct SetInitialAttitude float pitch = 0; ///< [radians] float heading = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -327,17 +329,18 @@ struct EstimationControl FunctionSelector function = static_cast(0); EnableFlags enable; ///< See above - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -358,11 +361,12 @@ struct EstimationControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; EnableFlags enable; ///< See above @@ -407,11 +411,12 @@ struct ExternalGnssUpdate Vector3f pos_uncertainty; ///< NED Frame, 1-sigma [meters] Vector3f vel_uncertainty; ///< NED Frame, 1-sigma [meters/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -455,11 +460,12 @@ struct ExternalHeadingUpdate float heading_uncertainty = 0; ///< 1-sigma [radians] uint8_t type = 0; ///< 1 - True, 2 - Magnetic - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -509,11 +515,12 @@ struct ExternalHeadingUpdateWithTime float heading_uncertainty = 0; ///< 1-sigma [radians] uint8_t type = 0; ///< 1 - True, 2 - Magnetic - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -579,17 +586,18 @@ struct TareOrientation FunctionSelector function = static_cast(0); MipTareAxes axes; ///< Axes to tare - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -610,11 +618,12 @@ struct TareOrientation struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; MipTareAxes axes; ///< Axes to tare @@ -657,17 +666,18 @@ struct VehicleDynamicsMode FunctionSelector function = static_cast(0); DynamicsMode mode = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -688,11 +698,12 @@ struct VehicleDynamicsMode struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; DynamicsMode mode = static_cast(0); @@ -751,17 +762,18 @@ struct SensorToVehicleRotationEuler float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -782,11 +794,12 @@ struct SensorToVehicleRotationEuler struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] @@ -851,17 +864,18 @@ struct SensorToVehicleRotationDcm FunctionSelector function = static_cast(0); Matrix3f dcm; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -882,11 +896,12 @@ struct SensorToVehicleRotationDcm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f dcm; @@ -948,17 +963,18 @@ struct SensorToVehicleRotationQuaternion FunctionSelector function = static_cast(0); Quatf quat; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -979,11 +995,12 @@ struct SensorToVehicleRotationQuaternion struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Quatf quat; @@ -1026,17 +1043,18 @@ struct SensorToVehicleOffset FunctionSelector function = static_cast(0); Vector3f offset; ///< [meters] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1057,11 +1075,12 @@ struct SensorToVehicleOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f offset; ///< [meters] @@ -1101,17 +1120,18 @@ struct AntennaOffset FunctionSelector function = static_cast(0); Vector3f offset; ///< [meters] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1132,11 +1152,12 @@ struct AntennaOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f offset; ///< [meters] @@ -1183,17 +1204,18 @@ struct GnssSource FunctionSelector function = static_cast(0); Source source = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1214,11 +1236,12 @@ struct GnssSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); @@ -1276,17 +1299,18 @@ struct HeadingSource FunctionSelector function = static_cast(0); Source source = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1307,11 +1331,12 @@ struct HeadingSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); @@ -1354,17 +1379,18 @@ struct AutoInitControl FunctionSelector function = static_cast(0); uint8_t enable = 0; ///< See above - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1385,11 +1411,12 @@ struct AutoInitControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< See above @@ -1430,17 +1457,18 @@ struct AccelNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1461,11 +1489,12 @@ struct AccelNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] @@ -1506,17 +1535,18 @@ struct GyroNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1537,11 +1567,12 @@ struct GyroNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] @@ -1580,17 +1611,18 @@ struct AccelBiasModel Vector3f beta; ///< Accel Bias Beta [1/second] Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1611,11 +1643,12 @@ struct AccelBiasModel struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f beta; ///< Accel Bias Beta [1/second] Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] @@ -1655,17 +1688,18 @@ struct GyroBiasModel Vector3f beta; ///< Gyro Bias Beta [1/second] Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1686,11 +1720,12 @@ struct GyroBiasModel struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f beta; ///< Gyro Bias Beta [1/second] Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] @@ -1736,17 +1771,18 @@ struct AltitudeAiding FunctionSelector function = static_cast(0); AidingSelector selector = static_cast(0); ///< See above - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1767,11 +1803,12 @@ struct AltitudeAiding struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AidingSelector selector = static_cast(0); ///< See above @@ -1813,17 +1850,18 @@ struct PitchRollAiding FunctionSelector function = static_cast(0); AidingSource source = static_cast(0); ///< Controls the aiding source - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1844,11 +1882,12 @@ struct PitchRollAiding struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AidingSource source = static_cast(0); ///< Controls the aiding source @@ -1885,17 +1924,18 @@ struct AutoZupt uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [meters/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1916,11 +1956,12 @@ struct AutoZupt struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [meters/second] @@ -1959,17 +2000,18 @@ struct AutoAngularZupt uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [radians/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1990,11 +2032,12 @@ struct AutoAngularZupt struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [radians/second] @@ -2028,11 +2071,12 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device); struct CommandedZupt { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2061,11 +2105,12 @@ CmdResult commandedZupt(C::mip_interface& device); struct CommandedAngularZupt { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2097,16 +2142,17 @@ struct MagCaptureAutoCal { FunctionSelector function = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8000; - static const uint32_t READ_PARAMS = 0x0000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8000; + static constexpr const uint32_t READ_PARAMS = 0x0000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2151,17 +2197,18 @@ struct GravityNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Gravity Noise 1-sigma [gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2182,11 +2229,12 @@ struct GravityNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Gravity Noise 1-sigma [gauss] @@ -2226,17 +2274,18 @@ struct PressureAltitudeNoise FunctionSelector function = static_cast(0); float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2257,11 +2306,12 @@ struct PressureAltitudeNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] @@ -2303,17 +2353,18 @@ struct HardIronOffsetNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2334,11 +2385,12 @@ struct HardIronOffsetNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] @@ -2379,17 +2431,18 @@ struct SoftIronMatrixNoise FunctionSelector function = static_cast(0); Matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2410,11 +2463,12 @@ struct SoftIronMatrixNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] @@ -2455,17 +2509,18 @@ struct MagNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Mag Noise 1-sigma [gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2486,11 +2541,12 @@ struct MagNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Mag Noise 1-sigma [gauss] @@ -2530,17 +2586,18 @@ struct InclinationSource FilterMagParamSource source = static_cast(0); ///< Inclination Source float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2561,11 +2618,12 @@ struct InclinationSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Inclination Source float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) @@ -2606,17 +2664,18 @@ struct MagneticDeclinationSource FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2637,11 +2696,12 @@ struct MagneticDeclinationSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) @@ -2681,17 +2741,18 @@ struct MagFieldMagnitudeSource FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2712,11 +2773,12 @@ struct MagFieldMagnitudeSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) @@ -2758,17 +2820,18 @@ struct ReferencePosition double longitude = 0; ///< [degrees] double altitude = 0; ///< [meters] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2789,11 +2852,12 @@ struct ReferencePosition struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< enable/disable double latitude = 0; ///< [degrees] double longitude = 0; ///< [degrees] @@ -2850,17 +2914,18 @@ struct AccelMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x807F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x807F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2881,11 +2946,12 @@ struct AccelMagnitudeErrorAdaptiveMeasurement struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float low_limit = 0; ///< [meters/second^2] @@ -2940,17 +3006,18 @@ struct MagMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x807F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x807F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2971,11 +3038,12 @@ struct MagMagnitudeErrorAdaptiveMeasurement struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float low_limit = 0; ///< [meters/second^2] @@ -3030,17 +3098,18 @@ struct MagDipAngleErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3061,11 +3130,12 @@ struct MagDipAngleErrorAdaptiveMeasurement struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< Enable/Disable float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float high_limit = 0; ///< [meters/second^2] @@ -3118,17 +3188,18 @@ struct AidingMeasurementEnable AidingSource aiding_source = static_cast(0); ///< Aiding measurement source bool enable = 0; ///< Controls the aiding source - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3150,11 +3221,12 @@ struct AidingMeasurementEnable struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AidingSource aiding_source = static_cast(0); ///< Aiding measurement source bool enable = 0; ///< Controls the aiding source @@ -3190,11 +3262,12 @@ CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasure struct Run { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3229,17 +3302,18 @@ struct KinematicConstraint uint8_t velocity_constraint_selection = 0; ///< 0=None (default),
1=Zero-velocity,
2=Wheeled-vehicle.
uint8_t angular_constraint_selection = 0; ///< 0=None (default), 1=Zero-angular rate (ZUPT). - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3260,11 +3334,12 @@ struct KinematicConstraint struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t acceleration_constraint_selection = 0; ///< Acceleration constraint:
0=None (default),
1=Zero-acceleration. uint8_t velocity_constraint_selection = 0; ///< 0=None (default),
1=Zero-velocity,
2=Wheeled-vehicle.
uint8_t angular_constraint_selection = 0; ///< 0=None (default), 1=Zero-angular rate (ZUPT). @@ -3355,17 +3430,18 @@ struct InitializationConfiguration Vector3f initial_velocity; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) FilterReferenceFrame reference_frame_selector = static_cast(0); ///< User-specified initial position/velocity reference frames - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x81FF; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x81FF; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3386,11 +3462,12 @@ struct InitializationConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t wait_for_run_command = 0; ///< Initialize filter only after receiving "run" command InitialConditionSource initial_cond_src = static_cast(0); ///< Initial condition source: AlignmentSelector auto_heading_alignment_selector; ///< Bitfield specifying the allowed automatic heading alignment methods for automatic initial conditions. Bits are set to 1 to enable, and the correspond to the following:
@@ -3434,17 +3511,18 @@ struct AdaptiveFilterOptions uint8_t level = 0; ///< Auto-adaptive operating level:
0=Off,
1=Conservative,
2=Moderate (default),
3=Aggressive. uint16_t time_limit = 0; ///< Maximum duration of measurement rejection before entering recovery mode (ms) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3465,11 +3543,12 @@ struct AdaptiveFilterOptions struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t level = 0; ///< Auto-adaptive operating level:
0=Off,
1=Conservative,
2=Moderate (default),
3=Aggressive. uint16_t time_limit = 0; ///< Maximum duration of measurement rejection before entering recovery mode (ms) @@ -3509,17 +3588,18 @@ struct MultiAntennaOffset uint8_t receiver_id = 0; ///< Receiver: 1, 2, etc... Vector3f antenna_offset; ///< Antenna lever arm offset vector in the vehicle frame (m) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3541,11 +3621,12 @@ struct MultiAntennaOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t receiver_id = 0; Vector3f antenna_offset; @@ -3583,17 +3664,18 @@ struct RelPosConfiguration FilterReferenceFrame reference_frame_selector = static_cast(0); ///< ECEF or LLH Vector3d reference_coordinates; ///< reference coordinates, units determined by source selection - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3614,11 +3696,12 @@ struct RelPosConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t source = 0; ///< 0 - auto (RTK base station), 1 - manual FilterReferenceFrame reference_frame_selector = static_cast(0); ///< ECEF or LLH Vector3d reference_coordinates; ///< reference coordinates, units determined by source selection @@ -3668,17 +3751,18 @@ struct RefPointLeverArm ReferencePointSelector ref_point_sel = static_cast(0); ///< Reserved, must be 1 Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3699,11 +3783,12 @@ struct RefPointLeverArm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; ReferencePointSelector ref_point_sel = static_cast(0); ///< Reserved, must be 1 Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. @@ -3743,11 +3828,12 @@ struct SpeedMeasurement float speed = 0; ///< Estimated speed along vehicle's x-axis (may be positive or negative) [meters/second] float speed_uncertainty = 0; ///< Estimated uncertainty in the speed measurement (1-sigma value) [meters/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3785,17 +3871,18 @@ struct SpeedLeverArm uint8_t source = 0; ///< Reserved, must be 1. Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3817,11 +3904,12 @@ struct SpeedLeverArm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t source = 0; ///< Reserved, must be 1. Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. @@ -3863,17 +3951,18 @@ struct WheeledVehicleConstraintControl FunctionSelector function = static_cast(0); uint8_t enable = 0; ///< 0 - Disable, 1 - Enable - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3894,11 +3983,12 @@ struct WheeledVehicleConstraintControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable @@ -3937,17 +4027,18 @@ struct VerticalGyroConstraintControl FunctionSelector function = static_cast(0); uint8_t enable = 0; ///< 0 - Disable, 1 - Enable - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3968,11 +4059,12 @@ struct VerticalGyroConstraintControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable @@ -4010,17 +4102,18 @@ struct GnssAntennaCalControl uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float max_offset = 0; ///< Maximum absolute value of lever arm offset error in the vehicle frame [meters]. See device user manual for the valid range of this parameter. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -4041,11 +4134,12 @@ struct GnssAntennaCalControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float max_offset = 0; ///< Maximum absolute value of lever arm offset error in the vehicle frame [meters]. See device user manual for the valid range of this parameter. @@ -4083,11 +4177,12 @@ struct SetInitialHeading { float heading = 0; ///< Initial heading in radians [-pi, pi] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index ee33ec593..d0b7cb4b4 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -45,14 +45,14 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint16_t GNSS_GPS_ENABLE_L1CA = 0x0001; -static const uint16_t GNSS_GPS_ENABLE_L2C = 0x0002; -static const uint16_t GNSS_GLONASS_ENABLE_L1OF = 0x0001; -static const uint16_t GNSS_GLONASS_ENABLE_L2OF = 0x0002; -static const uint16_t GNSS_GALILEO_ENABLE_E1 = 0x0001; -static const uint16_t GNSS_GALILEO_ENABLE_E5B = 0x0002; -static const uint16_t GNSS_BEIDOU_ENABLE_B1 = 0x0001; -static const uint16_t GNSS_BEIDOU_ENABLE_B2 = 0x0002; +static constexpr const uint16_t GNSS_GPS_ENABLE_L1CA = 0x0001; +static constexpr const uint16_t GNSS_GPS_ENABLE_L2C = 0x0002; +static constexpr const uint16_t GNSS_GLONASS_ENABLE_L1OF = 0x0001; +static constexpr const uint16_t GNSS_GLONASS_ENABLE_L2OF = 0x0002; +static constexpr const uint16_t GNSS_GALILEO_ENABLE_E1 = 0x0001; +static constexpr const uint16_t GNSS_GALILEO_ENABLE_E5B = 0x0002; +static constexpr const uint16_t GNSS_BEIDOU_ENABLE_B1 = 0x0001; +static constexpr const uint16_t GNSS_BEIDOU_ENABLE_B2 = 0x0002; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -75,12 +75,13 @@ struct ReceiverInfo }; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -93,11 +94,12 @@ struct ReceiverInfo } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_receivers = 0; ///< Number of physical receivers in the device Info receiver_info[5]; @@ -137,17 +139,18 @@ struct SignalConfiguration uint8_t beidou_enable = 0; ///< Bitfield 0: Enable B1, 1: Enable B2 uint8_t reserved[4] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -168,11 +171,12 @@ struct SignalConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t gps_enable = 0; ///< Bitfield 0: Enable L1CA, 1: Enable L2C uint8_t glonass_enable = 0; ///< Bitfield 0: Enable L1OF, 1: Enable L2OF uint8_t galileo_enable = 0; ///< Bitfield 0: Enable E1, 1: Enable E5B @@ -213,17 +217,18 @@ struct RtkDongleConfiguration uint8_t enable = 0; ///< 0 - Disabled, 1- Enabled uint8_t reserved[3] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -244,11 +249,12 @@ struct RtkDongleConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; uint8_t reserved[3] = {0}; diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index c5f0df4dd..1b7e145a6 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -199,12 +199,13 @@ struct GetStatusFlags }; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -217,11 +218,12 @@ struct GetStatusFlags } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; StatusFlags flags; ///< Model number dependent. See above structures. @@ -249,12 +251,13 @@ CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* struct GetImei { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -267,11 +270,12 @@ struct GetImei } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char IMEI[32] = {0}; @@ -299,12 +303,13 @@ CmdResult getImei(C::mip_interface& device, char* imeiOut); struct GetImsi { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -317,11 +322,12 @@ struct GetImsi } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char IMSI[32] = {0}; @@ -349,12 +355,13 @@ CmdResult getImsi(C::mip_interface& device, char* imsiOut); struct GetIccid { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -367,11 +374,12 @@ struct GetIccid } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char ICCID[32] = {0}; @@ -407,17 +415,18 @@ struct ConnectedDeviceType FunctionSelector function = static_cast(0); Type devType = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -438,11 +447,12 @@ struct ConnectedDeviceType struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Type devType = static_cast(0); @@ -474,12 +484,13 @@ CmdResult defaultConnectedDeviceType(C::mip_interface& device); struct GetActCode { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -492,11 +503,12 @@ struct GetActCode } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char ActivationCode[32] = {0}; @@ -524,12 +536,13 @@ CmdResult getActCode(C::mip_interface& device, char* activationcodeOut); struct GetModemFirmwareVersion { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -542,11 +555,12 @@ struct GetModemFirmwareVersion } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char ModemFirmwareVersion[32] = {0}; @@ -575,12 +589,13 @@ CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwarev struct GetRssi { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -593,11 +608,12 @@ struct GetRssi } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool valid = 0; int32_t rssi = 0; int32_t signalQuality = 0; @@ -661,12 +677,13 @@ struct ServiceStatus uint32_t reserved1 = 0; uint32_t reserved2 = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -679,11 +696,12 @@ struct ServiceStatus } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; ServiceFlags flags; uint32_t receivedBytes = 0; uint32_t lastBytes = 0; @@ -717,11 +735,12 @@ struct ProdEraseStorage { MediaSelector media = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -754,11 +773,12 @@ struct LedControl LedAction act = static_cast(0); uint32_t period = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -788,11 +808,12 @@ CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, cons struct ModemHardReset { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index 537e0d042..dd49bc142 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -46,10 +46,10 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_PASSTHRU = 0x00; -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_NORMAL = 0x01; -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_IMU = 0x02; -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_GPS = 0x03; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_PASSTHRU = 0x00; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_NORMAL = 0x01; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_IMU = 0x02; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_GPS = 0x03; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -74,17 +74,18 @@ struct CommMode FunctionSelector function = static_cast(0); uint8_t mode = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -105,11 +106,12 @@ struct CommMode struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t mode = 0; diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index d55259ec2..bba20ba2d 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -363,8 +363,9 @@ struct PositionLlh double ellipsoid_height = 0; ///< [meters] uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -396,8 +397,9 @@ struct VelocityNed float down = 0; ///< [meters/second] uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -435,8 +437,9 @@ struct AttitudeQuaternion Quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -476,8 +479,9 @@ struct AttitudeDcm Matrix3f dcm; ///< Matrix elements in row-major order. uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -510,8 +514,9 @@ struct EulerAngles float yaw = 0; ///< [radians] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -541,8 +546,9 @@ struct GyroBias Vector3f bias; ///< (x, y, z) [radians/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -572,8 +578,9 @@ struct AccelBias Vector3f bias; ///< (x, y, z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -605,8 +612,9 @@ struct PositionLlhUncertainty float down = 0; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -638,8 +646,9 @@ struct VelocityNedUncertainty float down = 0; ///< [meters/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -672,8 +681,9 @@ struct EulerAnglesUncertainty float yaw = 0; ///< [radians] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -703,8 +713,9 @@ struct GyroBiasUncertainty Vector3f bias_uncert; ///< (x,y,z) [radians/sec] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -734,8 +745,9 @@ struct AccelBiasUncertainty Vector3f bias_uncert; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -772,8 +784,9 @@ struct Timestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -804,8 +817,9 @@ struct Status FilterDynamicsMode dynamics_mode = static_cast(0); ///< Device-specific dynamics mode. Please consult the user manual for definition. FilterStatusFlags status_flags; ///< Device-specific status flags. Please consult the user manual for definition. - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -836,8 +850,9 @@ struct LinearAccel Vector3f accel; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -867,8 +882,9 @@ struct GravityVector Vector3f gravity; ///< (x, y, z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -898,8 +914,9 @@ struct CompAccel Vector3f accel; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -929,8 +946,9 @@ struct CompAngularRate Vector3f gyro; ///< (x, y, z) [radians/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -960,8 +978,9 @@ struct QuaternionAttitudeUncertainty Quatf q; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -991,8 +1010,9 @@ struct Wgs84GravityMag float magnitude = 0; ///< [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1036,8 +1056,9 @@ struct HeadingUpdateState HeadingSource source = static_cast(0); uint16_t valid_flags = 0; ///< 1 if a valid heading update was received in 2 seconds, 0 otherwise. - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1072,8 +1093,9 @@ struct MagneticModel float declination = 0; ///< [radians] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1103,8 +1125,9 @@ struct AccelScaleFactor Vector3f scale_factor; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1134,8 +1157,9 @@ struct AccelScaleFactorUncertainty Vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1165,8 +1189,9 @@ struct GyroScaleFactor Vector3f scale_factor; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1196,8 +1221,9 @@ struct GyroScaleFactorUncertainty Vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1227,8 +1253,9 @@ struct MagBias Vector3f bias; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1258,8 +1285,9 @@ struct MagBiasUncertainty Vector3f bias_uncert; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1295,8 +1323,9 @@ struct StandardAtmosphere float standard_density = 0; ///< [kilogram/meter^3] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1330,8 +1359,9 @@ struct PressureAltitude float pressure_altitude = 0; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1360,8 +1390,9 @@ struct DensityAltitude float density_altitude = 0; ///< m uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1393,8 +1424,9 @@ struct AntennaOffsetCorrection Vector3f offset; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1424,8 +1456,9 @@ struct AntennaOffsetCorrectionUncertainty Vector3f offset_uncert; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1458,8 +1491,9 @@ struct MultiAntennaOffsetCorrection Vector3f offset; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1490,8 +1524,9 @@ struct MultiAntennaOffsetCorrectionUncertainty Vector3f offset_uncert; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1523,8 +1558,9 @@ struct MagnetometerOffset Vector3f hard_iron; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1556,8 +1592,9 @@ struct MagnetometerMatrix Matrix3f soft_iron; ///< Row-major [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1587,8 +1624,9 @@ struct MagnetometerOffsetUncertainty Vector3f hard_iron_uncertainty; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1618,8 +1656,9 @@ struct MagnetometerMatrixUncertainty Matrix3f soft_iron_uncertainty; ///< Row-major [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1648,8 +1687,9 @@ struct MagnetometerCovarianceMatrix Matrix3f covariance; uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1679,8 +1719,9 @@ struct MagnetometerResidualVector Vector3f residual; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1712,8 +1753,9 @@ struct ClockCorrection float bias_drift = 0; ///< [seconds/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1745,8 +1787,9 @@ struct ClockCorrectionUncertainty float bias_drift_uncertainty = 0; ///< [seconds/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1778,8 +1821,9 @@ struct GnssPosAidStatus GnssAidStatusFlags status; ///< Aiding measurement status bitfield uint8_t reserved[8] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1810,8 +1854,9 @@ struct GnssAttAidStatus GnssAidStatusFlags status; ///< Last valid aiding measurement status bitfield uint8_t reserved[8] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1848,8 +1893,9 @@ struct HeadAidStatus HeadingAidType type = static_cast(0); ///< 1 - Dual antenna, 2 - External heading message (user supplied) float reserved[2] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1879,8 +1925,9 @@ struct RelPosNed Vector3d relative_position; ///< [meters, NED] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1910,8 +1957,9 @@ struct EcefPos Vector3d position_ecef; ///< [meters, ECEF] uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1941,8 +1989,9 @@ struct EcefVel Vector3f velocity_ecef; ///< [meters/second, ECEF] uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1972,8 +2021,9 @@ struct EcefPosUncertainty Vector3f pos_uncertainty; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2003,8 +2053,9 @@ struct EcefVelUncertainty Vector3f vel_uncertainty; ///< [meters/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2036,8 +2087,9 @@ struct AidingMeasurementSummary FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) FilterMeasurementIndicator indicator; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2067,8 +2119,9 @@ struct OdometerScaleFactorError float scale_factor_error = 0; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2098,8 +2151,9 @@ struct OdometerScaleFactorErrorUncertainty float scale_factor_error_uncertainty = 0; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2171,8 +2225,9 @@ struct GnssDualAntennaStatus DualAntennaStatusFlags status_flags; uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 9a52a8644..b7cc0c6c9 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -68,11 +68,11 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint8_t MIP_GNSS1_DATA_DESC_SET = 0x91; -static const uint8_t MIP_GNSS2_DATA_DESC_SET = 0x92; -static const uint8_t MIP_GNSS3_DATA_DESC_SET = 0x93; -static const uint8_t MIP_GNSS4_DATA_DESC_SET = 0x94; -static const uint8_t MIP_GNSS5_DATA_DESC_SET = 0x95; +static constexpr const uint8_t MIP_GNSS1_DATA_DESC_SET = 0x91; +static constexpr const uint8_t MIP_GNSS2_DATA_DESC_SET = 0x92; +static constexpr const uint8_t MIP_GNSS3_DATA_DESC_SET = 0x93; +static constexpr const uint8_t MIP_GNSS4_DATA_DESC_SET = 0x94; +static constexpr const uint8_t MIP_GNSS5_DATA_DESC_SET = 0x95; enum class GnssConstellationId : uint8_t { UNKNOWN = 0, ///< @@ -161,8 +161,8 @@ enum class SbasSystem : uint8_t GAGAN = 4, ///< }; -static const uint32_t GNSS_DGPS_INFO_MAX_CHANNEL_NUMBER = 32; -static const uint32_t GNSS_SV_INFO_MAX_SV_NUMBER = 32; +static constexpr const uint32_t GNSS_DGPS_INFO_MAX_CHANNEL_NUMBER = 32; +static constexpr const uint32_t GNSS_SV_INFO_MAX_SV_NUMBER = 32; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -224,8 +224,9 @@ struct PosLlh float vertical_accuracy = 0; ///< [meters] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -287,8 +288,9 @@ struct PosEcef float x_accuracy = 0; ///< [meters] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -366,8 +368,9 @@ struct VelNed float heading_accuracy = 0; ///< [degrees] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -429,8 +432,9 @@ struct VelEcef float v_accuracy = 0; ///< [meters/second] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -512,8 +516,9 @@ struct Dop float edop = 0; ///< Easting DOP ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -580,8 +585,9 @@ struct UtcTime uint32_t msec = 0; ///< [Milliseconds] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -643,8 +649,9 @@ struct GpsTime uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -710,8 +717,9 @@ struct ClockInfo double accuracy_estimate = 0; ///< [seconds] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -816,8 +824,9 @@ struct FixInfo FixFlags fix_flags; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -925,8 +934,9 @@ struct SvInfo SVFlags sv_flags; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1015,8 +1025,9 @@ struct HwStatus AntennaPower antenna_power = static_cast(0); ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1098,8 +1109,9 @@ struct DgpsInfo float range_rate_correction = 0; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1171,8 +1183,9 @@ struct DgpsChannel float range_rate_correction = 0; ///< [m/s] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1244,8 +1257,9 @@ struct ClockInfo2 double drift_accuracy_estimate = 0; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1300,8 +1314,9 @@ struct GpsLeapSeconds uint8_t leap_seconds = 0; ///< [s] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1413,8 +1428,9 @@ struct SbasInfo SbasStatus sbas_status; ///< Status of the SBAS service ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1508,8 +1524,9 @@ struct SbasCorrection float iono_correction = 0; ///< Ionospheric correction [meters]. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1600,8 +1617,9 @@ struct RfErrorDetection uint8_t reserved[4] = {0}; ///< Reserved for future use ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1730,8 +1748,9 @@ struct BaseStationInfo IndicatorFlags indicators; ///< Bitfield ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1866,8 +1885,9 @@ struct RtkCorrectionsStatus uint32_t reserved[4] = {0}; ///< Reserved for future use ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1951,8 +1971,9 @@ struct SatelliteStatus bool health = 0; ///< True if the satellite is healthy. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2082,8 +2103,9 @@ struct Raw float lock_time = 0; ///< DOC Minimum carrier phase lock time [s]. Note: the maximum value is dependent on the receiver. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2176,8 +2198,9 @@ struct GpsEphemeris double c_rs = 0; ///< Harmonic Correction Term. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2270,8 +2293,9 @@ struct GalileoEphemeris double c_rs = 0; ///< Harmonic Correction Term. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2352,8 +2376,9 @@ struct GloEphemeris uint8_t P4 = 0; ///< Flag indicating ephemeris parameters are present ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2423,8 +2448,9 @@ struct GpsIonoCorr double beta[4] = {0}; ///< Ionospheric Correction Terms. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2494,8 +2520,9 @@ struct GalileoIonoCorr uint8_t disturbance_flags = 0; ///< Region disturbance flags (bits 1-5). ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 5e3142f72..be97a142e 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -81,8 +81,9 @@ struct RawAccel { Vector3f raw_accel; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -112,8 +113,9 @@ struct RawGyro { Vector3f raw_gyro; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -143,8 +145,9 @@ struct RawMag { Vector3f raw_mag; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -174,8 +177,9 @@ struct RawPressure { float raw_pressure = 0; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -205,8 +209,9 @@ struct ScaledAccel { Vector3f scaled_accel; ///< (x, y, z)[g] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -236,8 +241,9 @@ struct ScaledGyro { Vector3f scaled_gyro; ///< (x, y, z) [radians/second] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -267,8 +273,9 @@ struct ScaledMag { Vector3f scaled_mag; ///< (x, y, z) [Gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -297,8 +304,9 @@ struct ScaledPressure { float scaled_pressure = 0; ///< [mBar] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -328,8 +336,9 @@ struct DeltaTheta { Vector3f delta_theta; ///< (x, y, z) [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -359,8 +368,9 @@ struct DeltaVelocity { Vector3f delta_velocity; ///< (x, y, z) [g*sec] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -399,8 +409,9 @@ struct CompOrientationMatrix { Matrix3f m; ///< Matrix elements in row-major order. - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -437,8 +448,9 @@ struct CompQuaternion { Quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -470,8 +482,9 @@ struct CompEulerAngles float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -500,8 +513,9 @@ struct CompOrientationUpdateMatrix { Matrix3f m; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -530,8 +544,9 @@ struct OrientationRawTemp { uint16_t raw_temp[4] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -560,8 +575,9 @@ struct InternalTimestamp { uint32_t counts = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -591,8 +607,9 @@ struct PpsTimestamp uint32_t seconds = 0; uint32_t useconds = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -666,8 +683,9 @@ struct GpsTimestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -702,8 +720,9 @@ struct TemperatureAbs float max_temp = 0; ///< [degC] float mean_temp = 0; ///< [degC] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -738,8 +757,9 @@ struct UpVector { Vector3f up; ///< [Gs] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -771,8 +791,9 @@ struct NorthVector { Vector3f north; ///< [Gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -852,8 +873,9 @@ struct OverrangeStatus Status status; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -883,8 +905,9 @@ struct OdometerData float uncertainty = 0; ///< Uncertainty of velocity [m/s]. uint16_t valid_flags = 0; ///< If odometer is configured, bit 0 will be set to 1. - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 0f26f11fb..be95ce3fa 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -49,7 +49,7 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint8_t MIP_DATA_DESC_SHARED_START = 0xD0; +static constexpr const uint8_t MIP_DATA_DESC_SHARED_START = 0xD0; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -68,8 +68,9 @@ struct EventSource { uint8_t trigger_id = 0; ///< Trigger ID number. If 0, this message was emitted due to being scheduled in the 3DM Message Format Command (0x0C,0x0F). - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -101,8 +102,9 @@ struct Ticks { uint32_t ticks = 0; ///< Ticks since powerup. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -135,8 +137,9 @@ struct DeltaTicks { uint32_t ticks = 0; ///< Ticks since last output. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -201,8 +204,9 @@ struct GpsTimestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -240,8 +244,9 @@ struct DeltaTime { double seconds = 0; ///< Seconds since last output. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -277,8 +282,9 @@ struct ReferenceTimestamp { uint64_t nanoseconds = 0; ///< Nanoseconds since initialization. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -316,8 +322,9 @@ struct ReferenceTimeDelta { uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -380,8 +387,9 @@ struct ExternalTimestamp uint64_t nanoseconds = 0; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -448,8 +456,9 @@ struct ExternalTimeDelta uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 65982acab..80b457773 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -76,8 +76,9 @@ struct BuiltInTest { uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -107,8 +108,9 @@ struct TimeSyncStatus bool time_sync = 0; ///< True if sync with the PPS signal is currently valid. False if PPS feature is disabled or a PPS signal is not detected. uint8_t last_pps_rcvd = 0; ///< Elapsed time in seconds since last PPS was received, with a maximum value of 255. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -155,8 +157,9 @@ struct GpioState { uint8_t states = 0; ///< Bitfield containing the states for each GPIO pin.
Bit 0 (0x01): pin 1
Bit 1 (0x02): pin 2
Bit 2 (0x04): pin 3
Bit 3 (0x08): pin 4
Bits for pins that don't exist will read as 0. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -187,8 +190,9 @@ struct GpioAnalogValue uint8_t gpio_id = 0; ///< GPIO pin number starting with 1. float value = 0; ///< Value of the GPIO line in scaled volts. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const From eda963468373dc5603ba4b8b7b69134c1f8d6d3c Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 25 Aug 2023 18:39:27 -0400 Subject: [PATCH 119/252] Make CompositeDescriptor::operator= non-constexpr. --- src/mip/definitions/descriptors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index 1b26a2978..f1dfc8197 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -71,7 +71,7 @@ struct CompositeDescriptor constexpr CompositeDescriptor(uint8_t descSet, uint8_t fieldDesc) : descriptorSet(descSet), fieldDescriptor(fieldDesc) {} constexpr CompositeDescriptor(uint16_t combo) : descriptorSet(combo >> 8), fieldDescriptor(combo & 0xFF) {} - constexpr CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } + CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } constexpr uint16_t as_u16() const { return (uint16_t(descriptorSet) << 8) | fieldDescriptor; } From f613bcba2a62ff99a00d96b7e2a5af2cfafd418d Mon Sep 17 00:00:00 2001 From: msclissa Date: Thu, 31 Aug 2023 14:47:08 -0400 Subject: [PATCH 120/252] add Connection accessors for interface name and parameter --- src/mip/mip_device.hpp | 3 +++ src/mip/platform/serial_connection.hpp | 6 ++++-- src/mip/platform/tcp_connection.hpp | 4 ++++ 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 8eec41cce..2034142c3 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -161,6 +161,9 @@ class Connection const char* type() const { return mType; }; + virtual const char* interfaceName() const = 0; + virtual uint32_t parameter() const = 0; + protected: const char *mType; diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index da281b176..ef8bae4fd 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -34,8 +34,6 @@ class SerialConnection : public mip::Connection bool connect(); bool disconnect(); - - void connectionInfo(std::string &name, uint32_t &baudrate) const { name = mPortName; @@ -46,6 +44,10 @@ class SerialConnection : public mip::Connection serial_port mPort; std::string mPortName; uint32_t mBaudrate; + +public: + const char* interfaceName() const override { return mPortName.c_str(); } + uint32_t parameter() const override { return mBaudrate; } }; diff --git a/src/mip/platform/tcp_connection.hpp b/src/mip/platform/tcp_connection.hpp index e8a506269..a33bc9558 100644 --- a/src/mip/platform/tcp_connection.hpp +++ b/src/mip/platform/tcp_connection.hpp @@ -44,6 +44,10 @@ class TcpConnection : public mip::Connection tcp_socket mSocket; std::string mHostname; uint16_t mPort = 0; + +public: + const char* interfaceName() const override { return mHostname.c_str(); } + uint32_t parameter() const override { return mPort; } }; ///@} From 4b33df791fb32d8e8d84b62c96b38811af89f269 Mon Sep 17 00:00:00 2001 From: msclissa Date: Tue, 5 Sep 2023 18:00:30 -0400 Subject: [PATCH 121/252] add interfaceName, parameter functions to RecordingConnection --- src/mip/extras/recording_connection.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/mip/extras/recording_connection.hpp b/src/mip/extras/recording_connection.hpp index 8c83c1493..4297d7651 100644 --- a/src/mip/extras/recording_connection.hpp +++ b/src/mip/extras/recording_connection.hpp @@ -49,6 +49,9 @@ class RecordingConnection : public Connection return false; }; + const char* interfaceName() const override { return mConnection->interfaceName(); } + uint32_t parameter() const override { return mConnection->parameter(); } + uint64_t recvFileBytesWritten() { return mRecvFileWritten; From 349533d61259a0d1614875f23e5420a1a639a6de Mon Sep 17 00:00:00 2001 From: "Ian J. Moore" <46928068+ianjmoore@users.noreply.github.com> Date: Sat, 9 Sep 2023 16:24:03 -0400 Subject: [PATCH 122/252] Update CX5_15_example.c -> remove filter reset command as unnecessary --- examples/CX5_15/CX5_15_example.c | 9 --------- 1 file changed, 9 deletions(-) diff --git a/examples/CX5_15/CX5_15_example.c b/examples/CX5_15/CX5_15_example.c index a0f788e65..899e5630f 100644 --- a/examples/CX5_15/CX5_15_example.c +++ b/examples/CX5_15/CX5_15_example.c @@ -224,15 +224,6 @@ int main(int argc, const char* argv[]) if(mip_filter_write_auto_init_control(&device, 1) != MIP_ACK_OK) exit_gracefully("ERROR: Could not set filter autoinit control!"); - - // - //Reset the filter (note: this is good to do after filter setup is complete) - // - - if(mip_filter_reset(&device) != MIP_ACK_OK) - exit_gracefully("ERROR: Could not reset the filter!"); - - // // Register data callbacks // From 9dbbce49e6a45c2f9eb7a084b5a63b4604d52312 Mon Sep 17 00:00:00 2001 From: Nick DaCosta Date: Tue, 12 Sep 2023 10:05:37 -0400 Subject: [PATCH 123/252] Added Vector size function --- src/mip/definitions/common.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 1f70357e5..f8a55e958 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -157,6 +157,9 @@ struct Vector template void copyFrom(const U* ptr, size_t n) { if(n>N) n=N; for(size_t i=0; i Date: Thu, 21 Sep 2023 11:47:40 -0400 Subject: [PATCH 124/252] Added new simple CV7 example --- examples/CMakeLists.txt | 4 + examples/CV7/CV7_INS_simple_example.cpp | 345 ++++++++++++++++++++++++ src/mip/mip_all.hpp | 1 + 3 files changed, 350 insertions(+) create mode 100644 examples/CV7/CV7_INS_simple_example.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 0e38cf416..8a407b761 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -47,6 +47,10 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(CV7_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_Example PUBLIC "${MIP_EXAMPLE_DEFS}") + add_executable(CV7_INS_Simple_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7/CV7_INS_simple_example.cpp" ${DEVICE_SOURCES}) + target_link_libraries(CV7_INS_Simple_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") + target_compile_definitions(CV7_INS_Simple_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + add_executable(GX5_45_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GX5_45/GX5_45_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") diff --git a/examples/CV7/CV7_INS_simple_example.cpp b/examples/CV7/CV7_INS_simple_example.cpp new file mode 100644 index 000000000..d25a876b2 --- /dev/null +++ b/examples/CV7/CV7_INS_simple_example.cpp @@ -0,0 +1,345 @@ + +///////////////////////////////////////////////////////////////////////////// +// +// CV7_Example.cpp +// +// C++ Example set-up program for the CV7 +// +// This example shows a typical setup for the CV7 sensor using C++. +// It is not an exhaustive example of all CV7 settings. +// If your specific setup needs are not met by this example, please consult +// the MSCL-embedded API documentation for the proper commands. +// +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include "../example_utils.hpp" + +using namespace mip; + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + +data_shared::GpsTimestamp filter_gps_time; +data_filter::Status filter_status; +data_filter::EulerAngles filter_euler_angles; +data_filter::PositionLlh filter_llh_position; +data_filter::VelocityNed filter_ned_velocity; + +uint8_t external_heading_sensor_id = 0; +uint8_t gnss_antenna_sensor_id = 1; +uint8_t vehicle_frame_velocity_sensor_id = 2; + +bool filter_state_full_nav = false; + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0); + +void print_device_information(const commands_base::BaseDeviceInfo& device_info); + +void exit_gracefully(const char *message); +bool should_exit(); + +//////////////////////////////////////////////////////////////////////////////// +// Main Function +//////////////////////////////////////////////////////////////////////////////// + + +int main(int argc, const char* argv[]) +{ + + std::unique_ptr utils = handleCommonArgs(argc, argv); + std::unique_ptr& device = utils->device; + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(commands_base::ping(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + // + //Read device information + // + + commands_base::BaseDeviceInfo device_info; + if(commands_base::getDeviceInfo(*device, &device_info) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Failed to get device info"); + print_device_information(device_info); + + + // + //Idle the device (note: this is good to do during setup) + // + + if(commands_base::setIdle(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + +// if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) +// exit_gracefully("ERROR: Could not load default device settings!"); + + + // + //Setup external aiding sensor reference frames + // + + + // + //External heading sensor reference frame. + // + float external_heading_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // External heading sensor is aligned with vehicle frame + float external_heading_sensor_to_vehicle_frame_translation[3] = {0.0, 0.0, 0.0}; // Heading measurements are agnostic to translation, translation set to zero + if(commands_aiding::writeReferenceFrame(*device, external_heading_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, + external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Unable to configure external heading sensor frame ID"); + + + // + //External GNSS antenna reference frame + // + float external_gnss_antenna_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero + float external_gnss_antenna_to_vehicle_frame_translation[3] = {0.0, 1.0, 0.0}; // Antenna is translated 1 meter in vehicle frame Y direction + if(commands_aiding::writeReferenceFrame(*device, gnss_antenna_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, + external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); + + + // + //External bodyframe velocity reference frame + // + float external_velocity_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 1.57, 0.0}; // Rotated 90 deg around yaw axis + float external_velocity_sensor_to_vehicle_frame_translation[3] = {1.0, 0.0, 0.0}; // Sensor is translated 1 meter in X direction + if(commands_aiding::writeReferenceFrame(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, + external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Unable to configure external vehicle frame velocity sensor ID"); + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + + if(commands_3dm::getBaseRate(*device, data_filter::DESCRIPTOR_SET, &filter_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + std::array filter_descriptors = {{ + { data_shared::DATA_GPS_TIME, filter_decimation }, + { data_filter::DATA_FILTER_STATUS, filter_decimation }, + { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, + { data_filter::DATA_POS_LLH, filter_decimation }, + { data_filter::DATA_VEL_NED, filter_decimation }, + }}; + + if(commands_3dm::writeMessageFormat(*device, data_filter::DESCRIPTOR_SET, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(commands_filter::reset(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Filter Data + DispatchHandler filter_data_handlers[5]; + + device->registerExtractor(filter_data_handlers[0], &filter_gps_time, data_filter::DESCRIPTOR_SET); + device->registerExtractor(filter_data_handlers[1], &filter_status); + device->registerExtractor(filter_data_handlers[2], &filter_euler_angles); + device->registerExtractor(filter_data_handlers[3], &filter_llh_position); + device->registerExtractor(filter_data_handlers[4], &filter_ned_velocity); + + // + //Resume the device + // + + if(commands_base::resume(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + + // + //Main Loop: Update the interface and process data + // + + bool running = true; + mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); + mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); + + printf("Sensor is configured... waiting for filter to initialize\n"); + + while(running) + { + device->update(); + + //Check for full nav filter state transition + if((!filter_state_full_nav) && (filter_status.filter_state == data_filter::FilterMode::FULL_NAV)) + { + printf("NOTE: Filter has entered full navigation mode.\n"); + filter_state_full_nav = true; + } + + // Check that enough time has elapsed to send a new measurement update + mip::Timestamp current_timestamp = getCurrentTimestamp(); + mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; + + if (elapsed_time_from_last_measurement_update > 500) + { + // Use measurement time of arrival for timestamping method + commands_aiding::Time external_measurement_time; + external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; + + // External heading command + float external_heading = 0; + float external_heading_uncertainty = .001; + if(commands_aiding::trueHeading(*device, external_measurement_time, external_heading_sensor_id, external_heading, external_heading_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external heading to CV7-INS\n"); + + // External position command + double latitude = 44.43729093897896; // Lat/Lon for MicroStrain headquarters + double longitude = -73.10628129871753; + double height = 0; + float llh_uncertainty[3] = {1.0, 1.0, 1.0}; + if(commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, latitude, longitude, height, llh_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external position to CV7-INS\n"); + + // External global velocity command + float ned_velocity[3] = {0.0, 0.0, 0.0}; + float ned_velocity_uncertainty[3] = {0.1, 0.1, 0.1}; + if(commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id, ned_velocity, ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); + + // External vehicle frame velocity command + float vehicle_frame_velocity[3] = {0.0, 0.0, 0.0}; + float vehicle_frame_velocity_uncertainty[3] = {0.1, 0.1, 0.1}; + if(commands_aiding::vehicleFixedFrameVelocity(*device, external_measurement_time, vehicle_frame_velocity_sensor_id, vehicle_frame_velocity, vehicle_frame_velocity_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external vehicle frame velocity to CV7-INS\n"); + + prev_measurement_update_timestamp = current_timestamp; + } + + //Once in full nav, print out data at 10 Hz + if(filter_status.filter_state == data_filter::FilterMode::FULL_NAV) + { + if(current_timestamp - prev_print_timestamp >= 1000) + { + printf("\n\n****Filter navigation state****\n"); + printf("TIMESTAMP: %f\n", filter_gps_time.tow); + printf("ATTITUDE_EULER = [%f %f %f]\n", filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw); + printf("LLH_POSITION = [%f %f %f]\n", filter_llh_position.latitude, filter_llh_position.longitude, filter_llh_position.ellipsoid_height); + printf("NED_VELOCITY = [%f %f %f]\n", filter_ned_velocity.north, filter_ned_velocity.east, filter_ned_velocity.down); + + prev_print_timestamp = current_timestamp; + } + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print device information +//////////////////////////////////////////////////////////////////////////////// + +void print_device_information(const commands_base::BaseDeviceInfo& device_info) +{ + printf("Connected to:\n"); + + auto print_info = [](const char* name, const char info[16]) + { + char msg[17] = {0}; + std::strncpy(msg, info, 16); + printf(" %s%s\n", name, msg); + }; + + print_info("Model name: ", device_info.model_name); + print_info("Model number: ", device_info.model_number); + print_info("Serial Number: ", device_info.serial_number); + print_info("Device Options: ", device_info.device_options); + print_info("Lot Number: ", device_info.lot_number); + + printf( " Firmware version: %d.%d.%d\n\n", + (device_info.firmware_version / 1000), + (device_info.firmware_version / 100) % 10, + (device_info.firmware_version / 1) % 100 + ); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; + +} + diff --git a/src/mip/mip_all.hpp b/src/mip/mip_all.hpp index f24221576..da6d5476b 100644 --- a/src/mip/mip_all.hpp +++ b/src/mip/mip_all.hpp @@ -20,6 +20,7 @@ #include "definitions/commands_gnss.hpp" #include "definitions/commands_rtk.hpp" #include "definitions/commands_system.hpp" +#include "definitions/commands_aiding.hpp" //MIP Data #include "definitions/data_shared.hpp" From 1586ed74aa8d0718a44a7b05376039fcd1012aba Mon Sep 17 00:00:00 2001 From: David Robbins Date: Thu, 21 Sep 2023 12:27:17 -0400 Subject: [PATCH 125/252] Cleaned up CV7 INS example code --- examples/CV7/CV7_INS_simple_example.cpp | 41 +++++++++++-------------- 1 file changed, 18 insertions(+), 23 deletions(-) diff --git a/examples/CV7/CV7_INS_simple_example.cpp b/examples/CV7/CV7_INS_simple_example.cpp index d25a876b2..7d119b056 100644 --- a/examples/CV7/CV7_INS_simple_example.cpp +++ b/examples/CV7/CV7_INS_simple_example.cpp @@ -1,11 +1,10 @@ - ///////////////////////////////////////////////////////////////////////////// // -// CV7_Example.cpp +// CV7_INS_simple_example.cpp // -// C++ Example set-up program for the CV7 +// C++ Example usage program for the CV7-INS // -// This example shows a typical setup for the CV7 sensor using C++. +// This example shows a the basic setup for a CV-INS sensor using external aiding measurements. // It is not an exhaustive example of all CV7 settings. // If your specific setup needs are not met by this example, please consult // the MSCL-embedded API documentation for the proper commands. @@ -101,8 +100,8 @@ int main(int argc, const char* argv[]) //Load the device default settings (so the device is in a known state) // -// if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) -// exit_gracefully("ERROR: Could not load default device settings!"); + if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); // @@ -164,7 +163,6 @@ int main(int argc, const char* argv[]) exit_gracefully("ERROR: Could not set filter message format!"); - // //Reset the filter (note: this is good to do after filter setup is complete) // @@ -202,7 +200,7 @@ int main(int argc, const char* argv[]) mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); - printf("Sensor is configured... waiting for filter to initialize\n"); + printf("Sensor is configured... waiting for filter to initialize...\n"); while(running) { @@ -218,6 +216,7 @@ int main(int argc, const char* argv[]) // Check that enough time has elapsed to send a new measurement update mip::Timestamp current_timestamp = getCurrentTimestamp(); mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; + mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; if (elapsed_time_from_last_measurement_update > 500) { @@ -226,7 +225,7 @@ int main(int argc, const char* argv[]) external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; // External heading command - float external_heading = 0; + float external_heading = 0.0; float external_heading_uncertainty = .001; if(commands_aiding::trueHeading(*device, external_measurement_time, external_heading_sensor_id, external_heading, external_heading_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external heading to CV7-INS\n"); @@ -234,7 +233,7 @@ int main(int argc, const char* argv[]) // External position command double latitude = 44.43729093897896; // Lat/Lon for MicroStrain headquarters double longitude = -73.10628129871753; - double height = 0; + double height = 0.0; float llh_uncertainty[3] = {1.0, 1.0, 1.0}; if(commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, latitude, longitude, height, llh_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); @@ -254,19 +253,16 @@ int main(int argc, const char* argv[]) prev_measurement_update_timestamp = current_timestamp; } - //Once in full nav, print out data at 10 Hz - if(filter_status.filter_state == data_filter::FilterMode::FULL_NAV) + //Once in full nav, print out data at 1 Hz + if((filter_status.filter_state == data_filter::FilterMode::FULL_NAV) && (elapsed_time_from_last_message_print >= 1000)) { - if(current_timestamp - prev_print_timestamp >= 1000) - { - printf("\n\n****Filter navigation state****\n"); - printf("TIMESTAMP: %f\n", filter_gps_time.tow); - printf("ATTITUDE_EULER = [%f %f %f]\n", filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw); - printf("LLH_POSITION = [%f %f %f]\n", filter_llh_position.latitude, filter_llh_position.longitude, filter_llh_position.ellipsoid_height); - printf("NED_VELOCITY = [%f %f %f]\n", filter_ned_velocity.north, filter_ned_velocity.east, filter_ned_velocity.down); - - prev_print_timestamp = current_timestamp; - } + printf("\n\n****Filter navigation state****\n"); + printf("TIMESTAMP: %f\n", filter_gps_time.tow); + printf("ATTITUDE_EULER = [%f %f %f]\n", filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw); + printf("LLH_POSITION = [%f %f %f]\n", filter_llh_position.latitude, filter_llh_position.longitude, filter_llh_position.ellipsoid_height); + printf("NED_VELOCITY = [%f %f %f]\n", filter_ned_velocity.north, filter_ned_velocity.east, filter_ned_velocity.down); + + prev_print_timestamp = current_timestamp; } running = !should_exit(); @@ -340,6 +336,5 @@ void exit_gracefully(const char *message) bool should_exit() { return false; - } From 49748a751439396831f518b4678822b3700f18e0 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Thu, 21 Sep 2023 12:29:00 -0400 Subject: [PATCH 126/252] Moved CV7 INS example location --- examples/CMakeLists.txt | 2 +- examples/{CV7 => CV7_INS}/CV7_INS_simple_example.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) rename examples/{CV7 => CV7_INS}/CV7_INS_simple_example.cpp (99%) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8a407b761..f3ceb1a69 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -47,7 +47,7 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(CV7_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_Example PUBLIC "${MIP_EXAMPLE_DEFS}") - add_executable(CV7_INS_Simple_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7/CV7_INS_simple_example.cpp" ${DEVICE_SOURCES}) + add_executable(CV7_INS_Simple_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(CV7_INS_Simple_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") diff --git a/examples/CV7/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp similarity index 99% rename from examples/CV7/CV7_INS_simple_example.cpp rename to examples/CV7_INS/CV7_INS_simple_example.cpp index 7d119b056..10b6e2ebe 100644 --- a/examples/CV7/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -26,8 +26,7 @@ // Include Files //////////////////////////////////////////////////////////////////////////////// -#include -#include +#include "mip/mip_all.hpp" #include #include "../example_utils.hpp" From a03f48cf5791a73fa27b7e6a68eccf4dfd570c7e Mon Sep 17 00:00:00 2001 From: David Robbins Date: Fri, 22 Sep 2023 10:43:03 -0400 Subject: [PATCH 127/252] Changed sensor IDs for external frames --- examples/CV7_INS/CV7_INS_simple_example.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 10b6e2ebe..987cf646f 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -42,9 +42,9 @@ data_filter::EulerAngles filter_euler_angles; data_filter::PositionLlh filter_llh_position; data_filter::VelocityNed filter_ned_velocity; -uint8_t external_heading_sensor_id = 0; -uint8_t gnss_antenna_sensor_id = 1; -uint8_t vehicle_frame_velocity_sensor_id = 2; +uint8_t external_heading_sensor_id = 1; +uint8_t gnss_antenna_sensor_id = 2; +uint8_t vehicle_frame_velocity_sensor_id = 3; bool filter_state_full_nav = false; From 709fdde4c84f9dbc776c77badecdf03f253bca08 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 25 Sep 2023 13:10:31 -0400 Subject: [PATCH 128/252] Fix PacketBuf copy constructor. --- src/mip/mip.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index dac63581c..ab6c18711 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -320,10 +320,16 @@ class SizedPacketBuf : public PacketRef explicit SizedPacketBuf(const uint8_t* data, size_t length) : PacketRef(mData, sizeof(mData)) { copyFrom(data, length); } explicit SizedPacketBuf(const PacketRef& packet) : PacketRef(mData, sizeof(mData)) { copyFrom(packet); } + ///@brief Copy constructor + SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); } + ///@brief Copy constructor (required to put packets into std::vector in some cases). template explicit SizedPacketBuf(const SizedPacketBuf& other) : PacketRef(mData, sizeof(mData)) { copyFrom(other); }; + ///@brief Copy assignment operator + SizedPacketBuf& operator=(const SizedPacketBuf& other) { copyFrom(other); return *this; } + ///@brief Assignment operator, copies data from another buffer to this one. template SizedPacketBuf& operator=(const SizedPacketBuf& other) { copyFrom(other); return *this; } From 1f4677594123f0a3fc6cdccc6788b20265fcd7ac Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 25 Sep 2023 16:24:37 -0400 Subject: [PATCH 129/252] Generate MIP definitions from branches/dev@55408. --- src/mip/definitions/commands_aiding.c | 17 ++++++++++++----- src/mip/definitions/commands_aiding.cpp | 17 ++++++++++++----- src/mip/definitions/commands_aiding.h | 2 +- src/mip/definitions/commands_aiding.hpp | 8 ++++---- 4 files changed, 29 insertions(+), 15 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index b4333e05b..8ac655f16 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -184,10 +184,13 @@ void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const insert_u8(serializer, self->frame_id); - if( self->function == MIP_FUNCTION_WRITE ) + if( self->function == MIP_FUNCTION_WRITE || self->function == MIP_FUNCTION_READ ) { insert_mip_aiding_reference_frame_command_format(serializer, self->format); + } + if( self->function == MIP_FUNCTION_WRITE ) + { for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); @@ -202,10 +205,13 @@ void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_ extract_u8(serializer, &self->frame_id); - if( self->function == MIP_FUNCTION_WRITE ) + if( self->function == MIP_FUNCTION_WRITE || self->function == MIP_FUNCTION_READ ) { extract_mip_aiding_reference_frame_command_format(serializer, &self->format); + } + if( self->function == MIP_FUNCTION_WRITE ) + { for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); @@ -277,7 +283,7 @@ mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, ui return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out) +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, float* rotation_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -287,6 +293,8 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin insert_u8(&serializer, frame_id); + insert_mip_aiding_reference_frame_command_format(&serializer, format); + assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); @@ -299,8 +307,7 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin extract_u8(&deserializer, &frame_id); - assert(format_out); - extract_mip_aiding_reference_frame_command_format(&deserializer, format_out); + extract_mip_aiding_reference_frame_command_format(&deserializer, &format); assert(translation_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 1c08df6e2..2d201d4cf 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -164,10 +164,13 @@ void insert(Serializer& serializer, const ReferenceFrame& self) insert(serializer, self.frame_id); - if( self.function == FunctionSelector::WRITE ) + if( self.function == FunctionSelector::WRITE || self.function == FunctionSelector::READ ) { insert(serializer, self.format); + } + if( self.function == FunctionSelector::WRITE ) + { for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); @@ -182,10 +185,13 @@ void extract(Serializer& serializer, ReferenceFrame& self) extract(serializer, self.frame_id); - if( self.function == FunctionSelector::WRITE ) + if( self.function == FunctionSelector::WRITE || self.function == FunctionSelector::READ ) { extract(serializer, self.format); + } + if( self.function == FunctionSelector::WRITE ) + { for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); @@ -244,7 +250,7 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut) +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, float* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -252,6 +258,8 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc insert(serializer, FunctionSelector::READ); insert(serializer, frameId); + insert(serializer, format); + assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); @@ -263,8 +271,7 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc extract(deserializer, frameId); - assert(formatOut); - extract(deserializer, *formatOut); + extract(deserializer, format); assert(translationOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index b8ac265d9..ff6eafd32 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -156,7 +156,7 @@ void insert_mip_aiding_reference_frame_response(struct mip_serializer* serialize void extract_mip_aiding_reference_frame_response(struct mip_serializer* serializer, mip_aiding_reference_frame_response* self); mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation); -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out); +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, float* rotation_out); mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id); diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index e7cf75c2c..d50d2d2f3 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -179,11 +179,11 @@ struct ReferenceFrame static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; - static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8003; static constexpr const uint32_t SAVE_PARAMS = 0x8001; static constexpr const uint32_t LOAD_PARAMS = 0x8001; static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; - static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const @@ -210,7 +210,7 @@ struct ReferenceFrame static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. @@ -231,7 +231,7 @@ void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation); -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut); +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, float* rotationOut); CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); From 0b89d7be0e1001df2badcfea95418ff3670ed8b2 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 25 Sep 2023 17:18:52 -0400 Subject: [PATCH 130/252] Generate MIP definitions from branches/dev@55410. --- src/mip/definitions/commands_aiding.c | 84 ++++++++++++++++++------- src/mip/definitions/commands_aiding.cpp | 84 ++++++++++++++++++------- src/mip/definitions/commands_aiding.h | 15 +++-- src/mip/definitions/commands_aiding.hpp | 15 +++-- 4 files changed, 146 insertions(+), 52 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 8ac655f16..fd6c965a8 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -194,9 +194,16 @@ void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); - for(unsigned int i=0; i < 4; i++) - insert_float(serializer, self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + insert_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + insert_mip_quatf(serializer, &self->rotation.quaternion); + + } } } void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_aiding_reference_frame_command* self) @@ -215,9 +222,16 @@ void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_ for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); - for(unsigned int i=0; i < 4; i++) - extract_float(serializer, &self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + extract_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + extract_mip_quatf(serializer, &self->rotation.quaternion); + + } } } @@ -230,9 +244,16 @@ void insert_mip_aiding_reference_frame_response(mip_serializer* serializer, cons for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); - for(unsigned int i=0; i < 4; i++) - insert_float(serializer, self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + insert_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + insert_mip_quatf(serializer, &self->rotation.quaternion); + + } } void extract_mip_aiding_reference_frame_response(mip_serializer* serializer, mip_aiding_reference_frame_response* self) { @@ -243,9 +264,16 @@ void extract_mip_aiding_reference_frame_response(mip_serializer* serializer, mip for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); - for(unsigned int i=0; i < 4; i++) - extract_float(serializer, &self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + extract_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + extract_mip_quatf(serializer, &self->rotation.quaternion); + + } } void insert_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, const mip_aiding_reference_frame_command_format self) @@ -259,7 +287,7 @@ void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* se *self = tmp; } -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation) +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const mip_aiding_reference_frame_command_rotation* rotation) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -275,15 +303,21 @@ mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, ui for(unsigned int i=0; i < 3; i++) insert_float(&serializer, translation[i]); - assert(rotation || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - insert_float(&serializer, rotation[i]); - + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + insert_mip_vector3f(&serializer, &rotation->euler); + + } + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + insert_mip_quatf(&serializer, &rotation->quaternion); + + } assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, float* rotation_out) +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, mip_aiding_reference_frame_command_rotation* rotation_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -313,10 +347,16 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin for(unsigned int i=0; i < 3; i++) extract_float(&deserializer, &translation_out[i]); - assert(rotation_out || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - extract_float(&deserializer, &rotation_out[i]); - + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + extract_mip_vector3f(&deserializer, &rotation_out->euler); + + } + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + extract_mip_quatf(&deserializer, &rotation_out->quaternion); + + } if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 2d201d4cf..5761de4dc 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -174,9 +174,16 @@ void insert(Serializer& serializer, const ReferenceFrame& self) for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - insert(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + insert(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + insert(serializer, self.rotation.quaternion); + + } } } void extract(Serializer& serializer, ReferenceFrame& self) @@ -195,9 +202,16 @@ void extract(Serializer& serializer, ReferenceFrame& self) for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - extract(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + extract(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + extract(serializer, self.rotation.quaternion); + + } } } @@ -210,9 +224,16 @@ void insert(Serializer& serializer, const ReferenceFrame::Response& self) for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - insert(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + insert(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + insert(serializer, self.rotation.quaternion); + + } } void extract(Serializer& serializer, ReferenceFrame::Response& self) { @@ -223,12 +244,19 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - extract(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + extract(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + extract(serializer, self.rotation.quaternion); + + } } -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation) +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -242,15 +270,21 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen for(unsigned int i=0; i < 3; i++) insert(serializer, translation[i]); - assert(rotation || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - insert(serializer, rotation[i]); - + if( format == ReferenceFrame::Format::EULER ) + { + insert(serializer, rotation.euler); + + } + if( format == ReferenceFrame::Format::QUATERNION ) + { + insert(serializer, rotation.quaternion); + + } assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, float* rotationOut) +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -277,10 +311,16 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc for(unsigned int i=0; i < 3; i++) extract(deserializer, translationOut[i]); - assert(rotationOut || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - extract(deserializer, rotationOut[i]); - + if( format == ReferenceFrame::Format::EULER ) + { + extract(deserializer, rotationOut->euler); + + } + if( format == ReferenceFrame::Format::QUATERNION ) + { + extract(deserializer, rotationOut->quaternion); + + } if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index ff6eafd32..2befa76cb 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -127,13 +127,20 @@ typedef uint8_t mip_aiding_reference_frame_command_format; static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER = 1; ///< Translation vector followed by euler angles (roll, pitch, yaw). static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION = 2; ///< Translation vector followed by quaternion (w, x, y, z). +union mip_aiding_reference_frame_command_rotation +{ + mip_vector3f euler; + mip_quatf quaternion; +}; +typedef union mip_aiding_reference_frame_command_rotation mip_aiding_reference_frame_command_rotation; + struct mip_aiding_reference_frame_command { mip_function_selector function; uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_reference_frame_command_format format; ///< Format of the transformation. mip_vector3f translation; ///< Translation X, Y, and Z. - mip_quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + mip_aiding_reference_frame_command_rotation rotation; ///< Rotation as specified by format. }; typedef struct mip_aiding_reference_frame_command mip_aiding_reference_frame_command; @@ -148,15 +155,15 @@ struct mip_aiding_reference_frame_response uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_reference_frame_command_format format; ///< Format of the transformation. mip_vector3f translation; ///< Translation X, Y, and Z. - mip_quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + mip_aiding_reference_frame_command_rotation rotation; ///< Rotation as specified by format. }; typedef struct mip_aiding_reference_frame_response mip_aiding_reference_frame_response; void insert_mip_aiding_reference_frame_response(struct mip_serializer* serializer, const mip_aiding_reference_frame_response* self); void extract_mip_aiding_reference_frame_response(struct mip_serializer* serializer, mip_aiding_reference_frame_response* self); -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation); -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, float* rotation_out); +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const mip_aiding_reference_frame_command_rotation* rotation); +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, mip_aiding_reference_frame_command_rotation* rotation_out); mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id); diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index d50d2d2f3..0ab72d5ba 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -167,11 +167,18 @@ struct ReferenceFrame QUATERNION = 2, ///< Translation vector followed by quaternion (w, x, y, z). }; + union Rotation + { + Rotation() {} + Vector3f euler; + Quatf quaternion; + }; + FunctionSelector function = static_cast(0); uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. Vector3f translation; ///< Translation X, Y, and Z. - Quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + Rotation rotation; ///< Rotation as specified by format. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; @@ -215,7 +222,7 @@ struct ReferenceFrame uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. Vector3f translation; ///< Translation X, Y, and Z. - Quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + Rotation rotation; ///< Rotation as specified by format. auto as_tuple() @@ -230,8 +237,8 @@ void extract(Serializer& serializer, ReferenceFrame& self); void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation); -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, float* rotationOut); +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); From 41836cb7d92a9ec5b154a29cadecef4244db5a26 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 2 Oct 2023 13:49:37 -0400 Subject: [PATCH 131/252] Generate MIP definitions from branches/dev@55466. --- src/mip/definitions/data_filter.c | 64 ++++++++++++++++++++ src/mip/definitions/data_filter.cpp | 46 +++++++++++++++ src/mip/definitions/data_filter.h | 68 ++++++++++++++++++--- src/mip/definitions/data_filter.hpp | 92 ++++++++++++++++++++++++++--- 4 files changed, 256 insertions(+), 14 deletions(-) diff --git a/src/mip/definitions/data_filter.c b/src/mip/definitions/data_filter.c index f719207eb..0e395aba8 100644 --- a/src/mip/definitions/data_filter.c +++ b/src/mip/definitions/data_filter.c @@ -1687,6 +1687,70 @@ void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags( *self = tmp; } +void insert_mip_filter_aiding_frame_config_error_data(mip_serializer* serializer, const mip_filter_aiding_frame_config_error_data* self) +{ + insert_u8(serializer, self->frame_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->translation[i]); + + for(unsigned int i=0; i < 4; i++) + insert_float(serializer, self->attitude[i]); + +} +void extract_mip_filter_aiding_frame_config_error_data(mip_serializer* serializer, mip_filter_aiding_frame_config_error_data* self) +{ + extract_u8(serializer, &self->frame_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->translation[i]); + + for(unsigned int i=0; i < 4; i++) + extract_float(serializer, &self->attitude[i]); + +} +bool extract_mip_filter_aiding_frame_config_error_data_from_field(const mip_field* field, void* ptr) +{ + assert(ptr); + mip_filter_aiding_frame_config_error_data* self = ptr; + struct mip_serializer serializer; + mip_serializer_init_from_field(&serializer, field); + extract_mip_filter_aiding_frame_config_error_data(&serializer, self); + return mip_serializer_is_complete(&serializer); +} + +void insert_mip_filter_aiding_frame_config_error_uncertainty_data(mip_serializer* serializer, const mip_filter_aiding_frame_config_error_uncertainty_data* self) +{ + insert_u8(serializer, self->frame_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->attitude_unc[i]); + +} +void extract_mip_filter_aiding_frame_config_error_uncertainty_data(mip_serializer* serializer, mip_filter_aiding_frame_config_error_uncertainty_data* self) +{ + extract_u8(serializer, &self->frame_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->attitude_unc[i]); + +} +bool extract_mip_filter_aiding_frame_config_error_uncertainty_data_from_field(const mip_field* field, void* ptr) +{ + assert(ptr); + mip_filter_aiding_frame_config_error_uncertainty_data* self = ptr; + struct mip_serializer serializer; + mip_serializer_init_from_field(&serializer, field); + extract_mip_filter_aiding_frame_config_error_uncertainty_data(&serializer, self); + return mip_serializer_is_complete(&serializer); +} + #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/data_filter.cpp b/src/mip/definitions/data_filter.cpp index fdb85f32b..50e8ae9ba 100644 --- a/src/mip/definitions/data_filter.cpp +++ b/src/mip/definitions/data_filter.cpp @@ -1084,6 +1084,52 @@ void extract(Serializer& serializer, GnssDualAntennaStatus& self) } +void insert(Serializer& serializer, const AidingFrameConfigError& self) +{ + insert(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.translation[i]); + + for(unsigned int i=0; i < 4; i++) + insert(serializer, self.attitude[i]); + +} +void extract(Serializer& serializer, AidingFrameConfigError& self) +{ + extract(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.translation[i]); + + for(unsigned int i=0; i < 4; i++) + extract(serializer, self.attitude[i]); + +} + +void insert(Serializer& serializer, const AidingFrameConfigErrorUncertainty& self) +{ + insert(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.attitude_unc[i]); + +} +void extract(Serializer& serializer, AidingFrameConfigErrorUncertainty& self) +{ + extract(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.attitude_unc[i]); + +} + } // namespace data_filter } // namespace mip diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index b9aea9892..8898c974b 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -93,6 +93,8 @@ enum MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR = 0x47, MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, MIP_DATA_DESC_FILTER_GNSS_DUAL_ANTENNA_STATUS = 0x49, + MIP_DATA_DESC_FILTER_FRAME_CONFIG_ERROR = 0x50, + MIP_DATA_DESC_FILTER_FRAME_CONFIG_ERROR_UNCERTAINTY = 0x51, }; @@ -158,12 +160,18 @@ void insert_mip_filter_status_flags(struct mip_serializer* serializer, const mip void extract_mip_filter_status_flags(struct mip_serializer* serializer, mip_filter_status_flags* self); typedef uint8_t mip_filter_aiding_measurement_type; -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_GNSS = 1; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_DUAL_ANTENNA = 2; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING = 3; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_PRESSURE = 4; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_MAGNETOMETER = 5; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_SPEED = 6; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_GNSS = 1; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_DUAL_ANTENNA = 2; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING = 3; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_PRESSURE = 4; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_MAGNETOMETER = 5; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_SPEED = 6; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_POS_ECEF = 33; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_POS_LLH = 34; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_ECEF = 40; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_NED = 41; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_VEHICLE_FRAME = 42; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING_TRUE = 49; ///< void insert_mip_filter_aiding_measurement_type(struct mip_serializer* serializer, const mip_filter_aiding_measurement_type self); void extract_mip_filter_aiding_measurement_type(struct mip_serializer* serializer, mip_filter_aiding_measurement_type* self); @@ -1333,7 +1341,7 @@ struct mip_filter_aiding_measurement_summary_data { float time_of_week; ///< [seconds] uint8_t source; - mip_filter_aiding_measurement_type type; ///< (see product manual for supported types) + mip_filter_aiding_measurement_type type; ///< (see product manual for supported types) Note: values 0x20 and above correspond to commanded aiding measurements in the 0x13 Aiding command set. mip_filter_measurement_indicator indicator; }; @@ -1425,6 +1433,52 @@ void insert_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(s void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags* self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_aiding_frame_config_error (0x82,0x50) Aiding Frame Config Error [C] +/// Filter reported aiding source frame configuration error +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct mip_filter_aiding_frame_config_error_data +{ + uint8_t frame_id; ///< Frame ID for the receiver to which the antenna is attached + mip_vector3f translation; ///< Translation config X, Y, and Z (m). + mip_quatf attitude; ///< Attitude quaternion + +}; +typedef struct mip_filter_aiding_frame_config_error_data mip_filter_aiding_frame_config_error_data; +void insert_mip_filter_aiding_frame_config_error_data(struct mip_serializer* serializer, const mip_filter_aiding_frame_config_error_data* self); +void extract_mip_filter_aiding_frame_config_error_data(struct mip_serializer* serializer, mip_filter_aiding_frame_config_error_data* self); +bool extract_mip_filter_aiding_frame_config_error_data_from_field(const struct mip_field* field, void* ptr); + + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_aiding_frame_config_error_uncertainty (0x82,0x51) Aiding Frame Config Error Uncertainty [C] +/// Filter reported aiding source frame configuration error uncertainty +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct mip_filter_aiding_frame_config_error_uncertainty_data +{ + uint8_t frame_id; ///< Frame ID for the receiver to which the antenna is attached + mip_vector3f translation_unc; ///< Translation uncertaint X, Y, and Z (m). + mip_vector3f attitude_unc; ///< Attitude uncertainty, X, Y, and Z (radians). + +}; +typedef struct mip_filter_aiding_frame_config_error_uncertainty_data mip_filter_aiding_frame_config_error_uncertainty_data; +void insert_mip_filter_aiding_frame_config_error_uncertainty_data(struct mip_serializer* serializer, const mip_filter_aiding_frame_config_error_uncertainty_data* self); +void extract_mip_filter_aiding_frame_config_error_uncertainty_data(struct mip_serializer* serializer, mip_filter_aiding_frame_config_error_uncertainty_data* self); +bool extract_mip_filter_aiding_frame_config_error_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + + ///@} /// diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index bba20ba2d..a6926e3c5 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -92,6 +92,8 @@ enum DATA_ODOMETER_SCALE_FACTOR_ERROR = 0x47, DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, DATA_GNSS_DUAL_ANTENNA_STATUS = 0x49, + DATA_FRAME_CONFIG_ERROR = 0x50, + DATA_FRAME_CONFIG_ERROR_UNCERTAINTY = 0x51, }; @@ -227,12 +229,18 @@ struct FilterStatusFlags : Bitfield enum class FilterAidingMeasurementType : uint8_t { - GNSS = 1, ///< - DUAL_ANTENNA = 2, ///< - HEADING = 3, ///< - PRESSURE = 4, ///< - MAGNETOMETER = 5, ///< - SPEED = 6, ///< + GNSS = 1, ///< + DUAL_ANTENNA = 2, ///< + HEADING = 3, ///< + PRESSURE = 4, ///< + MAGNETOMETER = 5, ///< + SPEED = 6, ///< + POS_ECEF = 33, ///< + POS_LLH = 34, ///< + VEL_ECEF = 40, ///< + VEL_NED = 41, ///< + VEL_VEHICLE_FRAME = 42, ///< + HEADING_TRUE = 49, ///< }; struct FilterMeasurementIndicator : Bitfield @@ -2084,7 +2092,7 @@ struct AidingMeasurementSummary { float time_of_week = 0; ///< [seconds] uint8_t source = 0; - FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) + FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) Note: values 0x20 and above correspond to commanded aiding measurements in the 0x13 Aiding command set. FilterMeasurementIndicator indicator; static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; @@ -2244,6 +2252,76 @@ void insert(Serializer& serializer, const GnssDualAntennaStatus& self); void extract(Serializer& serializer, GnssDualAntennaStatus& self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_aiding_frame_config_error (0x82,0x50) Aiding Frame Config Error [CPP] +/// Filter reported aiding source frame configuration error +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct AidingFrameConfigError +{ + uint8_t frame_id = 0; ///< Frame ID for the receiver to which the antenna is attached + Vector3f translation; ///< Translation config X, Y, and Z (m). + Quatf attitude; ///< Attitude quaternion + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + + auto as_tuple() const + { + return std::make_tuple(frame_id,translation[0],translation[1],translation[2],attitude[0],attitude[1],attitude[2],attitude[3]); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(frame_id),std::ref(translation[0]),std::ref(translation[1]),std::ref(translation[2]),std::ref(attitude[0]),std::ref(attitude[1]),std::ref(attitude[2]),std::ref(attitude[3])); + } +}; +void insert(Serializer& serializer, const AidingFrameConfigError& self); +void extract(Serializer& serializer, AidingFrameConfigError& self); + + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_aiding_frame_config_error_uncertainty (0x82,0x51) Aiding Frame Config Error Uncertainty [CPP] +/// Filter reported aiding source frame configuration error uncertainty +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct AidingFrameConfigErrorUncertainty +{ + uint8_t frame_id = 0; ///< Frame ID for the receiver to which the antenna is attached + Vector3f translation_unc; ///< Translation uncertaint X, Y, and Z (m). + Vector3f attitude_unc; ///< Attitude uncertainty, X, Y, and Z (radians). + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + + auto as_tuple() const + { + return std::make_tuple(frame_id,translation_unc[0],translation_unc[1],translation_unc[2],attitude_unc[0],attitude_unc[1],attitude_unc[2]); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(frame_id),std::ref(translation_unc[0]),std::ref(translation_unc[1]),std::ref(translation_unc[2]),std::ref(attitude_unc[0]),std::ref(attitude_unc[1]),std::ref(attitude_unc[2])); + } +}; +void insert(Serializer& serializer, const AidingFrameConfigErrorUncertainty& self); +void extract(Serializer& serializer, AidingFrameConfigErrorUncertainty& self); + + ///@} /// From 5a4228a80b08df1230bb0d59933b8042a8b8e18a Mon Sep 17 00:00:00 2001 From: lpaicopolis <126186985+lpaicopolis@users.noreply.github.com> Date: Mon, 9 Oct 2023 11:37:43 -0400 Subject: [PATCH 132/252] Initial working version of Ublox PVT message parser with F9P --- examples/CMakeLists.txt | 4 + .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 528 ++++++++++++++++++ 2 files changed, 532 insertions(+) create mode 100644 examples/CV7_INS/CV7_INS_simple_ublox_example.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index f3ceb1a69..eda84bb73 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -51,6 +51,10 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(CV7_INS_Simple_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + add_executable(CV7_INS_Simple_Ublox_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp" ${DEVICE_SOURCES}) + target_link_libraries(CV7_INS_Simple_Ublox_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") + target_compile_definitions(CV7_INS_Simple_Ublox_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + add_executable(GX5_45_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GX5_45/GX5_45_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp new file mode 100644 index 000000000..6898d96cf --- /dev/null +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -0,0 +1,528 @@ +//////////////////////////////////////////////////////////////////////////////// +// Include Files +//////////////////////////////////////////////////////////////////////////////// + +#include "../../src/mip/mip_all.hpp" +#include +#include "../example_utils.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace mip; + +//////////////////////////////////////////////////////////////////////////////// +// Global Variables +//////////////////////////////////////////////////////////////////////////////// + +data_shared::GpsTimestamp filter_gps_time; +data_filter::Status filter_status; +data_filter::EulerAngles filter_euler_angles; +data_filter::PositionLlh filter_llh_position; +data_filter::VelocityNed filter_ned_velocity; + +uint8_t external_heading_sensor_id = 1; +uint8_t gnss_antenna_sensor_id = 2; +uint8_t vehicle_frame_velocity_sensor_id = 3; +uint8_t header_size = 6; +uint8_t checksum_size = 2; +uint8_t payload_size = 92; +uint8_t PVT_message_size = header_size + checksum_size + payload_size; + +bool filter_state_full_nav = false; + +struct UBlox_PVT_Message { + uint32_t iTOW; + uint16_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + double utc_second; + uint8_t lat_lon_valid_flag; + double nano_second; + uint8_t time_valid_flag; + double utc_time_accuracy; + double latitude; + double longitude; + uint8_t number_of_satellites; + float ned_velocity_uncertainty[3]; + double height_above_ellipsoid; + double horizontal_accuracy; + double vertical_accuracy; + float ned_velocity[3]; + float llh_uncertainty[3]; + double ground_speed; + double heading_of_motion_2d; + double heading_accuracy; + double heading_of_vehicle; + double magnetic_declination; + double magnetic_declination_accuracy; +}; + +struct UBlox_Payload_Part{ + uint8_t start_index; + uint8_t num_bytes; +}; + +std::map PAYLOAD_PART_MAP = { + {"iTOW", {0, 4}}, + {"YEAR", {4, 2}}, + {"MONTH", {6, 1}}, + {"DAY", {7, 1}}, + {"HOUR", {8, 1}}, + {"MIN", {9, 1}}, + {"SEC", {10, 1}}, + {"VALID", {11, 1}}, + {"T_ACC", {12, 4}}, + {"NANO_SEC", {16, 4}}, + {"FIX_TYPE", {20, 1}}, + {"FLAGS", {21, 1}}, + {"FLAGS_2", {22, 1}}, + {"NUM_SV", {23, 1}}, + {"LON", {24, 4}}, + {"LAT", {28, 4}}, + {"HEIGHT", {32, 4}}, + {"H_MSL", {36, 4}}, + {"H_ACC", {40, 4}}, + {"V_ACC", {44, 4}}, + {"VEL_N", {48, 4}}, + {"VEL_E", {52, 4}}, + {"VEL_D", {56, 4}}, + {"G_SPEED", {60, 4}}, + {"HEAD_MOT", {64, 4}}, + {"S_ACC", {68, 4}}, + {"HEAD_ACC", {72, 4}}, + {"FLAGS_3", {78, 1}}, + {"HEAD_VEH", {84, 4}}, + {"MAG_DEC", {88, 2}}, + {"MAG_ACC", {90, 2}} +}; + + + +//////////////////////////////////////////////////////////////////////////////// +// Function Prototypes +//////////////////////////////////////////////////////////////////////////////// + +std::unique_ptr parse_PVT_ublox_message(const uint8_t test_ublox_message[100]); + +template +void convert_mm_to_m(T& value); + +template +void convert_degrees_to_radians(T& degrees); + +float parse_2_bytes(const uint8_t payload[92], int start_index); + +double parse_4_bytes(const uint8_t payload[92], int start_index); + +double parse_long_lat(const uint8_t payload[92], int start_index); + +HANDLE open_uBlox_serial_port(const char* com_port, const int baud_rate); + +int usage(const char* argv0); + +void print_device_information(const commands_base::BaseDeviceInfo& device_info); + +void exit_gracefully(const char *message); +bool should_exit(); + + +int main(int argc, const char* argv[]) +{ + // for (int i = 0; i < argc; ++i) { + // std::cout << "Argument " << i << ": " << argv[i] << std::endl; + // } + + std::unique_ptr utils = handleCommonArgs(3, argv); + std::unique_ptr& device = utils->device; + + // + //Ping the device (note: this is good to do to make sure the device is present) + // + + if(commands_base::ping(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not ping the device!"); + + // + //Read device information + // + + commands_base::BaseDeviceInfo device_info; + if(commands_base::getDeviceInfo(*device, &device_info) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Failed to get device info"); + print_device_information(device_info); + + // + // Open ublox serial port + // TODO: Needs be refactored but works + const char* ublox_argv[2]; + ublox_argv[1] = argv[3]; + ublox_argv[2] = argv[4]; + + std::unique_ptr utils_ublox = handleCommonArgs(3, ublox_argv); + printf("Connecting to UBlox receiver ..." ); + std::cout << "Connected to " << std::string(argv[1]) << " at " << std::string(argv[2]) << std::endl; + + // + //Idle the device (note: this is good to do during setup) + // + + if(commands_base::setIdle(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set the device to idle!"); + + + // + //Load the device default settings (so the device is in a known state) + // + + if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); + + // + //Setup external aiding sensor reference frames + // + + + // + //External heading sensor reference frame. + // + float external_heading_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // External heading sensor is aligned with vehicle frame + float external_heading_sensor_to_vehicle_frame_translation[3] = {0.0, 0.0, 0.0}; // Heading measurements are agnostic to translation, translation set to zero + if(commands_aiding::writeReferenceFrame(*device, external_heading_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, + external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Unable to configure external heading sensor frame ID"); + + + // + //External GNSS antenna reference frame + // + float external_gnss_antenna_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero + float external_gnss_antenna_to_vehicle_frame_translation[3] = {0.0, 1.0, 0.0}; // Antenna is translated 1 meter in vehicle frame Y direction + if(commands_aiding::writeReferenceFrame(*device, gnss_antenna_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, + external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); + + + // + //External bodyframe velocity reference frame + // + float external_velocity_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 1.57, 0.0}; // Rotated 90 deg around yaw axis + float external_velocity_sensor_to_vehicle_frame_translation[3] = {1.0, 0.0, 0.0}; // Sensor is translated 1 meter in X direction + if(commands_aiding::writeReferenceFrame(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, + external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Unable to configure external vehicle frame velocity sensor ID"); + + + // + //Setup FILTER data format + // + + uint16_t filter_base_rate; + if(commands_3dm::getBaseRate(*device, data_filter::DESCRIPTOR_SET, &filter_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get filter base rate format!"); + + const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; + + std::array filter_descriptors = {{ + { data_shared::DATA_GPS_TIME, filter_decimation }, + { data_filter::DATA_FILTER_STATUS, filter_decimation }, + { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, + { data_filter::DATA_POS_LLH, filter_decimation }, + { data_filter::DATA_VEL_NED, filter_decimation }, + }}; + + if(commands_3dm::writeMessageFormat(*device, data_filter::DESCRIPTOR_SET, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set filter message format!"); + + + // + //Reset the filter (note: this is good to do after filter setup is complete) + // + + if(commands_filter::reset(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not reset the filter!"); + + + // + // Register data callbacks + // + + //Filter Data + DispatchHandler filter_data_handlers[5]; + + device->registerExtractor(filter_data_handlers[0], &filter_gps_time, data_filter::DESCRIPTOR_SET); + device->registerExtractor(filter_data_handlers[1], &filter_status); + device->registerExtractor(filter_data_handlers[2], &filter_euler_angles); + device->registerExtractor(filter_data_handlers[3], &filter_llh_position); + device->registerExtractor(filter_data_handlers[4], &filter_ned_velocity); + + // + //Resume the device + // + + if(commands_base::resume(*device) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not resume the device!"); + + //Main Loop: Update the interface and process data + // + + bool running = true; + mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); + mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); + + mip::Timeout wait_time; + uint8_t ublox_message_bytes[PVT_message_size]; + size_t max_length = sizeof(ublox_message_bytes); + size_t read_out = 0; + size_t* length_out = &read_out; + + printf("Sensor is configured... waiting for filter to initialize...\n"); + + while(running) { + + // Poll ublox receiver for PVT message ... + mip::Timestamp timestamp = getCurrentTimestamp(); + if (!utils_ublox->connection->recvFromDevice(ublox_message_bytes, max_length, wait_time, length_out, ×tamp)) { + std::cerr << "Error reading from the serial port." << std::endl; + } + + // Here's the message! + std::unique_ptr ublox_message = parse_PVT_ublox_message(ublox_message_bytes); + device->update(); + + //Check for full nav filter state transition + if((!filter_state_full_nav) && (filter_status.filter_state == data_filter::FilterMode::FULL_NAV)) + { + printf("NOTE: Filter has entered full navigation mode.\n"); + filter_state_full_nav = true; + } + + // Check that enough time has elapsed to send a new measurement update + mip::Timestamp current_timestamp = getCurrentTimestamp(); + mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; + mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; + + if (elapsed_time_from_last_measurement_update > 500 && read_out == max_length) + { + // Use measurement time of arrival for timestamping method + commands_aiding::Time external_measurement_time; + external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; + + // External position command + if(commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->latitude, ublox_message->longitude, ublox_message->height_above_ellipsoid, ublox_message->llh_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external position to CV7-INS\n"); + + // External global velocity command + if (commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->ned_velocity, ublox_message->ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); + } + + running = !should_exit(); + } + + exit_gracefully("Example Completed Successfully."); + +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse UBX-NAV-PVT Message +//////////////////////////////////////////////////////////////////////////////// + +std::unique_ptr parse_PVT_ublox_message(const uint8_t ublox_message_bytes[100]) { + + // TODO: Needs some refactoring but works!! + + std::unique_ptr ublox_message = std::make_unique(); + int payload_index = 0; + + // Check header + if (ublox_message_bytes[0] != 0xB5 || ublox_message_bytes[1] != 0x62) + return NULL; + + // Check class and ID to make sure its a UBX-NAV-PVT message + if (ublox_message_bytes[2] != 0x01 || ublox_message_bytes[3] != 0x07) + return NULL; + + // Check payload length + if (ublox_message_bytes[4] != 0x5C) + return NULL; + + payload_size = ublox_message_bytes[4]; + uint8_t payload[payload_size]; + + // Extract payload + for (int i = header_size; i < (PVT_message_size - checksum_size); i++) { + payload[payload_index] = ublox_message_bytes[i]; + payload_index += 1; + } + + ublox_message->iTOW = parse_4_bytes(payload, PAYLOAD_PART_MAP["iTOW"].start_index); + + ublox_message->utc_year = parse_2_bytes(payload, PAYLOAD_PART_MAP["YEAR"].start_index); + + ublox_message->utc_month = payload[PAYLOAD_PART_MAP["MONTH"].start_index]; + + ublox_message->utc_day = payload[PAYLOAD_PART_MAP["DAY"].start_index]; + + ublox_message->utc_hour = payload[PAYLOAD_PART_MAP["HOUR"].start_index]; + + ublox_message->utc_minute = payload[PAYLOAD_PART_MAP["MIN"].start_index]; + + ublox_message->utc_second = payload[PAYLOAD_PART_MAP["SEC"].start_index]; + + ublox_message->time_valid_flag = payload[PAYLOAD_PART_MAP["VALID"].start_index]; + + ublox_message->utc_time_accuracy = parse_4_bytes(payload, PAYLOAD_PART_MAP["T_ACC"].start_index); + + ublox_message->nano_second = parse_4_bytes(payload, PAYLOAD_PART_MAP["NANO_SEC"].start_index); + + ublox_message->longitude = parse_long_lat(payload, PAYLOAD_PART_MAP["LON"].start_index); + + ublox_message->latitude = parse_long_lat(payload, PAYLOAD_PART_MAP["LAT"].start_index); + + ublox_message->height_above_ellipsoid = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEIGHT"].start_index); + convert_mm_to_m(ublox_message->height_above_ellipsoid); + + // TODO: Make sure UBlox values are mapped correctly here + ublox_message->llh_uncertainty[0] = parse_4_bytes(payload, PAYLOAD_PART_MAP["V_ACC"].start_index); + convert_mm_to_m(ublox_message->llh_uncertainty[0]); + + ublox_message->llh_uncertainty[1] = parse_4_bytes(payload, PAYLOAD_PART_MAP["H_ACC"].start_index); + convert_mm_to_m(ublox_message->llh_uncertainty[1]); + + ublox_message->llh_uncertainty[2] = parse_4_bytes(payload, PAYLOAD_PART_MAP["V_ACC"].start_index); + convert_mm_to_m(ublox_message->llh_uncertainty[2]); + + ublox_message->ned_velocity[0] = parse_4_bytes(payload, PAYLOAD_PART_MAP["VEL_N"].start_index); + convert_mm_to_m(ublox_message->ned_velocity[0]); + + ublox_message->ned_velocity[1] = parse_4_bytes(payload, PAYLOAD_PART_MAP["VEL_E"].start_index); + convert_mm_to_m(ublox_message->ned_velocity[1]); + + ublox_message->ned_velocity[2] = parse_4_bytes(payload, PAYLOAD_PART_MAP["VEL_D"].start_index); + convert_mm_to_m(ublox_message->ned_velocity[2]); + + ublox_message->heading_of_motion_2d = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_MOT"].start_index); + + ublox_message->heading_accuracy = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_ACC"].start_index) * 1e-5; + convert_degrees_to_radians(ublox_message->heading_accuracy); + + ublox_message->lat_lon_valid_flag = payload[PAYLOAD_PART_MAP["FLAGS_3"].start_index]; + + ublox_message->heading_of_vehicle = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_VEH"].start_index); + convert_degrees_to_radians(ublox_message->heading_of_vehicle); + + return ublox_message; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Parse 2 bytes +//////////////////////////////////////////////////////////////////////////////// + +float parse_2_bytes(const uint8_t payload[92], int start_index) { + return (float)(*((int16_t *)&payload[start_index])); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse 4 bytes +//////////////////////////////////////////////////////////////////////////////// + +double parse_4_bytes(const uint8_t payload[92], int start_index) { + return (double)(*((int32_t *)&payload[start_index])); +} + +//////////////////////////////////////////////////////////////////////////////// +// Print device information +//////////////////////////////////////////////////////////////////////////////// + +double parse_long_lat(const uint8_t payload[92], int start_index) { + return (parse_4_bytes(payload, start_index) * 1e-7); +} + +template +void convert_mm_to_m(T& value) { + value = value / 1000.0; +} + +template +void convert_degrees_to_radians(T& degrees) { + degrees = degrees * (M_PI / 180.0); +} + +//////////////////////////////////////////////////////////////////////////////// +// Print device information +//////////////////////////////////////////////////////////////////////////////// + +void print_device_information(const commands_base::BaseDeviceInfo& device_info) +{ + printf("Connected to:\n"); + + auto print_info = [](const char* name, const char info[16]) + { + char msg[17] = {0}; + std::strncpy(msg, info, 16); + printf(" %s%s\n", name, msg); + }; + + print_info("Model name: ", device_info.model_name); + print_info("Model number: ", device_info.model_number); + print_info("Serial Number: ", device_info.serial_number); + print_info("Device Options: ", device_info.device_options); + print_info("Lot Number: ", device_info.lot_number); + + printf( " Firmware version: %d.%d.%d\n\n", + (device_info.firmware_version / 1000), + (device_info.firmware_version / 100) % 10, + (device_info.firmware_version / 1) % 100 + ); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Print Usage Function +//////////////////////////////////////////////////////////////////////////////// + +int usage(const char* argv0) +{ + printf("Usage: %s \n", argv0); + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + int dummy = getchar(); +#endif + + exit(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Check for Exit Condition +//////////////////////////////////////////////////////////////////////////////// + +bool should_exit() +{ + return false; +} + + From 29237ac382009ca21f914c879e5852324c1af703 Mon Sep 17 00:00:00 2001 From: lpaicopolis <126186985+lpaicopolis@users.noreply.github.com> Date: Mon, 9 Oct 2023 11:47:35 -0400 Subject: [PATCH 133/252] Update includes --- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 6898d96cf..6ae1719d7 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -2,8 +2,7 @@ // Include Files //////////////////////////////////////////////////////////////////////////////// -#include "../../src/mip/mip_all.hpp" -#include +#include "mip/mip_all.hpp" #include "../example_utils.hpp" #include #include @@ -13,6 +12,7 @@ #include #include #include +#include #include using namespace mip; From b0140b8420766977686b4c08472f5e0ead680b32 Mon Sep 17 00:00:00 2001 From: lpaicopolis <126186985+lpaicopolis@users.noreply.github.com> Date: Mon, 9 Oct 2023 11:49:53 -0400 Subject: [PATCH 134/252] Updated UBlox serial connection message to user --- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 6ae1719d7..98dc635e6 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -169,7 +169,7 @@ int main(int argc, const char* argv[]) std::unique_ptr utils_ublox = handleCommonArgs(3, ublox_argv); printf("Connecting to UBlox receiver ..." ); - std::cout << "Connected to " << std::string(argv[1]) << " at " << std::string(argv[2]) << std::endl; + std::cout << "Connected to " << std::string(ublox_argv[1]) << " at " << std::string(ublox_argv[2]) << std::endl; // //Idle the device (note: this is good to do during setup) From 6cbe750c4659a7438749371044d2542a92aadc44 Mon Sep 17 00:00:00 2001 From: lpaicopolis <126186985+lpaicopolis@users.noreply.github.com> Date: Mon, 9 Oct 2023 13:16:16 -0400 Subject: [PATCH 135/252] Updated user messages and added error handling --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 32 +++++++++---------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 98dc635e6..c3f227e45 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -4,15 +4,7 @@ #include "mip/mip_all.hpp" #include "../example_utils.hpp" -#include -#include -#include -#include #include -#include -#include -#include -#include #include using namespace mip; @@ -169,7 +161,7 @@ int main(int argc, const char* argv[]) std::unique_ptr utils_ublox = handleCommonArgs(3, ublox_argv); printf("Connecting to UBlox receiver ..." ); - std::cout << "Connected to " << std::string(ublox_argv[1]) << " at " << std::string(ublox_argv[2]) << std::endl; + printf("Connected to %d at %d\n", std::string(ublox_argv[1]), std::string(ublox_argv[2])); // //Idle the device (note: this is good to do during setup) @@ -288,15 +280,17 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to initialize...\n"); while(running) { + std::unique_ptr ublox_message; // Poll ublox receiver for PVT message ... mip::Timestamp timestamp = getCurrentTimestamp(); if (!utils_ublox->connection->recvFromDevice(ublox_message_bytes, max_length, wait_time, length_out, ×tamp)) { - std::cerr << "Error reading from the serial port." << std::endl; + exit_gracefully("ERROR: Error reading from serial port"); } // Here's the message! - std::unique_ptr ublox_message = parse_PVT_ublox_message(ublox_message_bytes); + if (read_out == max_length) + ublox_message = parse_PVT_ublox_message(ublox_message_bytes); device->update(); //Check for full nav filter state transition @@ -318,11 +312,15 @@ int main(int argc, const char* argv[]) external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; // External position command - if(commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->latitude, ublox_message->longitude, ublox_message->height_above_ellipsoid, ublox_message->llh_uncertainty, 1) != CmdResult::ACK_OK) - printf("WARNING: Failed to send external position to CV7-INS\n"); + if(!ublox_message->lat_lon_valid_flag) { + if(commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->latitude, ublox_message->longitude, ublox_message->height_above_ellipsoid, ublox_message->llh_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external position to CV7-INS\n"); + } + else + printf("WARNING: Invalid lon, lat, height and hMSL from ublox receiver"); // External global velocity command - if (commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->ned_velocity, ublox_message->ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->ned_velocity, ublox_message->ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); } @@ -346,15 +344,15 @@ std::unique_ptr parse_PVT_ublox_message(const uint8_t ublox_m // Check header if (ublox_message_bytes[0] != 0xB5 || ublox_message_bytes[1] != 0x62) - return NULL; + exit_gracefully("ERROR: Incorrect message header"); // Check class and ID to make sure its a UBX-NAV-PVT message if (ublox_message_bytes[2] != 0x01 || ublox_message_bytes[3] != 0x07) - return NULL; + exit_gracefully("ERROR: Not a UBX-NAV-PVT message"); // Check payload length if (ublox_message_bytes[4] != 0x5C) - return NULL; + exit_gracefully("ERROR: Incorrect payload size for PVT message"); payload_size = ublox_message_bytes[4]; uint8_t payload[payload_size]; From e4574dada2634436a1dc3315b5a3d449823ffa24 Mon Sep 17 00:00:00 2001 From: Nick DaCosta Date: Mon, 9 Oct 2023 16:09:52 -0400 Subject: [PATCH 136/252] Added send/receive timeouts for Windows TCP connections --- src/mip/utils/tcp_socket.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/mip/utils/tcp_socket.c b/src/mip/utils/tcp_socket.c index 89c573d54..1e4d05734 100644 --- a/src/mip/utils/tcp_socket.c +++ b/src/mip/utils/tcp_socket.c @@ -81,8 +81,13 @@ static bool tcp_socket_open_common(tcp_socket* socket_ptr, const char* hostname, if( socket_ptr->handle == INVALID_SOCKET ) return false; -#ifndef WIN32 - // Todo: timeouts on windows +#ifdef WIN32 + if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout_ms, sizeof(timeout_ms)) != 0 ) + return false; + + if( setsockopt(socket_ptr->handle, SOL_SOCKET, SO_SNDTIMEO, (char*)&timeout_ms, sizeof(timeout_ms)) != 0 ) + return false; +#else struct timeval timeout_option; timeout_option.tv_sec = timeout_ms / 1000; timeout_option.tv_usec = (timeout_ms % 1000) * 1000; From 2430829f69de9286a5d2a536c5ecb11e99147185 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 10 Oct 2023 13:11:27 -0400 Subject: [PATCH 137/252] Prepend \.\ to serial port names on windows. --- src/mip/utils/serial_port.c | 43 +++++++++++++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 4 deletions(-) diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index 04b426cf1..bb045b7d4 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -3,7 +3,10 @@ #include "serial_port.h" -#ifdef __APPLE__ +#if defined WIN32 +#include +#include +#elif defined __APPLE__ #include #endif @@ -77,20 +80,52 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) BOOL ready; DCB dcb; + // Prepend '\\.\' to the com port if not already present. + bool added_prefix = false; + const char* tmp_port_str = port_str; + size_t port_str_len = strlen(port_str); + + // Only prepend if port_str is of the form 'COMx' + if(port_str_len >= 4 && toupper(port_str[0]) == 'C' && toupper(port_str[1]) == 'O' && toupper(port_str[2]) == 'M' && isdigit(port_str[3])) + { + char* tmp = (char*)malloc(port_str_len + 4 + 1); + if (!tmp) + return false; + + tmp[0] = '\\'; + tmp[1] = '\\'; + tmp[2] = '.'; + tmp[3] = '\\'; + memcpy(&tmp[4], port_str, port_str_len+1); + + added_prefix = true; + tmp_port_str = tmp; + } + //Connect to the provided com port - port->handle = CreateFile(port_str, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); + port->handle = CreateFile(tmp_port_str, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); + + // Ensure that the free() call in the following 'if' block doesn't clobber an error value + DWORD last_error = GetLastError(); + + // If the port string was modified + if (added_prefix) + { + free(tmp_port_str); + tmp_port_str = NULL; + } //Check for an invalid handle if(port->handle == INVALID_HANDLE_VALUE) { - MIP_LOG_ERROR("Unable to open com port (%d)\n", GetLastError()); + MIP_LOG_ERROR("Unable to open com port (%d)\n", last_error); return false; } //Setup the com port buffer sizes if(SetupComm(port->handle, COM_PORT_BUFFER_SIZE, COM_PORT_BUFFER_SIZE) == 0) { - MIP_LOG_ERROR("Unable to setup com port buffer size (%d)\n", GetLastError()); + MIP_LOG_ERROR("Unable to setup com port buffer size (%d)\n", last_error); CloseHandle(port->handle); port->handle = INVALID_HANDLE_VALUE; return false; From 7c4aff9367d5ccb7f43f448b1c006f23e61edfd3 Mon Sep 17 00:00:00 2001 From: lpaicopolis <126186985+lpaicopolis@users.noreply.github.com> Date: Tue, 10 Oct 2023 18:04:09 -0400 Subject: [PATCH 138/252] Updated buffer to be more robust --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 108 ++++++++++++++++-- 1 file changed, 99 insertions(+), 9 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index c3f227e45..7c7c060b3 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -6,6 +6,7 @@ #include "../example_utils.hpp" #include #include +#include using namespace mip; @@ -119,6 +120,8 @@ double parse_long_lat(const uint8_t payload[92], int start_index); HANDLE open_uBlox_serial_port(const char* com_port, const int baud_rate); +bool verify_checksum(uint8_t *data); + int usage(const char* argv0); void print_device_information(const commands_base::BaseDeviceInfo& device_info); @@ -271,26 +274,89 @@ int main(int argc, const char* argv[]) mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); - mip::Timeout wait_time; - uint8_t ublox_message_bytes[PVT_message_size]; + mip::Timeout wait_time = 60000; + int buffer_length = 1024; + uint8_t ublox_message_bytes[buffer_length]; size_t max_length = sizeof(ublox_message_bytes); size_t read_out = 0; size_t* length_out = &read_out; + mip::Timestamp timestamp; + uint8_t PVT_message[PVT_message_size]; printf("Sensor is configured... waiting for filter to initialize...\n"); while(running) { std::unique_ptr ublox_message; - + bool pvt_message_found = false; + bool pvt_message_overflow = false; // Poll ublox receiver for PVT message ... - mip::Timestamp timestamp = getCurrentTimestamp(); if (!utils_ublox->connection->recvFromDevice(ublox_message_bytes, max_length, wait_time, length_out, ×tamp)) { exit_gracefully("ERROR: Error reading from serial port"); } - // Here's the message! - if (read_out == max_length) - ublox_message = parse_PVT_ublox_message(ublox_message_bytes); + for (int i = buffer_length - 1 - 4; i >= 0; i--) { + // look for 0xB5 + if (ublox_message_bytes[i] != 0xB5) + continue; + if (ublox_message_bytes[i+1] != 0x62) + continue; + if (ublox_message_bytes[i+2] != 0x01) + continue; + if (ublox_message_bytes[i+3] != 0x07) + continue; + + // If there are 100 bytes in buffer after (0xB5) is found and there were at least 100 bytes read + if (i + (PVT_message_size - 1) < buffer_length && (*length_out - i) >= PVT_message_size ) { + + // Extract payload + int payload_index = 0; + + for (int z = i; z < PVT_message_size; z++) { + PVT_message[payload_index] = ublox_message_bytes[z]; + payload_index += 1; + } + // If checksum valid + if (verify_checksum(PVT_message)) { + ublox_message = parse_PVT_ublox_message(PVT_message); + // send parsed ublox_message to CV7 + break; + } + else { + printf("Found packet, failed checksum verification"); + break; + } + } + + size_t start_index = *length_out - i; + size_t package_bytes_remaining = PVT_message_size - start_index; + + while (*length_out == package_bytes_remaining) { + std::memmove(ublox_message_bytes, ublox_message_bytes + start_index, package_bytes_remaining); + + if (!utils_ublox->connection->recvFromDevice(&ublox_message_bytes[start_index], package_bytes_remaining, wait_time, length_out, ×tamp)) + exit_gracefully("ERROR: Error reading from serial port"); + + start_index += *length_out; + package_bytes_remaining = PVT_message_size - start_index; + } + + for (int i = 0; i < payload_size; i++) { + PVT_message[i] = ublox_message_bytes[i]; + } + + // check checksum + if (verify_checksum(PVT_message)) { + + ublox_message = parse_PVT_ublox_message(PVT_message); + // send parsed ublox_message to CV7 + break; + } + // else then I clear ublox_message bytes and break; + std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); + break; + } + + device->update(); //Check for full nav filter state transition @@ -305,7 +371,7 @@ int main(int argc, const char* argv[]) mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; - if (elapsed_time_from_last_measurement_update > 500 && read_out == max_length) + if (elapsed_time_from_last_measurement_update > 500 && pvt_message_found) { // Use measurement time of arrival for timestamping method commands_aiding::Time external_measurement_time; @@ -331,11 +397,35 @@ int main(int argc, const char* argv[]) } +//////////////////////////////////////////////////////////////////////////////// +// Verify UBX-NAV-PVT Checksum +//////////////////////////////////////////////////////////////////////////////// + +bool verify_checksum(uint8_t *data) { + unsigned i, j; + uint8_t a, b; + + j = ((unsigned)data[4] + ((unsigned)data[5] << 8) + 6); + + a = 0; + b = 0; + + for(i=2; i parse_PVT_ublox_message(const uint8_t ublox_message_bytes[100]) { +std::unique_ptr parse_PVT_ublox_message(const uint8_t ublox_message_bytes[]) { // TODO: Needs some refactoring but works!! From c8e1b7a79b4eb36ee43d2d9bbc05aa73c7a3008f Mon Sep 17 00:00:00 2001 From: lpaicopolis <126186985+lpaicopolis@users.noreply.github.com> Date: Tue, 10 Oct 2023 23:08:59 -0400 Subject: [PATCH 139/252] Debugged and soft tested new buffer algorithm...currently working! --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 68 +++++++------------ 1 file changed, 26 insertions(+), 42 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 7c7c060b3..457badef1 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -104,7 +104,7 @@ std::map PAYLOAD_PART_MAP = { // Function Prototypes //////////////////////////////////////////////////////////////////////////////// -std::unique_ptr parse_PVT_ublox_message(const uint8_t test_ublox_message[100]); +std::unique_ptr parse_PVT_ublox_message(const uint8_t PVT_message[100]); template void convert_mm_to_m(T& value); @@ -132,9 +132,6 @@ bool should_exit(); int main(int argc, const char* argv[]) { - // for (int i = 0; i < argc; ++i) { - // std::cout << "Argument " << i << ": " << argv[i] << std::endl; - // } std::unique_ptr utils = handleCommonArgs(3, argv); std::unique_ptr& device = utils->device; @@ -158,13 +155,14 @@ int main(int argc, const char* argv[]) // // Open ublox serial port // TODO: Needs be refactored but works - const char* ublox_argv[2]; + const char* ublox_argv[3]; + ublox_argv[0] = argv[0]; ublox_argv[1] = argv[3]; ublox_argv[2] = argv[4]; std::unique_ptr utils_ublox = handleCommonArgs(3, ublox_argv); - printf("Connecting to UBlox receiver ..." ); - printf("Connected to %d at %d\n", std::string(ublox_argv[1]), std::string(ublox_argv[2])); + printf("Connecting to UBlox F9P ..." ); + printf("Connected to %s at %s\n", ublox_argv[1], ublox_argv[2]); // //Idle the device (note: this is good to do during setup) @@ -274,21 +272,22 @@ int main(int argc, const char* argv[]) mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); - mip::Timeout wait_time = 60000; - int buffer_length = 1024; - uint8_t ublox_message_bytes[buffer_length]; + mip::Timeout wait_time = 3000; + size_t buffer_length = 1024; + uint8_t ublox_message_bytes[1024]; size_t max_length = sizeof(ublox_message_bytes); size_t read_out = 0; size_t* length_out = &read_out; mip::Timestamp timestamp; - uint8_t PVT_message[PVT_message_size]; + uint8_t PVT_message[100]; printf("Sensor is configured... waiting for filter to initialize...\n"); - while(running) { + while(running) { std::unique_ptr ublox_message; bool pvt_message_found = false; - bool pvt_message_overflow = false; + // else then I clear ublox_message bytes and break; ++ std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); // Poll ublox receiver for PVT message ... if (!utils_ublox->connection->recvFromDevice(ublox_message_bytes, max_length, wait_time, length_out, ×tamp)) { exit_gracefully("ERROR: Error reading from serial port"); @@ -311,13 +310,14 @@ int main(int argc, const char* argv[]) // Extract payload int payload_index = 0; - for (int z = i; z < PVT_message_size; z++) { + for (int z = i; z < PVT_message_size + i; z++) { PVT_message[payload_index] = ublox_message_bytes[z]; payload_index += 1; } // If checksum valid if (verify_checksum(PVT_message)) { - ublox_message = parse_PVT_ublox_message(PVT_message); + ublox_message = parse_PVT_ublox_message(PVT_message); + pvt_message_found = true; // send parsed ublox_message to CV7 break; } @@ -346,16 +346,15 @@ int main(int argc, const char* argv[]) // check checksum if (verify_checksum(PVT_message)) { - ublox_message = parse_PVT_ublox_message(PVT_message); // send parsed ublox_message to CV7 + pvt_message_found = true; break; } // else then I clear ublox_message bytes and break; std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); break; } - device->update(); @@ -403,19 +402,19 @@ int main(int argc, const char* argv[]) bool verify_checksum(uint8_t *data) { unsigned i, j; - uint8_t a, b; + uint8_t ck_a, ck_b; j = ((unsigned)data[4] + ((unsigned)data[5] << 8) + 6); - a = 0; - b = 0; + ck_a = 0; + ck_b = 0; - for(i=2; i parse_PVT_ublox_message(const uint8_t ublox_message_bytes[]) { +std::unique_ptr parse_PVT_ublox_message(const uint8_t PVT_message[]) { // TODO: Needs some refactoring but works!! std::unique_ptr ublox_message = std::make_unique(); int payload_index = 0; - - // Check header - if (ublox_message_bytes[0] != 0xB5 || ublox_message_bytes[1] != 0x62) - exit_gracefully("ERROR: Incorrect message header"); - - // Check class and ID to make sure its a UBX-NAV-PVT message - if (ublox_message_bytes[2] != 0x01 || ublox_message_bytes[3] != 0x07) - exit_gracefully("ERROR: Not a UBX-NAV-PVT message"); - - // Check payload length - if (ublox_message_bytes[4] != 0x5C) - exit_gracefully("ERROR: Incorrect payload size for PVT message"); - - payload_size = ublox_message_bytes[4]; - uint8_t payload[payload_size]; + uint8_t payload[92]; // Extract payload for (int i = header_size; i < (PVT_message_size - checksum_size); i++) { - payload[payload_index] = ublox_message_bytes[i]; + payload[payload_index] = PVT_message[i]; payload_index += 1; } @@ -613,4 +598,3 @@ bool should_exit() return false; } - From 1581040da4a5b1b1e54330b547eacdcf5d4d8f3c Mon Sep 17 00:00:00 2001 From: lpaicopolis <126186985+lpaicopolis@users.noreply.github.com> Date: Wed, 11 Oct 2023 12:08:34 -0400 Subject: [PATCH 140/252] Temporary changes --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 457badef1..7c6f2d86a 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -132,6 +132,9 @@ bool should_exit(); int main(int argc, const char* argv[]) { + for (int i = 0; i < argc; ++i) { + std::cout << "Argument " << i << ": " << argv[i] << std::endl; + } std::unique_ptr utils = handleCommonArgs(3, argv); std::unique_ptr& device = utils->device; @@ -160,6 +163,8 @@ int main(int argc, const char* argv[]) ublox_argv[1] = argv[3]; ublox_argv[2] = argv[4]; + + std::unique_ptr utils_ublox = handleCommonArgs(3, ublox_argv); printf("Connecting to UBlox F9P ..." ); printf("Connected to %s at %s\n", ublox_argv[1], ublox_argv[2]); @@ -286,14 +291,14 @@ int main(int argc, const char* argv[]) while(running) { std::unique_ptr ublox_message; bool pvt_message_found = false; - // else then I clear ublox_message bytes and break; -+ std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); + std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); // Poll ublox receiver for PVT message ... if (!utils_ublox->connection->recvFromDevice(ublox_message_bytes, max_length, wait_time, length_out, ×tamp)) { exit_gracefully("ERROR: Error reading from serial port"); } for (int i = buffer_length - 1 - 4; i >= 0; i--) { + // look for 0xB5 if (ublox_message_bytes[i] != 0xB5) continue; @@ -320,10 +325,8 @@ int main(int argc, const char* argv[]) pvt_message_found = true; // send parsed ublox_message to CV7 break; - } - else { + } else { printf("Found packet, failed checksum verification"); - break; } } @@ -332,6 +335,7 @@ int main(int argc, const char* argv[]) while (*length_out == package_bytes_remaining) { std::memmove(ublox_message_bytes, ublox_message_bytes + start_index, package_bytes_remaining); + std::memset(&ublox_message_bytes[*length_out], 0, buffer_length-*length_out); if (!utils_ublox->connection->recvFromDevice(&ublox_message_bytes[start_index], package_bytes_remaining, wait_time, length_out, ×tamp)) exit_gracefully("ERROR: Error reading from serial port"); @@ -350,12 +354,15 @@ int main(int argc, const char* argv[]) // send parsed ublox_message to CV7 pvt_message_found = true; break; + } else { + printf("Found packet, failed checksum verification"); + break; } - // else then I clear ublox_message bytes and break; - std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); - break; } + if (!pvt_message_found) + continue; + device->update(); //Check for full nav filter state transition @@ -370,7 +377,7 @@ int main(int argc, const char* argv[]) mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; - if (elapsed_time_from_last_measurement_update > 500 && pvt_message_found) + if (elapsed_time_from_last_measurement_update > 500) { // Use measurement time of arrival for timestamping method commands_aiding::Time external_measurement_time; From c78c29925926b2d7bdde6ddb10210e7ffa184e9d Mon Sep 17 00:00:00 2001 From: David Robbins Date: Wed, 11 Oct 2023 16:15:37 -0400 Subject: [PATCH 141/252] Added PPS support to ublox example --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 315 ++++++++++++++---- 1 file changed, 252 insertions(+), 63 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 7c6f2d86a..1fbd08f5c 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -7,6 +7,8 @@ #include #include #include +#include +#include using namespace mip; @@ -14,11 +16,12 @@ using namespace mip; // Global Variables //////////////////////////////////////////////////////////////////////////////// -data_shared::GpsTimestamp filter_gps_time; -data_filter::Status filter_status; -data_filter::EulerAngles filter_euler_angles; -data_filter::PositionLlh filter_llh_position; -data_filter::VelocityNed filter_ned_velocity; +data_shared::GpsTimestamp filter_gps_time; +data_filter::Status filter_status; +data_filter::EulerAngles filter_euler_angles; +data_filter::PositionLlh filter_llh_position; +data_filter::VelocityNed filter_ned_velocity; +data_system::TimeSyncStatus system_time_sync_status; uint8_t external_heading_sensor_id = 1; uint8_t gnss_antenna_sensor_id = 2; @@ -98,6 +101,24 @@ std::map PAYLOAD_PART_MAP = { {"MAG_ACC", {90, 2}} }; +struct InputArguments +{ + std::string mip_device_port_name; + std::string mip_device_baudrate; + std::string mip_binary_filepath; + + std::string ublox_device_port_name; + std::string ublox_device_baudrate; + + bool enable_pps_sync = false; + + int pps_input_pin_id = 1; + + commands_filter::InitializationConfiguration::AlignmentSelector filter_heading_alignment_method = commands_filter::InitializationConfiguration::AlignmentSelector::KINEMATIC; + + float gnss_antenna_lever_arm[3] = {0,0,0}; +}; + //////////////////////////////////////////////////////////////////////////////// @@ -118,7 +139,7 @@ double parse_4_bytes(const uint8_t payload[92], int start_index); double parse_long_lat(const uint8_t payload[92], int start_index); -HANDLE open_uBlox_serial_port(const char* com_port, const int baud_rate); +//HANDLE open_uBlox_serial_port(const char* com_port, const int baud_rate); bool verify_checksum(uint8_t *data); @@ -129,16 +150,26 @@ void print_device_information(const commands_base::BaseDeviceInfo& device_info); void exit_gracefully(const char *message); bool should_exit(); +InputArguments parse_input_arguments(int argc, const char* argv[]); + +uint64_t convert_gps_tow_to_nanoseconds(int week_number, float time_of_week); + +int get_gps_week(int year, int month, int day); + int main(int argc, const char* argv[]) { - for (int i = 0; i < argc; ++i) { - std::cout << "Argument " << i << ": " << argv[i] << std::endl; - } - - std::unique_ptr utils = handleCommonArgs(3, argv); + + InputArguments input_arguments = parse_input_arguments(argc, argv); + + std::unique_ptr utils = openFromArgs(input_arguments.mip_device_port_name, input_arguments.mip_device_baudrate, input_arguments.mip_binary_filepath); std::unique_ptr& device = utils->device; + // + //Attempt to idle the device before pinging + // + commands_base::setIdle(*device); + // //Ping the device (note: this is good to do to make sure the device is present) // @@ -156,19 +187,12 @@ int main(int argc, const char* argv[]) print_device_information(device_info); // - // Open ublox serial port - // TODO: Needs be refactored but works - const char* ublox_argv[3]; - ublox_argv[0] = argv[0]; - ublox_argv[1] = argv[3]; - ublox_argv[2] = argv[4]; - + // Open uBlox serial port + // + printf("Connecting to UBlox F9P on %s at %s...", input_arguments.ublox_device_port_name.c_str(), input_arguments.ublox_device_baudrate.c_str()); + std::unique_ptr utils_ublox = openFromArgs(input_arguments.ublox_device_port_name, input_arguments.ublox_device_baudrate, {}); - std::unique_ptr utils_ublox = handleCommonArgs(3, ublox_argv); - printf("Connecting to UBlox F9P ..." ); - printf("Connected to %s at %s\n", ublox_argv[1], ublox_argv[2]); - // //Idle the device (note: this is good to do during setup) // @@ -184,39 +208,24 @@ int main(int argc, const char* argv[]) if(commands_3dm::defaultDeviceSettings(*device) != CmdResult::ACK_OK) exit_gracefully("ERROR: Could not load default device settings!"); - // - //Setup external aiding sensor reference frames - // - - - // - //External heading sensor reference frame. - // - float external_heading_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // External heading sensor is aligned with vehicle frame - float external_heading_sensor_to_vehicle_frame_translation[3] = {0.0, 0.0, 0.0}; // Heading measurements are agnostic to translation, translation set to zero - if(commands_aiding::writeReferenceFrame(*device, external_heading_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Unable to configure external heading sensor frame ID"); - // //External GNSS antenna reference frame // float external_gnss_antenna_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero - float external_gnss_antenna_to_vehicle_frame_translation[3] = {0.0, 1.0, 0.0}; // Antenna is translated 1 meter in vehicle frame Y direction if(commands_aiding::writeReferenceFrame(*device, gnss_antenna_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + input_arguments.gnss_antenna_lever_arm, external_gnss_antenna_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); // - //External bodyframe velocity reference frame + // Set heading initialization source // - float external_velocity_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 1.57, 0.0}; // Rotated 90 deg around yaw axis - float external_velocity_sensor_to_vehicle_frame_translation[3] = {1.0, 0.0, 0.0}; // Sensor is translated 1 meter in X direction - if(commands_aiding::writeReferenceFrame(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Unable to configure external vehicle frame velocity sensor ID"); + + float default_init[3] = {0,0,0}; + if(commands_filter::writeInitializationConfiguration(*device, false, commands_filter::InitializationConfiguration::InitialConditionSource::AUTO_POS_VEL_ATT, input_arguments.filter_heading_alignment_method, + 0, 0, 0, default_init, default_init, commands_filter::FilterReferenceFrame::ECEF) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not load default device settings!"); // @@ -227,7 +236,7 @@ int main(int argc, const char* argv[]) if(commands_3dm::getBaseRate(*device, data_filter::DESCRIPTOR_SET, &filter_base_rate) != CmdResult::ACK_OK) exit_gracefully("ERROR: Could not get filter base rate format!"); - const uint16_t filter_sample_rate = 100; // Hz + const uint16_t filter_sample_rate = 10; // Hz const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; std::array filter_descriptors = {{ @@ -242,6 +251,48 @@ int main(int argc, const char* argv[]) exit_gracefully("ERROR: Could not set filter message format!"); + if (input_arguments.enable_pps_sync) + { + + // + //Setup SYSTEM data format to monitor PPS status + // + + uint16_t system_data_base_rate; + if(commands_3dm::getBaseRate(*device, data_system::DESCRIPTOR_SET, &system_data_base_rate) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not get system data base rate format!"); + + const uint16_t system_data_sample_rate = 10; // Hz + const uint16_t system_data_decimation = system_data_base_rate / system_data_sample_rate; + + std::array system_data_descriptors = {{ + { data_shared::DATA_GPS_TIME, system_data_decimation }, + { data_system::DATA_TIME_SYNC_STATUS, system_data_decimation }, + }}; + + if(commands_3dm::writeMessageFormat(*device, data_system::DESCRIPTOR_SET, system_data_descriptors.size(), system_data_descriptors.data()) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set system data message format!"); + + + // + // Setup GPIO for PPS input functionality + // + + if (commands_3dm::writeGpioConfig(*device, input_arguments.pps_input_pin_id, mip::commands_3dm::GpioConfig::Feature::PPS, mip::commands_3dm::GpioConfig::Behavior::PPS_INPUT, mip::commands_3dm::GpioConfig::PinMode::NONE) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set GPIO to PPS input!"); + + + // + // Setup PPS source as GPIO + // + + if (mip::commands_3dm::writePpsSource(*device, mip::commands_3dm::PpsSource::Source::GPIO) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Failed to set PPS source to GPIO!"); + + } + + + // //Reset the filter (note: this is good to do after filter setup is complete) // @@ -263,6 +314,11 @@ int main(int argc, const char* argv[]) device->registerExtractor(filter_data_handlers[3], &filter_llh_position); device->registerExtractor(filter_data_handlers[4], &filter_ned_velocity); + //System Data + DispatchHandler system_data_handlers[1]; + + device->registerExtractor(system_data_handlers[0], &system_time_sync_status, data_system::DESCRIPTOR_SET); + // //Resume the device // @@ -274,6 +330,7 @@ int main(int argc, const char* argv[]) // bool running = true; + bool pps_sync_valid = false; mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); @@ -289,6 +346,25 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to initialize...\n"); while(running) { + + device->update(); + + // Wait for valid PPS lock + if (input_arguments.enable_pps_sync && !pps_sync_valid) + { + pps_sync_valid = system_time_sync_status.time_sync; + + mip::Timestamp current_timestamp = getCurrentTimestamp(); + mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; + if (elapsed_time_from_last_message_print > 1000) + { + printf("Waiting for valid PPS lock...\n"); + prev_print_timestamp = current_timestamp; + } + continue; + } + + std::unique_ptr ublox_message; bool pvt_message_found = false; std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); @@ -363,8 +439,6 @@ int main(int argc, const char* argv[]) if (!pvt_message_found) continue; - device->update(); - //Check for full nav filter state transition if((!filter_state_full_nav) && (filter_status.filter_state == data_filter::FilterMode::FULL_NAV)) { @@ -374,26 +448,65 @@ int main(int argc, const char* argv[]) // Check that enough time has elapsed to send a new measurement update mip::Timestamp current_timestamp = getCurrentTimestamp(); - mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; + + bool ublox_data_valid = ublox_message->time_valid_flag && ublox_message->lat_lon_valid_flag; + if (!ublox_data_valid) + { + printf("WARNING: Ublox data invalid"); + continue; + } + mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; + //Once in full nav, print out data at 1 Hz + if((filter_status.filter_state == data_filter::FilterMode::FULL_NAV) && (elapsed_time_from_last_message_print >= 1000)) + { + printf("\n\n****Filter navigation state****\n"); + printf("TIMESTAMP: %f\n", filter_gps_time.tow); + printf("ATTITUDE_EULER = [%f %f %f]\n", filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw); + printf("LLH_POSITION = [%f %f %f]\n", filter_llh_position.latitude, filter_llh_position.longitude, filter_llh_position.ellipsoid_height); + printf("NED_VELOCITY = [%f %f %f]\n", filter_ned_velocity.north, filter_ned_velocity.east, filter_ned_velocity.down); + + prev_print_timestamp = current_timestamp; + } + mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; if (elapsed_time_from_last_measurement_update > 500) { - // Use measurement time of arrival for timestamping method + printf("Sending measurement update...\n"); + commands_aiding::Time external_measurement_time; - external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; - // External position command - if(!ublox_message->lat_lon_valid_flag) { - if(commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->latitude, ublox_message->longitude, ublox_message->height_above_ellipsoid, ublox_message->llh_uncertainty, 1) != CmdResult::ACK_OK) - printf("WARNING: Failed to send external position to CV7-INS\n"); + if (input_arguments.enable_pps_sync) + { + // Update week number + uint32_t week_number = get_gps_week(ublox_message->utc_year, ublox_message->utc_month, ublox_message->utc_day); + if (!commands_base::writeGpsTimeUpdate(*device, commands_base::GpsTimeUpdate::FieldId::WEEK_NUMBER, week_number)) + printf("WARNING: Failed to send week number time update to CV7-INS\n"); + + // Update time of week + float time_of_week = float(ublox_message->iTOW) * 1e-3; + uint32_t time_of_week_int = floor(time_of_week); + if (!commands_base::writeGpsTimeUpdate(*device, commands_base::GpsTimeUpdate::FieldId::TIME_OF_WEEK, time_of_week_int)) + printf("WARNING: Failed to send time of week update to CV7-INS\n"); + + external_measurement_time.timebase = commands_aiding::Time::Timebase::EXTERNAL_TIME; + external_measurement_time.nanoseconds = convert_gps_tow_to_nanoseconds(week_number, time_of_week); } else - printf("WARNING: Invalid lon, lat, height and hMSL from ublox receiver"); + { + // If no PPS sync is supplied use device time of arrival for data timestamping method + external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; + } + + // External position command + if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id,ublox_message->latitude, ublox_message->longitude,ublox_message->height_above_ellipsoid, ublox_message->llh_uncertainty, 1) != CmdResult::ACK_OK) + printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command - if(commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id, ublox_message->ned_velocity, ublox_message->ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id,ublox_message->ned_velocity, ublox_message->ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); + + prev_measurement_update_timestamp = current_timestamp; } running = !should_exit(); @@ -473,11 +586,11 @@ std::unique_ptr parse_PVT_ublox_message(const uint8_t PVT_mes convert_mm_to_m(ublox_message->height_above_ellipsoid); // TODO: Make sure UBlox values are mapped correctly here - ublox_message->llh_uncertainty[0] = parse_4_bytes(payload, PAYLOAD_PART_MAP["V_ACC"].start_index); - convert_mm_to_m(ublox_message->llh_uncertainty[0]); + double horizontal_uncertainty = parse_4_bytes(payload, PAYLOAD_PART_MAP["H_ACC"].start_index); + convert_mm_to_m(horizontal_uncertainty); - ublox_message->llh_uncertainty[1] = parse_4_bytes(payload, PAYLOAD_PART_MAP["H_ACC"].start_index); - convert_mm_to_m(ublox_message->llh_uncertainty[1]); + ublox_message->llh_uncertainty[0] = horizontal_uncertainty; + ublox_message->llh_uncertainty[1] = horizontal_uncertainty; ublox_message->llh_uncertainty[2] = parse_4_bytes(payload, PAYLOAD_PART_MAP["V_ACC"].start_index); convert_mm_to_m(ublox_message->llh_uncertainty[2]); @@ -490,13 +603,18 @@ std::unique_ptr parse_PVT_ublox_message(const uint8_t PVT_mes ublox_message->ned_velocity[2] = parse_4_bytes(payload, PAYLOAD_PART_MAP["VEL_D"].start_index); convert_mm_to_m(ublox_message->ned_velocity[2]); + + float speed_accuracy = parse_4_bytes(payload, PAYLOAD_PART_MAP["S_ACC"].start_index); + convert_mm_to_m(speed_accuracy); + for (int i = 0; i<3; i++) + ublox_message->ned_velocity_uncertainty[i] = speed_accuracy; ublox_message->heading_of_motion_2d = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_MOT"].start_index); ublox_message->heading_accuracy = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_ACC"].start_index) * 1e-5; convert_degrees_to_radians(ublox_message->heading_accuracy); - ublox_message->lat_lon_valid_flag = payload[PAYLOAD_PART_MAP["FLAGS_3"].start_index]; + ublox_message->lat_lon_valid_flag = !payload[PAYLOAD_PART_MAP["FLAGS_3"].start_index]; ublox_message->heading_of_vehicle = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_VEH"].start_index); convert_degrees_to_radians(ublox_message->heading_of_vehicle); @@ -522,7 +640,7 @@ double parse_4_bytes(const uint8_t payload[92], int start_index) { } //////////////////////////////////////////////////////////////////////////////// -// Print device information +// Utility functions //////////////////////////////////////////////////////////////////////////////// double parse_long_lat(const uint8_t payload[92], int start_index) { @@ -539,6 +657,29 @@ void convert_degrees_to_radians(T& degrees) { degrees = degrees * (M_PI / 180.0); } +uint64_t convert_gps_tow_to_nanoseconds(int week_number, float time_of_week) +{ + return floor(float(week_number) * 604800 * 1e9 + time_of_week * 1e9); +} + +time_t time_from_ymd(int year, int month, int day) +{ + struct tm tm = {0}; + tm.tm_year = year - 1900; + tm.tm_mon = month - 1; + tm.tm_mday = day; + return mktime(&tm); +} + +#define SECS_PER_WEEK (60L*60*24*7) + +int get_gps_week(int year, int month, int day) +{ + // See update below + double diff = difftime(time_from_ymd(year, month, day), time_from_ymd(1980, 1, 1)); // See update + return (int) (diff / SECS_PER_WEEK); +} + //////////////////////////////////////////////////////////////////////////////// // Print device information //////////////////////////////////////////////////////////////////////////////// @@ -568,13 +709,61 @@ void print_device_information(const commands_base::BaseDeviceInfo& device_info) } +InputArguments parse_input_arguments(int argc, const char* argv[]) +{ + if (argc < 5) + { + usage(argv[0]); + exit_gracefully("ERROR: Incorrect input arguments"); + } + + // Look for help flag + for (int i = 1; i < argc; i++) + { + if(strcmp(argv[i], "-h") == 0) + { + usage(argv[0]); + exit_gracefully(""); + } + } + + InputArguments input_arguments; + + input_arguments.mip_device_port_name = argv[1]; + input_arguments.mip_device_baudrate = argv[2]; + + input_arguments.ublox_device_port_name = argv[3]; + input_arguments.ublox_device_baudrate = argv[4]; + + if (argc >= 6) + { + int heading_alignment_int = std::stoi(argv[5]); + + if (heading_alignment_int == 0) + input_arguments.filter_heading_alignment_method = commands_filter::InitializationConfiguration::AlignmentSelector::KINEMATIC; + else if (heading_alignment_int == 1) + input_arguments.filter_heading_alignment_method = commands_filter::InitializationConfiguration::AlignmentSelector::MAGNETOMETER; + else + exit_gracefully("Heading alignment selector out of range"); + } + + if (argc >= 7) + input_arguments.mip_binary_filepath = argv[6]; + + if (argc >= 8) + input_arguments.enable_pps_sync = std::stoi(argv[7]); + + return input_arguments; +} + + //////////////////////////////////////////////////////////////////////////////// // Print Usage Function //////////////////////////////////////////////////////////////////////////////// int usage(const char* argv0) { - printf("Usage: %s \n", argv0); + printf("Usage: %s [OPTIONAL, (0=Kinematic, 1=Magnetometer)] [OPTIONAL] [OPTIONAL, (bool, 0|1)] [OPTIONAL, (int, 1-4)] \n", argv0); return 1; } From 88b6ffde51190393d0d9071d0e9f2e57c5d41af2 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Wed, 11 Oct 2023 16:20:30 -0400 Subject: [PATCH 142/252] Changed minimum update rate --- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 1fbd08f5c..90a9caa19 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -470,7 +470,7 @@ int main(int argc, const char* argv[]) } mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; - if (elapsed_time_from_last_measurement_update > 500) + if (elapsed_time_from_last_measurement_update > 250) { printf("Sending measurement update...\n"); From d65434c1ca90c6778e23efd8ad4209ddaeaea821 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Wed, 11 Oct 2023 17:00:50 -0400 Subject: [PATCH 143/252] Updated defines to include MIP_USE_EXTRAS --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 24bf60d35..a9b380f6a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,6 +191,7 @@ if(MIP_USE_TCP) ) endif() if(MIP_USE_EXTRAS) + list(APPEND MIP_DEFINES "MIP_USE_EXTRAS") set(MIP_EXTRAS_DIR "${MIP_DIR}/extras") set(MIP_EXTRA_SOURCES "${MIP_EXTRAS_DIR}/recording_connection.hpp" From 767e59795b5a8786f1394403101bfad0e33ec437 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Thu, 12 Oct 2023 09:03:26 -0400 Subject: [PATCH 144/252] Added rough ubloc parser --- examples/CMakeLists.txt | 3 +- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 369 ++---------------- examples/CV7_INS/ublox_parser.h | 318 +++++++++++++++ 3 files changed, 350 insertions(+), 340 deletions(-) create mode 100644 examples/CV7_INS/ublox_parser.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index eda84bb73..1f9557157 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -51,7 +51,8 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(CV7_INS_Simple_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") - add_executable(CV7_INS_Simple_Ublox_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp" ${DEVICE_SOURCES}) + add_executable(CV7_INS_Simple_Ublox_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp" ${DEVICE_SOURCES} + CV7_INS/ublox_parser.h) target_link_libraries(CV7_INS_Simple_Ublox_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Ublox_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 90a9caa19..565634594 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -4,6 +4,7 @@ #include "mip/mip_all.hpp" #include "../example_utils.hpp" +#include "ublox_parser.h" #include #include #include @@ -23,84 +24,10 @@ data_filter::PositionLlh filter_llh_position; data_filter::VelocityNed filter_ned_velocity; data_system::TimeSyncStatus system_time_sync_status; -uint8_t external_heading_sensor_id = 1; -uint8_t gnss_antenna_sensor_id = 2; -uint8_t vehicle_frame_velocity_sensor_id = 3; -uint8_t header_size = 6; -uint8_t checksum_size = 2; -uint8_t payload_size = 92; -uint8_t PVT_message_size = header_size + checksum_size + payload_size; +uint8_t gnss_antenna_sensor_id = 1; bool filter_state_full_nav = false; -struct UBlox_PVT_Message { - uint32_t iTOW; - uint16_t utc_year; - uint8_t utc_month; - uint8_t utc_day; - uint8_t utc_hour; - uint8_t utc_minute; - double utc_second; - uint8_t lat_lon_valid_flag; - double nano_second; - uint8_t time_valid_flag; - double utc_time_accuracy; - double latitude; - double longitude; - uint8_t number_of_satellites; - float ned_velocity_uncertainty[3]; - double height_above_ellipsoid; - double horizontal_accuracy; - double vertical_accuracy; - float ned_velocity[3]; - float llh_uncertainty[3]; - double ground_speed; - double heading_of_motion_2d; - double heading_accuracy; - double heading_of_vehicle; - double magnetic_declination; - double magnetic_declination_accuracy; -}; - -struct UBlox_Payload_Part{ - uint8_t start_index; - uint8_t num_bytes; -}; - -std::map PAYLOAD_PART_MAP = { - {"iTOW", {0, 4}}, - {"YEAR", {4, 2}}, - {"MONTH", {6, 1}}, - {"DAY", {7, 1}}, - {"HOUR", {8, 1}}, - {"MIN", {9, 1}}, - {"SEC", {10, 1}}, - {"VALID", {11, 1}}, - {"T_ACC", {12, 4}}, - {"NANO_SEC", {16, 4}}, - {"FIX_TYPE", {20, 1}}, - {"FLAGS", {21, 1}}, - {"FLAGS_2", {22, 1}}, - {"NUM_SV", {23, 1}}, - {"LON", {24, 4}}, - {"LAT", {28, 4}}, - {"HEIGHT", {32, 4}}, - {"H_MSL", {36, 4}}, - {"H_ACC", {40, 4}}, - {"V_ACC", {44, 4}}, - {"VEL_N", {48, 4}}, - {"VEL_E", {52, 4}}, - {"VEL_D", {56, 4}}, - {"G_SPEED", {60, 4}}, - {"HEAD_MOT", {64, 4}}, - {"S_ACC", {68, 4}}, - {"HEAD_ACC", {72, 4}}, - {"FLAGS_3", {78, 1}}, - {"HEAD_VEH", {84, 4}}, - {"MAG_DEC", {88, 2}}, - {"MAG_ACC", {90, 2}} -}; - struct InputArguments { std::string mip_device_port_name; @@ -125,24 +52,6 @@ struct InputArguments // Function Prototypes //////////////////////////////////////////////////////////////////////////////// -std::unique_ptr parse_PVT_ublox_message(const uint8_t PVT_message[100]); - -template -void convert_mm_to_m(T& value); - -template -void convert_degrees_to_radians(T& degrees); - -float parse_2_bytes(const uint8_t payload[92], int start_index); - -double parse_4_bytes(const uint8_t payload[92], int start_index); - -double parse_long_lat(const uint8_t payload[92], int start_index); - -//HANDLE open_uBlox_serial_port(const char* com_port, const int baud_rate); - -bool verify_checksum(uint8_t *data); - int usage(const char* argv0); void print_device_information(const commands_base::BaseDeviceInfo& device_info); @@ -164,6 +73,14 @@ int main(int argc, const char* argv[]) std::unique_ptr utils = openFromArgs(input_arguments.mip_device_port_name, input_arguments.mip_device_baudrate, input_arguments.mip_binary_filepath); std::unique_ptr& device = utils->device; + + // + // Open uBlox serial port + // + + printf("Connecting to UBlox F9P on %s at %s...", input_arguments.ublox_device_port_name.c_str(), input_arguments.ublox_device_baudrate.c_str()); + std::unique_ptr utils_ublox = openFromArgs(input_arguments.ublox_device_port_name, input_arguments.ublox_device_baudrate, {}); + UbloxDevice ublox_device(std::move(utils_ublox->connection)); // //Attempt to idle the device before pinging @@ -186,13 +103,6 @@ int main(int argc, const char* argv[]) exit_gracefully("ERROR: Failed to get device info"); print_device_information(device_info); - // - // Open uBlox serial port - // - - printf("Connecting to UBlox F9P on %s at %s...", input_arguments.ublox_device_port_name.c_str(), input_arguments.ublox_device_baudrate.c_str()); - std::unique_ptr utils_ublox = openFromArgs(input_arguments.ublox_device_port_name, input_arguments.ublox_device_baudrate, {}); - // //Idle the device (note: this is good to do during setup) // @@ -333,22 +243,19 @@ int main(int argc, const char* argv[]) bool pps_sync_valid = false; mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); - - mip::Timeout wait_time = 3000; - size_t buffer_length = 1024; - uint8_t ublox_message_bytes[1024]; - size_t max_length = sizeof(ublox_message_bytes); - size_t read_out = 0; - size_t* length_out = &read_out; - mip::Timestamp timestamp; - uint8_t PVT_message[100]; printf("Sensor is configured... waiting for filter to initialize...\n"); while(running) { + // Spin MIP device device->update(); + // Get ublox data + std::pair ubox_parser_result = ublox_device.update(); + bool pvt_message_valid = ubox_parser_result.first; + UBlox_PVT_Message pvt_message = ubox_parser_result.second; + // Wait for valid PPS lock if (input_arguments.enable_pps_sync && !pps_sync_valid) { @@ -363,101 +270,18 @@ int main(int argc, const char* argv[]) } continue; } - - - std::unique_ptr ublox_message; - bool pvt_message_found = false; - std::memset(ublox_message_bytes, 0, sizeof(ublox_message_bytes)); - // Poll ublox receiver for PVT message ... - if (!utils_ublox->connection->recvFromDevice(ublox_message_bytes, max_length, wait_time, length_out, ×tamp)) { - exit_gracefully("ERROR: Error reading from serial port"); - } - - for (int i = buffer_length - 1 - 4; i >= 0; i--) { - - // look for 0xB5 - if (ublox_message_bytes[i] != 0xB5) - continue; - if (ublox_message_bytes[i+1] != 0x62) - continue; - if (ublox_message_bytes[i+2] != 0x01) - continue; - if (ublox_message_bytes[i+3] != 0x07) - continue; - - // If there are 100 bytes in buffer after (0xB5) is found and there were at least 100 bytes read - if (i + (PVT_message_size - 1) < buffer_length && (*length_out - i) >= PVT_message_size ) { - - // Extract payload - int payload_index = 0; - - for (int z = i; z < PVT_message_size + i; z++) { - PVT_message[payload_index] = ublox_message_bytes[z]; - payload_index += 1; - } - // If checksum valid - if (verify_checksum(PVT_message)) { - ublox_message = parse_PVT_ublox_message(PVT_message); - pvt_message_found = true; - // send parsed ublox_message to CV7 - break; - } else { - printf("Found packet, failed checksum verification"); - } - } - - size_t start_index = *length_out - i; - size_t package_bytes_remaining = PVT_message_size - start_index; - - while (*length_out == package_bytes_remaining) { - std::memmove(ublox_message_bytes, ublox_message_bytes + start_index, package_bytes_remaining); - std::memset(&ublox_message_bytes[*length_out], 0, buffer_length-*length_out); - - if (!utils_ublox->connection->recvFromDevice(&ublox_message_bytes[start_index], package_bytes_remaining, wait_time, length_out, ×tamp)) - exit_gracefully("ERROR: Error reading from serial port"); - - start_index += *length_out; - package_bytes_remaining = PVT_message_size - start_index; - } - - for (int i = 0; i < payload_size; i++) { - PVT_message[i] = ublox_message_bytes[i]; - } - - // check checksum - if (verify_checksum(PVT_message)) { - ublox_message = parse_PVT_ublox_message(PVT_message); - // send parsed ublox_message to CV7 - pvt_message_found = true; - break; - } else { - printf("Found packet, failed checksum verification"); - break; - } - } - - if (!pvt_message_found) - continue; - + //Check for full nav filter state transition if((!filter_state_full_nav) && (filter_status.filter_state == data_filter::FilterMode::FULL_NAV)) { printf("NOTE: Filter has entered full navigation mode.\n"); filter_state_full_nav = true; } - - // Check that enough time has elapsed to send a new measurement update + mip::Timestamp current_timestamp = getCurrentTimestamp(); - bool ublox_data_valid = ublox_message->time_valid_flag && ublox_message->lat_lon_valid_flag; - if (!ublox_data_valid) - { - printf("WARNING: Ublox data invalid"); - continue; - } - - mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; //Once in full nav, print out data at 1 Hz + mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; if((filter_status.filter_state == data_filter::FilterMode::FULL_NAV) && (elapsed_time_from_last_message_print >= 1000)) { printf("\n\n****Filter navigation state****\n"); @@ -469,8 +293,12 @@ int main(int argc, const char* argv[]) prev_print_timestamp = current_timestamp; } + // Check if measurement update is valid to send mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; - if (elapsed_time_from_last_measurement_update > 250) + bool ublox_data_valid = pvt_message_valid && pvt_message.time_valid_flag && pvt_message.lat_lon_valid_flag; + bool send_measurement_update = ublox_data_valid && elapsed_time_from_last_measurement_update > 250; + + if (send_measurement_update) { printf("Sending measurement update...\n"); @@ -478,13 +306,13 @@ int main(int argc, const char* argv[]) if (input_arguments.enable_pps_sync) { - // Update week number - uint32_t week_number = get_gps_week(ublox_message->utc_year, ublox_message->utc_month, ublox_message->utc_day); + // Send week number update to device + uint32_t week_number = get_gps_week(pvt_message.utc_year, pvt_message.utc_month, pvt_message.utc_day); if (!commands_base::writeGpsTimeUpdate(*device, commands_base::GpsTimeUpdate::FieldId::WEEK_NUMBER, week_number)) printf("WARNING: Failed to send week number time update to CV7-INS\n"); - // Update time of week - float time_of_week = float(ublox_message->iTOW) * 1e-3; + // Send time of week update to device + float time_of_week = float(pvt_message.iTOW) * 1e-3; uint32_t time_of_week_int = floor(time_of_week); if (!commands_base::writeGpsTimeUpdate(*device, commands_base::GpsTimeUpdate::FieldId::TIME_OF_WEEK, time_of_week_int)) printf("WARNING: Failed to send time of week update to CV7-INS\n"); @@ -499,11 +327,11 @@ int main(int argc, const char* argv[]) } // External position command - if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id,ublox_message->latitude, ublox_message->longitude,ublox_message->height_above_ellipsoid, ublox_message->llh_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id,pvt_message.latitude, pvt_message.longitude,pvt_message.height_above_ellipsoid, pvt_message.llh_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command - if (commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id,ublox_message->ned_velocity, ublox_message->ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id,pvt_message.ned_velocity, pvt_message.ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); prev_measurement_update_timestamp = current_timestamp; @@ -516,147 +344,10 @@ int main(int argc, const char* argv[]) } -//////////////////////////////////////////////////////////////////////////////// -// Verify UBX-NAV-PVT Checksum -//////////////////////////////////////////////////////////////////////////////// - -bool verify_checksum(uint8_t *data) { - unsigned i, j; - uint8_t ck_a, ck_b; - - j = ((unsigned)data[4] + ((unsigned)data[5] << 8) + 6); - - ck_a = 0; - ck_b = 0; - - for (i = 2; i < j; i++) { - ck_a += data[i]; - ck_b += ck_a; - } - - if (ck_a == data[i+0] && ck_b == data[i+1]) - return true; - - return false; -} - -//////////////////////////////////////////////////////////////////////////////// -// Parse UBX-NAV-PVT Message -//////////////////////////////////////////////////////////////////////////////// - -std::unique_ptr parse_PVT_ublox_message(const uint8_t PVT_message[]) { - - // TODO: Needs some refactoring but works!! - - std::unique_ptr ublox_message = std::make_unique(); - int payload_index = 0; - uint8_t payload[92]; - - // Extract payload - for (int i = header_size; i < (PVT_message_size - checksum_size); i++) { - payload[payload_index] = PVT_message[i]; - payload_index += 1; - } - - ublox_message->iTOW = parse_4_bytes(payload, PAYLOAD_PART_MAP["iTOW"].start_index); - - ublox_message->utc_year = parse_2_bytes(payload, PAYLOAD_PART_MAP["YEAR"].start_index); - - ublox_message->utc_month = payload[PAYLOAD_PART_MAP["MONTH"].start_index]; - - ublox_message->utc_day = payload[PAYLOAD_PART_MAP["DAY"].start_index]; - - ublox_message->utc_hour = payload[PAYLOAD_PART_MAP["HOUR"].start_index]; - - ublox_message->utc_minute = payload[PAYLOAD_PART_MAP["MIN"].start_index]; - - ublox_message->utc_second = payload[PAYLOAD_PART_MAP["SEC"].start_index]; - - ublox_message->time_valid_flag = payload[PAYLOAD_PART_MAP["VALID"].start_index]; - - ublox_message->utc_time_accuracy = parse_4_bytes(payload, PAYLOAD_PART_MAP["T_ACC"].start_index); - - ublox_message->nano_second = parse_4_bytes(payload, PAYLOAD_PART_MAP["NANO_SEC"].start_index); - - ublox_message->longitude = parse_long_lat(payload, PAYLOAD_PART_MAP["LON"].start_index); - - ublox_message->latitude = parse_long_lat(payload, PAYLOAD_PART_MAP["LAT"].start_index); - - ublox_message->height_above_ellipsoid = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEIGHT"].start_index); - convert_mm_to_m(ublox_message->height_above_ellipsoid); - - // TODO: Make sure UBlox values are mapped correctly here - double horizontal_uncertainty = parse_4_bytes(payload, PAYLOAD_PART_MAP["H_ACC"].start_index); - convert_mm_to_m(horizontal_uncertainty); - - ublox_message->llh_uncertainty[0] = horizontal_uncertainty; - ublox_message->llh_uncertainty[1] = horizontal_uncertainty; - - ublox_message->llh_uncertainty[2] = parse_4_bytes(payload, PAYLOAD_PART_MAP["V_ACC"].start_index); - convert_mm_to_m(ublox_message->llh_uncertainty[2]); - - ublox_message->ned_velocity[0] = parse_4_bytes(payload, PAYLOAD_PART_MAP["VEL_N"].start_index); - convert_mm_to_m(ublox_message->ned_velocity[0]); - - ublox_message->ned_velocity[1] = parse_4_bytes(payload, PAYLOAD_PART_MAP["VEL_E"].start_index); - convert_mm_to_m(ublox_message->ned_velocity[1]); - - ublox_message->ned_velocity[2] = parse_4_bytes(payload, PAYLOAD_PART_MAP["VEL_D"].start_index); - convert_mm_to_m(ublox_message->ned_velocity[2]); - - float speed_accuracy = parse_4_bytes(payload, PAYLOAD_PART_MAP["S_ACC"].start_index); - convert_mm_to_m(speed_accuracy); - for (int i = 0; i<3; i++) - ublox_message->ned_velocity_uncertainty[i] = speed_accuracy; - - ublox_message->heading_of_motion_2d = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_MOT"].start_index); - - ublox_message->heading_accuracy = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_ACC"].start_index) * 1e-5; - convert_degrees_to_radians(ublox_message->heading_accuracy); - - ublox_message->lat_lon_valid_flag = !payload[PAYLOAD_PART_MAP["FLAGS_3"].start_index]; - - ublox_message->heading_of_vehicle = parse_4_bytes(payload, PAYLOAD_PART_MAP["HEAD_VEH"].start_index); - convert_degrees_to_radians(ublox_message->heading_of_vehicle); - - return ublox_message; -} - - -//////////////////////////////////////////////////////////////////////////////// -// Parse 2 bytes -//////////////////////////////////////////////////////////////////////////////// - -float parse_2_bytes(const uint8_t payload[92], int start_index) { - return (float)(*((int16_t *)&payload[start_index])); -} - -//////////////////////////////////////////////////////////////////////////////// -// Parse 4 bytes -//////////////////////////////////////////////////////////////////////////////// - -double parse_4_bytes(const uint8_t payload[92], int start_index) { - return (double)(*((int32_t *)&payload[start_index])); -} - //////////////////////////////////////////////////////////////////////////////// // Utility functions //////////////////////////////////////////////////////////////////////////////// -double parse_long_lat(const uint8_t payload[92], int start_index) { - return (parse_4_bytes(payload, start_index) * 1e-7); -} - -template -void convert_mm_to_m(T& value) { - value = value / 1000.0; -} - -template -void convert_degrees_to_radians(T& degrees) { - degrees = degrees * (M_PI / 180.0); -} - uint64_t convert_gps_tow_to_nanoseconds(int week_number, float time_of_week) { return floor(float(week_number) * 604800 * 1e9 + time_of_week * 1e9); diff --git a/examples/CV7_INS/ublox_parser.h b/examples/CV7_INS/ublox_parser.h new file mode 100644 index 000000000..b941c406a --- /dev/null +++ b/examples/CV7_INS/ublox_parser.h @@ -0,0 +1,318 @@ +// +// Created by davidrobbins on 10/12/23. +// + +#ifndef MIP_SDK_UBLOX_PARSER_H +#define MIP_SDK_UBLOX_PARSER_H + +#include +#include +#include +#include +#include +#include +#include + +#include "mip/platform/serial_connection.hpp" + +#define PVT_PAYLOAD_SIZE 92 + +const int HEADER_SIZE = 6; +const int CHECKSUM_SIZE = 2; + +struct UBlox_PVT_Message { + uint32_t iTOW; + uint16_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + double utc_second; + uint8_t lat_lon_valid_flag; + double nano_second; + uint8_t time_valid_flag; + double utc_time_accuracy; + double latitude; + double longitude; + uint8_t number_of_satellites; + float ned_velocity_uncertainty[3]; + double height_above_ellipsoid; + double horizontal_accuracy; + double vertical_accuracy; + float ned_velocity[3]; + float llh_uncertainty[3]; + double ground_speed; + double heading_of_motion_2d; + double heading_accuracy; + double heading_of_vehicle; + double magnetic_declination; + double magnetic_declination_accuracy; +}; + +struct UBlox_Payload_Part { + uint8_t start_index; + uint8_t num_bytes; +}; + +constexpr UBlox_Payload_Part PAYLOAD_PART_ITOW = {0, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_YEAR = {4, 2}; +constexpr UBlox_Payload_Part PAYLOAD_PART_MONTH = {6, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_DAY = {7, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_HOUR = {8, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_MIN = {9, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_SEC = {10, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_VALID = {11, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_T_ACC = {12, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_NANO_SEC = {16, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_FIX_TYPE = {20, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS = {21, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS_2 = {22, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_NUM_SV = {23, 1}; +constexpr UBlox_Payload_Part PAYLOAD_PART_LON = {24, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_LAT = {28, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_HEIGHT = {32, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_H_MSL = {36, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_H_ACC = {40, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_V_ACC = {44, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_N = {48, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_E = {52, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_D = {56, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_G_SPEED = {60, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_MOT= {64, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_S_ACC = {68, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_ACC = {72, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS_3 = {78, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_VEH = {84, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_MAG_DEC= {88, 4}; +constexpr UBlox_Payload_Part PAYLOAD_PART_MAG_ACC = {90, 4}; + +bool verify_checksum(const std::vector& packet) +{ + uint8_t ck_a, ck_b; + + ck_a = 0; + ck_b = 0; + + int num_bytes = packet.size(); + int num_bytes_without_checksum = num_bytes-2; + + for (int i = 2; i < num_bytes_without_checksum; i++) { + ck_a += packet[i]; + ck_b += ck_a; + } + + if (ck_a == packet[num_bytes - 2] && ck_b == packet[num_bytes - 1]) + return true; + + return false; +} + +template +T parse_bytes(T data_type, const uint8_t* payload_start, int data_start_index) { + std::memcpy(&data_type, &payload_start[data_start_index], sizeof(data_type)); + return data_type; +} + +template +void convert_mm_to_m(T& value) { + value = value / 1000.0; +} + +template +void convert_degrees_to_radians(T& degrees) { + degrees = degrees * (M_PI / 180.0); +} + +UBlox_PVT_Message extract_pvt_message(const uint8_t payload[PVT_PAYLOAD_SIZE]) +{ + UBlox_PVT_Message ublox_message; + + int32_t four_bytes; + int16_t two_bytes; + + ublox_message.iTOW = parse_bytes(four_bytes, payload, PAYLOAD_PART_ITOW.start_index); + + ublox_message.utc_year = parse_bytes(two_bytes, payload, PAYLOAD_PART_YEAR.start_index); + + ublox_message.utc_month = payload[PAYLOAD_PART_MONTH.start_index]; + + ublox_message.utc_day = payload[PAYLOAD_PART_DAY.start_index]; + + ublox_message.utc_hour = payload[PAYLOAD_PART_HOUR.start_index]; + + ublox_message.utc_minute = payload[PAYLOAD_PART_MIN.start_index]; + + ublox_message.utc_second = payload[PAYLOAD_PART_SEC.start_index]; + + ublox_message.time_valid_flag = payload[PAYLOAD_PART_FLAGS.start_index]; + + ublox_message.utc_time_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_T_ACC.start_index); + + ublox_message.nano_second = parse_bytes(four_bytes, payload, PAYLOAD_PART_NANO_SEC.start_index); + + ublox_message.longitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LON.start_index) * 1e-7; + ublox_message.latitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LAT.start_index) * 1e-7; + + ublox_message.height_above_ellipsoid = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEIGHT.start_index); + convert_mm_to_m(ublox_message.height_above_ellipsoid); + + ublox_message.llh_uncertainty[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); + convert_mm_to_m(ublox_message.llh_uncertainty[0]); + + ublox_message.llh_uncertainty[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_H_ACC.start_index); + convert_mm_to_m(ublox_message.llh_uncertainty[1]); + + ublox_message.llh_uncertainty[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); + convert_mm_to_m(ublox_message.llh_uncertainty[2]); + + ublox_message.ned_velocity[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_N.start_index); + convert_mm_to_m(ublox_message.ned_velocity[0]); + + ublox_message.ned_velocity[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_E.start_index); + convert_mm_to_m(ublox_message.ned_velocity[1]); + + ublox_message.ned_velocity[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_D.start_index); + convert_mm_to_m(ublox_message.ned_velocity[2]); + + ublox_message.heading_of_motion_2d = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_MOT.start_index); + + ublox_message.heading_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_ACC.start_index) * 1e-5; + convert_degrees_to_radians(ublox_message.heading_accuracy); + + ublox_message.lat_lon_valid_flag = parse_bytes(four_bytes, payload, PAYLOAD_PART_FLAGS_3.start_index); + + ublox_message.heading_of_vehicle = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_VEH.start_index); + convert_degrees_to_radians(ublox_message.heading_of_vehicle); + + return ublox_message; +} + +class UbloxMessageParser +{ +public: + + UbloxMessageParser(std::function)> packet_callback) : _packet_callback(packet_callback) + {} + + void parse_bytes(uint8_t* buffer, size_t num_input_bytes) + { + // Copy into parser buffer + for (size_t i = 0; i= 2) + { + if (header_found()) + break; + + _buffer.pop_front(); + } + + // Check if header is valid + if (!header_found()) + return; + + // Check if buffer has full message header + if (_buffer.size() < 6) + return; + + // Get message length + uint8_t payload_length_bytes[2] = {_buffer[4], _buffer[5]}; + uint16_t payload_length; + memcpy(&payload_length, payload_length_bytes, sizeof(uint16_t)); + + int total_message_length = HEADER_SIZE + payload_length + CHECKSUM_SIZE; + + // Check if buffer contains full packet size + if (_buffer.size() < total_message_length) + return; + + // Extract packet + std::vector packet(total_message_length); + for (int i = 0; i)> _packet_callback; + + std::deque _buffer; +}; + + +class UbloxDevice +{ +public: + + UbloxDevice(std::unique_ptr connection) : _connection(std::move(connection)), + _message_parser([this](const std::vector& packet){ handle_packet(packet);}) + { + + } + + void handle_packet(const std::vector& packet) + { + bool is_pvt_message = (packet[2] == 0x01) && (packet[3] == 0x07); + if (!is_pvt_message) + return; + + // Should never happen + size_t expected_packet_size = HEADER_SIZE + PVT_PAYLOAD_SIZE + CHECKSUM_SIZE; + if (packet.size() != expected_packet_size) + return; + + // Extract message payload + uint8_t payload_bytes[PVT_PAYLOAD_SIZE]; + for (int i = 0; i < PVT_PAYLOAD_SIZE; i++) + payload_bytes[i] = packet[i + HEADER_SIZE]; + + _current_message = extract_pvt_message(payload_bytes); + _new_message_received = true; + } + + std::pair update() + { + _new_message_received = false; + + uint8_t input_bytes[1024]; + size_t num_input_bytes; + mip::Timestamp timestamp_out; + _connection->recvFromDevice(input_bytes, 1024, 1, &num_input_bytes, ×tamp_out); + + _message_parser.parse_bytes(input_bytes, num_input_bytes); + + return {_new_message_received, _current_message}; + } + +protected: + + std::unique_ptr _connection; + UbloxMessageParser _message_parser; + + bool _new_message_received = false; + UBlox_PVT_Message _current_message; +}; + +#endif //MIP_SDK_UBLOX_PARSER_H From 888c501ab693ae6166ac5dc5cac721a50e52fc96 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Thu, 12 Oct 2023 10:15:46 -0400 Subject: [PATCH 145/252] Split parser into separate file --- examples/CMakeLists.txt | 3 +- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 14 +- examples/CV7_INS/simple_ublox_parser.hpp | 119 +++++++ examples/CV7_INS/ublox_device.hpp | 220 ++++++++++++ examples/CV7_INS/ublox_parser.h | 318 ------------------ 5 files changed, 351 insertions(+), 323 deletions(-) create mode 100644 examples/CV7_INS/simple_ublox_parser.hpp create mode 100644 examples/CV7_INS/ublox_device.hpp delete mode 100644 examples/CV7_INS/ublox_parser.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1f9557157..3215250cf 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -52,7 +52,8 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_compile_definitions(CV7_INS_Simple_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") add_executable(CV7_INS_Simple_Ublox_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp" ${DEVICE_SOURCES} - CV7_INS/ublox_parser.h) + CV7_INS/ublox_device.hpp + CV7_INS/simple_ublox_parser.hpp) target_link_libraries(CV7_INS_Simple_Ublox_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Ublox_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 565634594..2cc5ba6fe 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -4,7 +4,7 @@ #include "mip/mip_all.hpp" #include "../example_utils.hpp" -#include "ublox_parser.h" +#include "ublox_device.hpp" #include #include #include @@ -80,7 +80,7 @@ int main(int argc, const char* argv[]) printf("Connecting to UBlox F9P on %s at %s...", input_arguments.ublox_device_port_name.c_str(), input_arguments.ublox_device_baudrate.c_str()); std::unique_ptr utils_ublox = openFromArgs(input_arguments.ublox_device_port_name, input_arguments.ublox_device_baudrate, {}); - UbloxDevice ublox_device(std::move(utils_ublox->connection)); + ublox::UbloxDevice ublox_device(std::move(utils_ublox->connection)); // //Attempt to idle the device before pinging @@ -252,9 +252,15 @@ int main(int argc, const char* argv[]) device->update(); // Get ublox data - std::pair ubox_parser_result = ublox_device.update(); + std::pair ubox_parser_result = ublox_device.update(); bool pvt_message_valid = ubox_parser_result.first; - UBlox_PVT_Message pvt_message = ubox_parser_result.second; + ublox::UBlox_PVT_Message pvt_message = ubox_parser_result.second; + + if (pvt_message_valid) + { + std::cout << pvt_message.latitude << std::endl; + } + continue; // Wait for valid PPS lock if (input_arguments.enable_pps_sync && !pps_sync_valid) diff --git a/examples/CV7_INS/simple_ublox_parser.hpp b/examples/CV7_INS/simple_ublox_parser.hpp new file mode 100644 index 000000000..d663ba60a --- /dev/null +++ b/examples/CV7_INS/simple_ublox_parser.hpp @@ -0,0 +1,119 @@ +// +// Created by davidrobbins on 10/12/23. +// + +#ifndef MIP_SDK_SIMPLE_UBLOX_PARSER_HPP +#define MIP_SDK_SIMPLE_UBLOX_PARSER_HPP + +#include +#include +#include +#include +#include + +const int HEADER_SIZE = 6; +const int CHECKSUM_SIZE = 2; + +namespace mip::ublox +{ + + bool verify_checksum(const std::vector& packet) + { + uint8_t ck_a, ck_b; + + ck_a = 0; + ck_b = 0; + + int num_bytes = packet.size(); + int num_bytes_without_checksum = num_bytes-2; + + for (int i = 2; i < num_bytes_without_checksum; i++) { + ck_a += packet[i]; + ck_b += ck_a; + } + + if (ck_a == packet[num_bytes - 2] && ck_b == packet[num_bytes - 1]) + return true; + + return false; + } + + + class UbloxMessageParser + { + public: + + UbloxMessageParser(std::function)> packet_callback) : _packet_callback(packet_callback) + {} + + void parse_bytes(uint8_t* buffer, size_t num_input_bytes) + { + // Copy into parser buffer + for (size_t i = 0; i= 2) + { + if (header_found()) + break; + + _buffer.pop_front(); + } + + // Check if header is valid + if (!header_found()) + return; + + // Check if buffer has full message header + if (_buffer.size() < 6) + return; + + // Get message length + uint8_t payload_length_bytes[2] = {_buffer[4], _buffer[5]}; + uint16_t payload_length; + memcpy(&payload_length, payload_length_bytes, sizeof(uint16_t)); + + int total_message_length = HEADER_SIZE + payload_length + CHECKSUM_SIZE; + + // Check if buffer contains full packet size + if (_buffer.size() < total_message_length) + return; + + // Extract packet + std::vector packet(total_message_length); + for (int i = 0; i)> _packet_callback; + + std::deque _buffer; + }; +} + + +#endif //MIP_SDK_SIMPLE_UBLOX_PARSER_HPP diff --git a/examples/CV7_INS/ublox_device.hpp b/examples/CV7_INS/ublox_device.hpp new file mode 100644 index 000000000..4afd6397a --- /dev/null +++ b/examples/CV7_INS/ublox_device.hpp @@ -0,0 +1,220 @@ +// +// Created by davidrobbins on 10/12/23. +// + +#ifndef MIP_SDK_UBLOX_DEVICE_HPP +#define MIP_SDK_UBLOX_DEVICE_HPP + +#include + +#include "simple_ublox_parser.hpp" + +#include "mip/platform/serial_connection.hpp" + +#define PVT_PAYLOAD_SIZE 92 + +namespace mip::ublox +{ + struct UBlox_PVT_Message { + uint32_t iTOW; + uint16_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + double utc_second; + uint8_t lat_lon_valid_flag; + double nano_second; + uint8_t time_valid_flag; + double utc_time_accuracy; + double latitude; + double longitude; + uint8_t number_of_satellites; + float ned_velocity_uncertainty[3]; + double height_above_ellipsoid; + double horizontal_accuracy; + double vertical_accuracy; + float ned_velocity[3]; + float llh_uncertainty[3]; + double ground_speed; + double heading_of_motion_2d; + double heading_accuracy; + double heading_of_vehicle; + double magnetic_declination; + double magnetic_declination_accuracy; + }; + + struct UBlox_Payload_Part { + uint8_t start_index; + uint8_t num_bytes; + }; + + constexpr UBlox_Payload_Part PAYLOAD_PART_ITOW = {0, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_YEAR = {4, 2}; + constexpr UBlox_Payload_Part PAYLOAD_PART_MONTH = {6, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_DAY = {7, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_HOUR = {8, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_MIN = {9, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_SEC = {10, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_VALID = {11, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_T_ACC = {12, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_NANO_SEC = {16, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_FIX_TYPE = {20, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS = {21, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS_2 = {22, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_NUM_SV = {23, 1}; + constexpr UBlox_Payload_Part PAYLOAD_PART_LON = {24, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_LAT = {28, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_HEIGHT = {32, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_H_MSL = {36, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_H_ACC = {40, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_V_ACC = {44, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_N = {48, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_E = {52, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_D = {56, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_G_SPEED = {60, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_MOT= {64, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_S_ACC = {68, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_ACC = {72, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS_3 = {78, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_VEH = {84, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_MAG_DEC= {88, 4}; + constexpr UBlox_Payload_Part PAYLOAD_PART_MAG_ACC = {90, 4}; + + + template + T parse_bytes(T data_type, const uint8_t* payload_start, int data_start_index) { + std::memcpy(&data_type, &payload_start[data_start_index], sizeof(data_type)); + return data_type; + } + + template + void convert_mm_to_m(T& value) { + value = value / 1000.0; + } + + template + void convert_degrees_to_radians(T& degrees) { + degrees = degrees * (M_PI / 180.0); + } + + UBlox_PVT_Message extract_pvt_message(const uint8_t payload[PVT_PAYLOAD_SIZE]) + { + UBlox_PVT_Message ublox_message; + + int32_t four_bytes; + int16_t two_bytes; + + ublox_message.iTOW = parse_bytes(four_bytes, payload, PAYLOAD_PART_ITOW.start_index); + + ublox_message.utc_year = parse_bytes(two_bytes, payload, PAYLOAD_PART_YEAR.start_index); + + ublox_message.utc_month = payload[PAYLOAD_PART_MONTH.start_index]; + + ublox_message.utc_day = payload[PAYLOAD_PART_DAY.start_index]; + + ublox_message.utc_hour = payload[PAYLOAD_PART_HOUR.start_index]; + + ublox_message.utc_minute = payload[PAYLOAD_PART_MIN.start_index]; + + ublox_message.utc_second = payload[PAYLOAD_PART_SEC.start_index]; + + ublox_message.time_valid_flag = payload[PAYLOAD_PART_FLAGS.start_index]; + + ublox_message.utc_time_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_T_ACC.start_index); + + ublox_message.nano_second = parse_bytes(four_bytes, payload, PAYLOAD_PART_NANO_SEC.start_index); + + ublox_message.longitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LON.start_index) * 1e-7; + ublox_message.latitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LAT.start_index) * 1e-7; + + ublox_message.height_above_ellipsoid = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEIGHT.start_index); + convert_mm_to_m(ublox_message.height_above_ellipsoid); + + ublox_message.llh_uncertainty[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); + convert_mm_to_m(ublox_message.llh_uncertainty[0]); + + ublox_message.llh_uncertainty[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_H_ACC.start_index); + convert_mm_to_m(ublox_message.llh_uncertainty[1]); + + ublox_message.llh_uncertainty[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); + convert_mm_to_m(ublox_message.llh_uncertainty[2]); + + ublox_message.ned_velocity[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_N.start_index); + convert_mm_to_m(ublox_message.ned_velocity[0]); + + ublox_message.ned_velocity[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_E.start_index); + convert_mm_to_m(ublox_message.ned_velocity[1]); + + ublox_message.ned_velocity[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_D.start_index); + convert_mm_to_m(ublox_message.ned_velocity[2]); + + ublox_message.heading_of_motion_2d = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_MOT.start_index); + + ublox_message.heading_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_ACC.start_index) * 1e-5; + convert_degrees_to_radians(ublox_message.heading_accuracy); + + ublox_message.lat_lon_valid_flag = parse_bytes(four_bytes, payload, PAYLOAD_PART_FLAGS_3.start_index); + + ublox_message.heading_of_vehicle = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_VEH.start_index); + convert_degrees_to_radians(ublox_message.heading_of_vehicle); + + return ublox_message; + } + + + class UbloxDevice + { + public: + + UbloxDevice(std::unique_ptr connection) : _connection(std::move(connection)), + _message_parser([this](const std::vector& packet){ handle_packet(packet);}) + {} + + void handle_packet(const std::vector& packet) + { + bool is_pvt_message = (packet[2] == 0x01) && (packet[3] == 0x07); + if (!is_pvt_message) + return; + + // Should never happen + size_t expected_packet_size = HEADER_SIZE + PVT_PAYLOAD_SIZE + CHECKSUM_SIZE; + if (packet.size() != expected_packet_size) + return; + + // Extract message payload + uint8_t payload_bytes[PVT_PAYLOAD_SIZE]; + for (int i = 0; i < PVT_PAYLOAD_SIZE; i++) + payload_bytes[i] = packet[i + HEADER_SIZE]; + + _current_message = extract_pvt_message(payload_bytes); + _new_message_received = true; + } + + std::pair update() + { + _new_message_received = false; + + uint8_t input_bytes[1024]; + size_t num_input_bytes; + mip::Timestamp timestamp_out; + _connection->recvFromDevice(input_bytes, 1024, 1, &num_input_bytes, ×tamp_out); + + _message_parser.parse_bytes(input_bytes, num_input_bytes); + + return {_new_message_received, _current_message}; + } + + protected: + + std::unique_ptr _connection; + UbloxMessageParser _message_parser; + + bool _new_message_received = false; + UBlox_PVT_Message _current_message; + }; +} + + + +#endif //MIP_SDK_UBLOX_DEVICE_HPP diff --git a/examples/CV7_INS/ublox_parser.h b/examples/CV7_INS/ublox_parser.h deleted file mode 100644 index b941c406a..000000000 --- a/examples/CV7_INS/ublox_parser.h +++ /dev/null @@ -1,318 +0,0 @@ -// -// Created by davidrobbins on 10/12/23. -// - -#ifndef MIP_SDK_UBLOX_PARSER_H -#define MIP_SDK_UBLOX_PARSER_H - -#include -#include -#include -#include -#include -#include -#include - -#include "mip/platform/serial_connection.hpp" - -#define PVT_PAYLOAD_SIZE 92 - -const int HEADER_SIZE = 6; -const int CHECKSUM_SIZE = 2; - -struct UBlox_PVT_Message { - uint32_t iTOW; - uint16_t utc_year; - uint8_t utc_month; - uint8_t utc_day; - uint8_t utc_hour; - uint8_t utc_minute; - double utc_second; - uint8_t lat_lon_valid_flag; - double nano_second; - uint8_t time_valid_flag; - double utc_time_accuracy; - double latitude; - double longitude; - uint8_t number_of_satellites; - float ned_velocity_uncertainty[3]; - double height_above_ellipsoid; - double horizontal_accuracy; - double vertical_accuracy; - float ned_velocity[3]; - float llh_uncertainty[3]; - double ground_speed; - double heading_of_motion_2d; - double heading_accuracy; - double heading_of_vehicle; - double magnetic_declination; - double magnetic_declination_accuracy; -}; - -struct UBlox_Payload_Part { - uint8_t start_index; - uint8_t num_bytes; -}; - -constexpr UBlox_Payload_Part PAYLOAD_PART_ITOW = {0, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_YEAR = {4, 2}; -constexpr UBlox_Payload_Part PAYLOAD_PART_MONTH = {6, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_DAY = {7, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_HOUR = {8, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_MIN = {9, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_SEC = {10, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_VALID = {11, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_T_ACC = {12, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_NANO_SEC = {16, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_FIX_TYPE = {20, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS = {21, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS_2 = {22, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_NUM_SV = {23, 1}; -constexpr UBlox_Payload_Part PAYLOAD_PART_LON = {24, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_LAT = {28, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_HEIGHT = {32, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_H_MSL = {36, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_H_ACC = {40, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_V_ACC = {44, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_N = {48, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_E = {52, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_VEL_D = {56, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_G_SPEED = {60, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_MOT= {64, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_S_ACC = {68, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_ACC = {72, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_FLAGS_3 = {78, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_HEAD_VEH = {84, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_MAG_DEC= {88, 4}; -constexpr UBlox_Payload_Part PAYLOAD_PART_MAG_ACC = {90, 4}; - -bool verify_checksum(const std::vector& packet) -{ - uint8_t ck_a, ck_b; - - ck_a = 0; - ck_b = 0; - - int num_bytes = packet.size(); - int num_bytes_without_checksum = num_bytes-2; - - for (int i = 2; i < num_bytes_without_checksum; i++) { - ck_a += packet[i]; - ck_b += ck_a; - } - - if (ck_a == packet[num_bytes - 2] && ck_b == packet[num_bytes - 1]) - return true; - - return false; -} - -template -T parse_bytes(T data_type, const uint8_t* payload_start, int data_start_index) { - std::memcpy(&data_type, &payload_start[data_start_index], sizeof(data_type)); - return data_type; -} - -template -void convert_mm_to_m(T& value) { - value = value / 1000.0; -} - -template -void convert_degrees_to_radians(T& degrees) { - degrees = degrees * (M_PI / 180.0); -} - -UBlox_PVT_Message extract_pvt_message(const uint8_t payload[PVT_PAYLOAD_SIZE]) -{ - UBlox_PVT_Message ublox_message; - - int32_t four_bytes; - int16_t two_bytes; - - ublox_message.iTOW = parse_bytes(four_bytes, payload, PAYLOAD_PART_ITOW.start_index); - - ublox_message.utc_year = parse_bytes(two_bytes, payload, PAYLOAD_PART_YEAR.start_index); - - ublox_message.utc_month = payload[PAYLOAD_PART_MONTH.start_index]; - - ublox_message.utc_day = payload[PAYLOAD_PART_DAY.start_index]; - - ublox_message.utc_hour = payload[PAYLOAD_PART_HOUR.start_index]; - - ublox_message.utc_minute = payload[PAYLOAD_PART_MIN.start_index]; - - ublox_message.utc_second = payload[PAYLOAD_PART_SEC.start_index]; - - ublox_message.time_valid_flag = payload[PAYLOAD_PART_FLAGS.start_index]; - - ublox_message.utc_time_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_T_ACC.start_index); - - ublox_message.nano_second = parse_bytes(four_bytes, payload, PAYLOAD_PART_NANO_SEC.start_index); - - ublox_message.longitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LON.start_index) * 1e-7; - ublox_message.latitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LAT.start_index) * 1e-7; - - ublox_message.height_above_ellipsoid = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEIGHT.start_index); - convert_mm_to_m(ublox_message.height_above_ellipsoid); - - ublox_message.llh_uncertainty[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); - convert_mm_to_m(ublox_message.llh_uncertainty[0]); - - ublox_message.llh_uncertainty[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_H_ACC.start_index); - convert_mm_to_m(ublox_message.llh_uncertainty[1]); - - ublox_message.llh_uncertainty[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); - convert_mm_to_m(ublox_message.llh_uncertainty[2]); - - ublox_message.ned_velocity[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_N.start_index); - convert_mm_to_m(ublox_message.ned_velocity[0]); - - ublox_message.ned_velocity[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_E.start_index); - convert_mm_to_m(ublox_message.ned_velocity[1]); - - ublox_message.ned_velocity[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_D.start_index); - convert_mm_to_m(ublox_message.ned_velocity[2]); - - ublox_message.heading_of_motion_2d = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_MOT.start_index); - - ublox_message.heading_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_ACC.start_index) * 1e-5; - convert_degrees_to_radians(ublox_message.heading_accuracy); - - ublox_message.lat_lon_valid_flag = parse_bytes(four_bytes, payload, PAYLOAD_PART_FLAGS_3.start_index); - - ublox_message.heading_of_vehicle = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_VEH.start_index); - convert_degrees_to_radians(ublox_message.heading_of_vehicle); - - return ublox_message; -} - -class UbloxMessageParser -{ -public: - - UbloxMessageParser(std::function)> packet_callback) : _packet_callback(packet_callback) - {} - - void parse_bytes(uint8_t* buffer, size_t num_input_bytes) - { - // Copy into parser buffer - for (size_t i = 0; i= 2) - { - if (header_found()) - break; - - _buffer.pop_front(); - } - - // Check if header is valid - if (!header_found()) - return; - - // Check if buffer has full message header - if (_buffer.size() < 6) - return; - - // Get message length - uint8_t payload_length_bytes[2] = {_buffer[4], _buffer[5]}; - uint16_t payload_length; - memcpy(&payload_length, payload_length_bytes, sizeof(uint16_t)); - - int total_message_length = HEADER_SIZE + payload_length + CHECKSUM_SIZE; - - // Check if buffer contains full packet size - if (_buffer.size() < total_message_length) - return; - - // Extract packet - std::vector packet(total_message_length); - for (int i = 0; i)> _packet_callback; - - std::deque _buffer; -}; - - -class UbloxDevice -{ -public: - - UbloxDevice(std::unique_ptr connection) : _connection(std::move(connection)), - _message_parser([this](const std::vector& packet){ handle_packet(packet);}) - { - - } - - void handle_packet(const std::vector& packet) - { - bool is_pvt_message = (packet[2] == 0x01) && (packet[3] == 0x07); - if (!is_pvt_message) - return; - - // Should never happen - size_t expected_packet_size = HEADER_SIZE + PVT_PAYLOAD_SIZE + CHECKSUM_SIZE; - if (packet.size() != expected_packet_size) - return; - - // Extract message payload - uint8_t payload_bytes[PVT_PAYLOAD_SIZE]; - for (int i = 0; i < PVT_PAYLOAD_SIZE; i++) - payload_bytes[i] = packet[i + HEADER_SIZE]; - - _current_message = extract_pvt_message(payload_bytes); - _new_message_received = true; - } - - std::pair update() - { - _new_message_received = false; - - uint8_t input_bytes[1024]; - size_t num_input_bytes; - mip::Timestamp timestamp_out; - _connection->recvFromDevice(input_bytes, 1024, 1, &num_input_bytes, ×tamp_out); - - _message_parser.parse_bytes(input_bytes, num_input_bytes); - - return {_new_message_received, _current_message}; - } - -protected: - - std::unique_ptr _connection; - UbloxMessageParser _message_parser; - - bool _new_message_received = false; - UBlox_PVT_Message _current_message; -}; - -#endif //MIP_SDK_UBLOX_PARSER_H From 39c4860fe2c90385cb7622b83240a2ce465a6a40 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Thu, 12 Oct 2023 10:23:24 -0400 Subject: [PATCH 146/252] Added documentation --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 39 ++++++++++++------- 1 file changed, 25 insertions(+), 14 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 2cc5ba6fe..e8087b278 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -1,3 +1,24 @@ +///////////////////////////////////////////////////////////////////////////// +// +// CV7_INS_simple_ublox_example.cpp +// +// C++ Example usage program for the CV7-INS with a UBlox receiver +// +// This example shows the basic interface between the CV7-INS and a UBlox receiver configured to stream a the UBX-NAV-PVT message. +// It is intended to demonstrate the relevant MIP API calls to input GNSS data to the CV7-INS +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + + //////////////////////////////////////////////////////////////////////////////// // Include Files //////////////////////////////////////////////////////////////////////////////// @@ -46,8 +67,6 @@ struct InputArguments float gnss_antenna_lever_arm[3] = {0,0,0}; }; - - //////////////////////////////////////////////////////////////////////////////// // Function Prototypes //////////////////////////////////////////////////////////////////////////////// @@ -65,7 +84,6 @@ uint64_t convert_gps_tow_to_nanoseconds(int week_number, float time_of_week); int get_gps_week(int year, int month, int day); - int main(int argc, const char* argv[]) { @@ -201,8 +219,6 @@ int main(int argc, const char* argv[]) } - - // //Reset the filter (note: this is good to do after filter setup is complete) // @@ -247,6 +263,7 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to initialize...\n"); while(running) { + mip::Timestamp current_timestamp = getCurrentTimestamp(); // Spin MIP device device->update(); @@ -255,19 +272,12 @@ int main(int argc, const char* argv[]) std::pair ubox_parser_result = ublox_device.update(); bool pvt_message_valid = ubox_parser_result.first; ublox::UBlox_PVT_Message pvt_message = ubox_parser_result.second; - - if (pvt_message_valid) - { - std::cout << pvt_message.latitude << std::endl; - } - continue; // Wait for valid PPS lock if (input_arguments.enable_pps_sync && !pps_sync_valid) { pps_sync_valid = system_time_sync_status.time_sync; - mip::Timestamp current_timestamp = getCurrentTimestamp(); mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; if (elapsed_time_from_last_message_print > 1000) { @@ -283,8 +293,6 @@ int main(int argc, const char* argv[]) printf("NOTE: Filter has entered full navigation mode.\n"); filter_state_full_nav = true; } - - mip::Timestamp current_timestamp = getCurrentTimestamp(); //Once in full nav, print out data at 1 Hz mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; @@ -405,6 +413,9 @@ void print_device_information(const commands_base::BaseDeviceInfo& device_info) ); } +//////////////////////////////////////////////////////////////////////////////// +// Parse input arguments +//////////////////////////////////////////////////////////////////////////////// InputArguments parse_input_arguments(int argc, const char* argv[]) { From 49a3383f6e9d5702fe62328899f909b71336f901 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Thu, 12 Oct 2023 11:25:42 -0400 Subject: [PATCH 147/252] Cleaned up code --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 29 ++++++++----- examples/CV7_INS/simple_ublox_parser.hpp | 24 ++++++++--- examples/CV7_INS/ublox_device.hpp | 41 +++++++++++++------ 3 files changed, 64 insertions(+), 30 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index e8087b278..ea967283e 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -4,7 +4,7 @@ // // C++ Example usage program for the CV7-INS with a UBlox receiver // -// This example shows the basic interface between the CV7-INS and a UBlox receiver configured to stream a the UBX-NAV-PVT message. +// This example shows the basic interface between the CV7-INS and a UBlox receiver preconfigured to stream a the UBX-NAV-PVT message. // It is intended to demonstrate the relevant MIP API calls to input GNSS data to the CV7-INS // //!@section LICENSE @@ -86,7 +86,6 @@ int get_gps_week(int year, int month, int day); int main(int argc, const char* argv[]) { - InputArguments input_arguments = parse_input_arguments(argc, argv); std::unique_ptr utils = openFromArgs(input_arguments.mip_device_port_name, input_arguments.mip_device_baudrate, input_arguments.mip_binary_filepath); @@ -294,15 +293,21 @@ int main(int argc, const char* argv[]) filter_state_full_nav = true; } - //Once in full nav, print out data at 1 Hz + //Print status at 1Hz mip::Timestamp elapsed_time_from_last_message_print = current_timestamp - prev_print_timestamp; - if((filter_status.filter_state == data_filter::FilterMode::FULL_NAV) && (elapsed_time_from_last_message_print >= 1000)) + bool print_new_update_message = elapsed_time_from_last_message_print >= 1000; + if (print_new_update_message) { - printf("\n\n****Filter navigation state****\n"); - printf("TIMESTAMP: %f\n", filter_gps_time.tow); - printf("ATTITUDE_EULER = [%f %f %f]\n", filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw); - printf("LLH_POSITION = [%f %f %f]\n", filter_llh_position.latitude, filter_llh_position.longitude, filter_llh_position.ellipsoid_height); - printf("NED_VELOCITY = [%f %f %f]\n", filter_ned_velocity.north, filter_ned_velocity.east, filter_ned_velocity.down); + if(filter_status.filter_state == data_filter::FilterMode::FULL_NAV) + { + printf("\n\n****Filter navigation state****\n"); + printf("TIMESTAMP: %f\n", filter_gps_time.tow); + printf("ATTITUDE_EULER = [%f %f %f]\n", filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw); + printf("LLH_POSITION = [%f %f %f]\n", filter_llh_position.latitude, filter_llh_position.longitude, filter_llh_position.ellipsoid_height); + printf("NED_VELOCITY = [%f %f %f]\n", filter_ned_velocity.north, filter_ned_velocity.east, filter_ned_velocity.down); + } + else + printf("Waiting for navigation filter to initialize...\n"); prev_print_timestamp = current_timestamp; } @@ -314,7 +319,9 @@ int main(int argc, const char* argv[]) if (send_measurement_update) { - printf("Sending measurement update...\n"); + printf("\n\n****Sending Measurement Update****\n"); + printf("LLH_POSITION_GNSS_MEAS = [%f %f %f]\n", pvt_message.latitude, pvt_message.longitude, pvt_message.height_above_ellipsoid); + printf("NED_VELOCITY_GNSS_MEAS = [%f %f %f]\n", pvt_message.ned_velocity[0], pvt_message.ned_velocity[1], pvt_message.ned_velocity[2]); commands_aiding::Time external_measurement_time; @@ -341,7 +348,7 @@ int main(int argc, const char* argv[]) } // External position command - if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id,pvt_message.latitude, pvt_message.longitude,pvt_message.height_above_ellipsoid, pvt_message.llh_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, pvt_message.latitude, pvt_message.longitude,pvt_message.height_above_ellipsoid, pvt_message.llh_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command diff --git a/examples/CV7_INS/simple_ublox_parser.hpp b/examples/CV7_INS/simple_ublox_parser.hpp index d663ba60a..06338f6ff 100644 --- a/examples/CV7_INS/simple_ublox_parser.hpp +++ b/examples/CV7_INS/simple_ublox_parser.hpp @@ -1,9 +1,24 @@ +///////////////////////////////////////////////////////////////////////////// // -// Created by davidrobbins on 10/12/23. +// simple_ublox_parser.hpp // +// Basic UBlox binary message parser +// +// This class intends to be a simple helper utility for an example to demonstrate CV7-INS functionality and is not intended +// to be reused for any other application +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// -#ifndef MIP_SDK_SIMPLE_UBLOX_PARSER_HPP -#define MIP_SDK_SIMPLE_UBLOX_PARSER_HPP +#pragma once #include #include @@ -114,6 +129,3 @@ namespace mip::ublox std::deque _buffer; }; } - - -#endif //MIP_SDK_SIMPLE_UBLOX_PARSER_HPP diff --git a/examples/CV7_INS/ublox_device.hpp b/examples/CV7_INS/ublox_device.hpp index 4afd6397a..9b2c5b8df 100644 --- a/examples/CV7_INS/ublox_device.hpp +++ b/examples/CV7_INS/ublox_device.hpp @@ -1,9 +1,24 @@ +///////////////////////////////////////////////////////////////////////////// // -// Created by davidrobbins on 10/12/23. +// ublox_device.hpp // +// Basic UBlox serial device interface to parse out the UBlox UBX-NAV-PVT message from a serial port +// +// This class intends to be a simple helper utility for an example to demonstrate CV7-INS functionality and is not intended +// to be reused for any other application +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// -#ifndef MIP_SDK_UBLOX_DEVICE_HPP -#define MIP_SDK_UBLOX_DEVICE_HPP +#pragma once #include @@ -131,11 +146,11 @@ namespace mip::ublox ublox_message.height_above_ellipsoid = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEIGHT.start_index); convert_mm_to_m(ublox_message.height_above_ellipsoid); - ublox_message.llh_uncertainty[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); - convert_mm_to_m(ublox_message.llh_uncertainty[0]); + float horizontal_uncertainty = parse_bytes(four_bytes, payload, PAYLOAD_PART_H_ACC.start_index); + convert_mm_to_m(horizontal_uncertainty); - ublox_message.llh_uncertainty[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_H_ACC.start_index); - convert_mm_to_m(ublox_message.llh_uncertainty[1]); + ublox_message.llh_uncertainty[0] = horizontal_uncertainty; + ublox_message.llh_uncertainty[1] = horizontal_uncertainty; ublox_message.llh_uncertainty[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); convert_mm_to_m(ublox_message.llh_uncertainty[2]); @@ -149,12 +164,16 @@ namespace mip::ublox ublox_message.ned_velocity[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_D.start_index); convert_mm_to_m(ublox_message.ned_velocity[2]); - ublox_message.heading_of_motion_2d = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_MOT.start_index); + float speed_uncertainty = parse_bytes(four_bytes, payload, PAYLOAD_PART_S_ACC.start_index); + convert_mm_to_m(speed_uncertainty); + for (int i =0; i<3; i++) + ublox_message.ned_velocity_uncertainty[i] = speed_uncertainty; ublox_message.heading_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_ACC.start_index) * 1e-5; convert_degrees_to_radians(ublox_message.heading_accuracy); ublox_message.lat_lon_valid_flag = parse_bytes(four_bytes, payload, PAYLOAD_PART_FLAGS_3.start_index); + ublox_message.lat_lon_valid_flag = !ublox_message.lat_lon_valid_flag; ublox_message.heading_of_vehicle = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_VEH.start_index); convert_degrees_to_radians(ublox_message.heading_of_vehicle); @@ -213,8 +232,4 @@ namespace mip::ublox bool _new_message_received = false; UBlox_PVT_Message _current_message; }; -} - - - -#endif //MIP_SDK_UBLOX_DEVICE_HPP +} \ No newline at end of file From 6c7d6b9700e19f9508f3e37a35409b00082e0cb0 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Thu, 12 Oct 2023 14:27:09 -0400 Subject: [PATCH 148/252] Added GNSS antenna lever arm program arguments --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 72 ++++++++++++------- 1 file changed, 46 insertions(+), 26 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index ea967283e..05c9bd74e 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -159,23 +159,23 @@ int main(int argc, const char* argv[]) //Setup FILTER data format // - uint16_t filter_base_rate; - if(commands_3dm::getBaseRate(*device, data_filter::DESCRIPTOR_SET, &filter_base_rate) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not get filter base rate format!"); - - const uint16_t filter_sample_rate = 10; // Hz - const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; - - std::array filter_descriptors = {{ - { data_shared::DATA_GPS_TIME, filter_decimation }, - { data_filter::DATA_FILTER_STATUS, filter_decimation }, - { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, - { data_filter::DATA_POS_LLH, filter_decimation }, - { data_filter::DATA_VEL_NED, filter_decimation }, - }}; - - if(commands_3dm::writeMessageFormat(*device, data_filter::DESCRIPTOR_SET, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) - exit_gracefully("ERROR: Could not set filter message format!"); +// uint16_t filter_base_rate; +// if(commands_3dm::getBaseRate(*device, data_filter::DESCRIPTOR_SET, &filter_base_rate) != CmdResult::ACK_OK) +// exit_gracefully("ERROR: Could not get filter base rate format!"); +// +// const uint16_t filter_sample_rate = 10; // Hz +// const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; +// +// std::array filter_descriptors = {{ +// { data_shared::DATA_GPS_TIME, filter_decimation }, +// { data_filter::DATA_FILTER_STATUS, filter_decimation }, +// { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, +// { data_filter::DATA_POS_LLH, filter_decimation }, +// { data_filter::DATA_VEL_NED, filter_decimation }, +// }}; +// +// if(commands_3dm::writeMessageFormat(*device, data_filter::DESCRIPTOR_SET, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) +// exit_gracefully("ERROR: Could not set filter message format!"); if (input_arguments.enable_pps_sync) @@ -218,6 +218,13 @@ int main(int argc, const char* argv[]) } + // + //Configure factory streaming data + // + + if(commands_3dm::factoryStreaming(*device, commands_3dm::FactoryStreaming::Action::MERGE, 0) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not enable factory streaming support!"); + // //Reset the filter (note: this is good to do after filter setup is complete) // @@ -327,7 +334,6 @@ int main(int argc, const char* argv[]) if (input_arguments.enable_pps_sync) { - // Send week number update to device uint32_t week_number = get_gps_week(pvt_message.utc_year, pvt_message.utc_month, pvt_message.utc_day); if (!commands_base::writeGpsTimeUpdate(*device, commands_base::GpsTimeUpdate::FieldId::WEEK_NUMBER, week_number)) printf("WARNING: Failed to send week number time update to CV7-INS\n"); @@ -426,7 +432,7 @@ void print_device_information(const commands_base::BaseDeviceInfo& device_info) InputArguments parse_input_arguments(int argc, const char* argv[]) { - if (argc < 5) + if (argc < 8) { usage(argv[0]); exit_gracefully("ERROR: Incorrect input arguments"); @@ -444,15 +450,23 @@ InputArguments parse_input_arguments(int argc, const char* argv[]) InputArguments input_arguments; + // MIP device port parameters input_arguments.mip_device_port_name = argv[1]; input_arguments.mip_device_baudrate = argv[2]; + // UBlox device port parameters input_arguments.ublox_device_port_name = argv[3]; input_arguments.ublox_device_baudrate = argv[4]; - if (argc >= 6) + // GNSS antenna lever arm + input_arguments.gnss_antenna_lever_arm[0] = std::stof(argv[5]); + input_arguments.gnss_antenna_lever_arm[1] = std::stof(argv[6]); + input_arguments.gnss_antenna_lever_arm[2] = std::stof(argv[7]); + + // Heading alignment method + if (argc >= 9) { - int heading_alignment_int = std::stoi(argv[5]); + int heading_alignment_int = std::stoi(argv[8]); if (heading_alignment_int == 0) input_arguments.filter_heading_alignment_method = commands_filter::InitializationConfiguration::AlignmentSelector::KINEMATIC; @@ -462,11 +476,17 @@ InputArguments parse_input_arguments(int argc, const char* argv[]) exit_gracefully("Heading alignment selector out of range"); } - if (argc >= 7) - input_arguments.mip_binary_filepath = argv[6]; + // Output binary data filepath + if (argc >= 10) + input_arguments.mip_binary_filepath = argv[9]; + + // PPS sync enable + if (argc >= 11) + input_arguments.enable_pps_sync = std::stoi(argv[10]); - if (argc >= 8) - input_arguments.enable_pps_sync = std::stoi(argv[7]); + // PPS input pin ID + if (argc >= 12) + input_arguments.pps_input_pin_id = std::stoi(argv[11]); return input_arguments; } @@ -478,7 +498,7 @@ InputArguments parse_input_arguments(int argc, const char* argv[]) int usage(const char* argv0) { - printf("Usage: %s [OPTIONAL, (0=Kinematic, 1=Magnetometer)] [OPTIONAL] [OPTIONAL, (bool, 0|1)] [OPTIONAL, (int, 1-4)] \n", argv0); + printf("Usage: %s [OPTIONAL, (0=Kinematic, 1=Magnetometer)] [OPTIONAL] [OPTIONAL, (bool, (0|1)] [OPTIONAL, (int, 1-4)] \n", argv0); return 1; } From 9ce90e513e18731484ffa931a9f535d303696ceb Mon Sep 17 00:00:00 2001 From: David Robbins Date: Fri, 13 Oct 2023 09:56:43 -0400 Subject: [PATCH 149/252] Cleaned up UBlox message parsing --- examples/CV7_INS/CV7_INS_simple_example.cpp | 4 +- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 50 ++-- examples/CV7_INS/simple_ublox_parser.hpp | 6 +- examples/CV7_INS/ublox_device.hpp | 229 +++++++----------- 4 files changed, 109 insertions(+), 180 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 987cf646f..b4e6bb1fa 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -4,7 +4,7 @@ // // C++ Example usage program for the CV7-INS // -// This example shows a the basic setup for a CV-INS sensor using external aiding measurements. +// This example shows the basic setup for a CV-INS sensor using external aiding measurements. // It is not an exhaustive example of all CV7 settings. // If your specific setup needs are not met by this example, please consult // the MSCL-embedded API documentation for the proper commands. @@ -14,7 +14,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 05c9bd74e..f97809e30 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -11,7 +11,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. @@ -30,7 +30,7 @@ #include #include #include -#include +#include using namespace mip; @@ -95,7 +95,7 @@ int main(int argc, const char* argv[]) // Open uBlox serial port // - printf("Connecting to UBlox F9P on %s at %s...", input_arguments.ublox_device_port_name.c_str(), input_arguments.ublox_device_baudrate.c_str()); + printf("Connecting to UBlox F9P on %s at %s...\n", input_arguments.ublox_device_port_name.c_str(), input_arguments.ublox_device_baudrate.c_str()); std::unique_ptr utils_ublox = openFromArgs(input_arguments.ublox_device_port_name, input_arguments.ublox_device_baudrate, {}); ublox::UbloxDevice ublox_device(std::move(utils_ublox->connection)); @@ -155,29 +155,6 @@ int main(int argc, const char* argv[]) exit_gracefully("ERROR: Could not load default device settings!"); - // - //Setup FILTER data format - // - -// uint16_t filter_base_rate; -// if(commands_3dm::getBaseRate(*device, data_filter::DESCRIPTOR_SET, &filter_base_rate) != CmdResult::ACK_OK) -// exit_gracefully("ERROR: Could not get filter base rate format!"); -// -// const uint16_t filter_sample_rate = 10; // Hz -// const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; -// -// std::array filter_descriptors = {{ -// { data_shared::DATA_GPS_TIME, filter_decimation }, -// { data_filter::DATA_FILTER_STATUS, filter_decimation }, -// { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, -// { data_filter::DATA_POS_LLH, filter_decimation }, -// { data_filter::DATA_VEL_NED, filter_decimation }, -// }}; -// -// if(commands_3dm::writeMessageFormat(*device, data_filter::DESCRIPTOR_SET, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) -// exit_gracefully("ERROR: Could not set filter message format!"); - - if (input_arguments.enable_pps_sync) { @@ -219,7 +196,7 @@ int main(int argc, const char* argv[]) } // - //Configure factory streaming data + //Configure factory streaming data. This enables all critical data channels required for post-processing analysis // if(commands_3dm::factoryStreaming(*device, commands_3dm::FactoryStreaming::Action::MERGE, 0) != CmdResult::ACK_OK) @@ -275,9 +252,9 @@ int main(int argc, const char* argv[]) device->update(); // Get ublox data - std::pair ubox_parser_result = ublox_device.update(); + std::pair ubox_parser_result = ublox_device.update(); bool pvt_message_valid = ubox_parser_result.first; - ublox::UBlox_PVT_Message pvt_message = ubox_parser_result.second; + ublox::UbloxPVTMessage pvt_message = ubox_parser_result.second; // Wait for valid PPS lock if (input_arguments.enable_pps_sync && !pps_sync_valid) @@ -321,8 +298,8 @@ int main(int argc, const char* argv[]) // Check if measurement update is valid to send mip::Timestamp elapsed_time_from_last_measurement_update = current_timestamp - prev_measurement_update_timestamp; - bool ublox_data_valid = pvt_message_valid && pvt_message.time_valid_flag && pvt_message.lat_lon_valid_flag; - bool send_measurement_update = ublox_data_valid && elapsed_time_from_last_measurement_update > 250; + bool ublox_data_valid = pvt_message_valid && pvt_message.time_valid && pvt_message.llh_position_valid; + bool send_measurement_update = ublox_data_valid && elapsed_time_from_last_measurement_update > 250; // Cap maximum GNSS measurement input rate to 4hz if (send_measurement_update) { @@ -334,27 +311,28 @@ int main(int argc, const char* argv[]) if (input_arguments.enable_pps_sync) { + // Send week number update to device uint32_t week_number = get_gps_week(pvt_message.utc_year, pvt_message.utc_month, pvt_message.utc_day); if (!commands_base::writeGpsTimeUpdate(*device, commands_base::GpsTimeUpdate::FieldId::WEEK_NUMBER, week_number)) printf("WARNING: Failed to send week number time update to CV7-INS\n"); // Send time of week update to device - float time_of_week = float(pvt_message.iTOW) * 1e-3; - uint32_t time_of_week_int = floor(time_of_week); + uint32_t time_of_week_int = floor(pvt_message.time_of_week); if (!commands_base::writeGpsTimeUpdate(*device, commands_base::GpsTimeUpdate::FieldId::TIME_OF_WEEK, time_of_week_int)) printf("WARNING: Failed to send time of week update to CV7-INS\n"); + // Mark timestamp for aiding message input external_measurement_time.timebase = commands_aiding::Time::Timebase::EXTERNAL_TIME; - external_measurement_time.nanoseconds = convert_gps_tow_to_nanoseconds(week_number, time_of_week); + external_measurement_time.nanoseconds = convert_gps_tow_to_nanoseconds(week_number, pvt_message.time_of_week); } else { - // If no PPS sync is supplied use device time of arrival for data timestamping method + // If no PPS sync is supplied, use device time of arrival for data timestamping method external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; } // External position command - if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, pvt_message.latitude, pvt_message.longitude,pvt_message.height_above_ellipsoid, pvt_message.llh_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, pvt_message.latitude, pvt_message.longitude, pvt_message.height_above_ellipsoid, pvt_message.llh_position_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command diff --git a/examples/CV7_INS/simple_ublox_parser.hpp b/examples/CV7_INS/simple_ublox_parser.hpp index 06338f6ff..f1eb9e2fd 100644 --- a/examples/CV7_INS/simple_ublox_parser.hpp +++ b/examples/CV7_INS/simple_ublox_parser.hpp @@ -11,7 +11,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. @@ -105,13 +105,15 @@ namespace mip::ublox // Validate checksum if (verify_checksum(packet)) { + // Call packet callback _packet_callback(packet); + + // Clear packet from buffer for (int i = 0; i - T parse_bytes(T data_type, const uint8_t* payload_start, int data_start_index) { - std::memcpy(&data_type, &payload_start[data_start_index], sizeof(data_type)); - return data_type; - } - - template - void convert_mm_to_m(T& value) { - value = value / 1000.0; - } - - template - void convert_degrees_to_radians(T& degrees) { - degrees = degrees * (M_PI / 180.0); - } - UBlox_PVT_Message extract_pvt_message(const uint8_t payload[PVT_PAYLOAD_SIZE]) + struct UbloxPVTMessage { - UBlox_PVT_Message ublox_message; - - int32_t four_bytes; - int16_t two_bytes; - - ublox_message.iTOW = parse_bytes(four_bytes, payload, PAYLOAD_PART_ITOW.start_index); - - ublox_message.utc_year = parse_bytes(two_bytes, payload, PAYLOAD_PART_YEAR.start_index); - - ublox_message.utc_month = payload[PAYLOAD_PART_MONTH.start_index]; - - ublox_message.utc_day = payload[PAYLOAD_PART_DAY.start_index]; - - ublox_message.utc_hour = payload[PAYLOAD_PART_HOUR.start_index]; - - ublox_message.utc_minute = payload[PAYLOAD_PART_MIN.start_index]; - - ublox_message.utc_second = payload[PAYLOAD_PART_SEC.start_index]; - - ublox_message.time_valid_flag = payload[PAYLOAD_PART_FLAGS.start_index]; - - ublox_message.utc_time_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_T_ACC.start_index); - - ublox_message.nano_second = parse_bytes(four_bytes, payload, PAYLOAD_PART_NANO_SEC.start_index); - - ublox_message.longitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LON.start_index) * 1e-7; - ublox_message.latitude = parse_bytes(four_bytes, payload, PAYLOAD_PART_LAT.start_index) * 1e-7; - - ublox_message.height_above_ellipsoid = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEIGHT.start_index); - convert_mm_to_m(ublox_message.height_above_ellipsoid); - - float horizontal_uncertainty = parse_bytes(four_bytes, payload, PAYLOAD_PART_H_ACC.start_index); - convert_mm_to_m(horizontal_uncertainty); - - ublox_message.llh_uncertainty[0] = horizontal_uncertainty; - ublox_message.llh_uncertainty[1] = horizontal_uncertainty; - - ublox_message.llh_uncertainty[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_V_ACC.start_index); - convert_mm_to_m(ublox_message.llh_uncertainty[2]); - - ublox_message.ned_velocity[0] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_N.start_index); - convert_mm_to_m(ublox_message.ned_velocity[0]); - - ublox_message.ned_velocity[1] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_E.start_index); - convert_mm_to_m(ublox_message.ned_velocity[1]); - - ublox_message.ned_velocity[2] = parse_bytes(four_bytes, payload, PAYLOAD_PART_VEL_D.start_index); - convert_mm_to_m(ublox_message.ned_velocity[2]); - - float speed_uncertainty = parse_bytes(four_bytes, payload, PAYLOAD_PART_S_ACC.start_index); - convert_mm_to_m(speed_uncertainty); - for (int i =0; i<3; i++) - ublox_message.ned_velocity_uncertainty[i] = speed_uncertainty; - - ublox_message.heading_accuracy = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_ACC.start_index) * 1e-5; - convert_degrees_to_radians(ublox_message.heading_accuracy); + // Time + uint16_t utc_year = 0; + uint8_t utc_month = 0; + uint8_t utc_day = 0; + float time_of_week = 0; + bool time_valid = false; + + // LLH position + double latitude = 0; + double longitude = 0; + double height_above_ellipsoid = 0; + float llh_position_uncertainty[3] = {0,0,0}; + bool llh_position_valid = false; + + // NED velocity + float ned_velocity[3] = {0,0,0}; + float ned_velocity_uncertainty[3] = {0,0,0}; + }; - ublox_message.lat_lon_valid_flag = parse_bytes(four_bytes, payload, PAYLOAD_PART_FLAGS_3.start_index); - ublox_message.lat_lon_valid_flag = !ublox_message.lat_lon_valid_flag; - ublox_message.heading_of_vehicle = parse_bytes(four_bytes, payload, PAYLOAD_PART_HEAD_VEH.start_index); - convert_degrees_to_radians(ublox_message.heading_of_vehicle); + UbloxPVTMessage extract_pvt_message(const uint8_t payload[PVT_PAYLOAD_SIZE]) + { + // Unpack raw UBlox message data + UbloxPVTMessageRaw ublox_message_raw; + std::memcpy(&ublox_message_raw, payload, sizeof(ublox_message_raw)); + + // Build output message with properly scaled units + UbloxPVTMessage ublox_message; + + // Time + ublox_message.utc_year = ublox_message_raw.utc_year; + ublox_message.utc_month = ublox_message_raw.utc_month; + ublox_message.utc_day = ublox_message_raw.utc_day; + ublox_message.time_of_week = ublox_message_raw.iTOW * 1e-3; + ublox_message.time_valid = ublox_message_raw.time_valid_flag; + + // LLH position + ublox_message.latitude = ublox_message_raw.latitude * 1e-7; + ublox_message.longitude = ublox_message_raw.longitude * 1e-7;; + ublox_message.height_above_ellipsoid = ublox_message_raw.height_above_ellipsoid * 1e-3; + ublox_message.llh_position_uncertainty[0] = ublox_message_raw.horizontal_accuracy * 1e-3; + ublox_message.llh_position_uncertainty[1] = ublox_message_raw.horizontal_accuracy * 1e-3; + ublox_message.llh_position_uncertainty[2] = ublox_message_raw.vertical_accuracy * 1e-3; + ublox_message.llh_position_valid = !ublox_message_raw.llh_invalid_flag; + + // NED velocity + ublox_message.ned_velocity[0] = ublox_message_raw.north_velocity * 1e-3; + ublox_message.ned_velocity[1] = ublox_message_raw.east_velocity * 1e-3; + ublox_message.ned_velocity[2] = ublox_message_raw.down_velocity * 1e-3; + for (int i = 0; i<3; i++) + ublox_message.ned_velocity_uncertainty[i] = ublox_message_raw.speed_accuracy * 1e-3; return ublox_message; } @@ -206,22 +149,28 @@ namespace mip::ublox for (int i = 0; i < PVT_PAYLOAD_SIZE; i++) payload_bytes[i] = packet[i + HEADER_SIZE]; + // Parse message payload _current_message = extract_pvt_message(payload_bytes); - _new_message_received = true; + + // Mark flag indicating a new message has been received + _new_pvt_message_received = true; } - std::pair update() + std::pair update() { - _new_message_received = false; + // Reset new message indicator flag + _new_pvt_message_received = false; + // Get incoming bytes from serial port uint8_t input_bytes[1024]; size_t num_input_bytes; mip::Timestamp timestamp_out; _connection->recvFromDevice(input_bytes, 1024, 1, &num_input_bytes, ×tamp_out); + // Spin message parser _message_parser.parse_bytes(input_bytes, num_input_bytes); - return {_new_message_received, _current_message}; + return {_new_pvt_message_received, _current_message}; } protected: @@ -229,7 +178,7 @@ namespace mip::ublox std::unique_ptr _connection; UbloxMessageParser _message_parser; - bool _new_message_received = false; - UBlox_PVT_Message _current_message; + bool _new_pvt_message_received = false; + UbloxPVTMessage _current_message; }; } \ No newline at end of file From 445ed2e491246e492f14ab03d81bfa575d385249 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Fri, 13 Oct 2023 09:57:59 -0400 Subject: [PATCH 150/252] Updated CMakeLists.txt --- examples/CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 3215250cf..403976dfe 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -51,9 +51,7 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(CV7_INS_Simple_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") - add_executable(CV7_INS_Simple_Ublox_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp" ${DEVICE_SOURCES} - CV7_INS/ublox_device.hpp - CV7_INS/simple_ublox_parser.hpp) + add_executable(CV7_INS_Simple_Ublox_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp" ${DEVICE_SOURCES} CV7_INS/ublox_device.hpp CV7_INS/simple_ublox_parser.hpp) target_link_libraries(CV7_INS_Simple_Ublox_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Ublox_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") From 112828f7f7a1ebc1b4c7420062a1450d6884fd87 Mon Sep 17 00:00:00 2001 From: David Robbins Date: Mon, 16 Oct 2023 13:19:59 -0400 Subject: [PATCH 151/252] Removed nested namespaces --- examples/CV7_INS/simple_ublox_parser.hpp | 144 +++++------ examples/CV7_INS/ublox_device.hpp | 299 ++++++++++++----------- 2 files changed, 225 insertions(+), 218 deletions(-) diff --git a/examples/CV7_INS/simple_ublox_parser.hpp b/examples/CV7_INS/simple_ublox_parser.hpp index f1eb9e2fd..9e99bddbc 100644 --- a/examples/CV7_INS/simple_ublox_parser.hpp +++ b/examples/CV7_INS/simple_ublox_parser.hpp @@ -29,105 +29,105 @@ const int HEADER_SIZE = 6; const int CHECKSUM_SIZE = 2; -namespace mip::ublox +namespace mip { - - bool verify_checksum(const std::vector& packet) + namespace ublox { - uint8_t ck_a, ck_b; - - ck_a = 0; - ck_b = 0; - int num_bytes = packet.size(); - int num_bytes_without_checksum = num_bytes-2; + bool verify_checksum(const std::vector& packet) + { + uint8_t ck_a, ck_b; - for (int i = 2; i < num_bytes_without_checksum; i++) { - ck_a += packet[i]; - ck_b += ck_a; - } + ck_a = 0; + ck_b = 0; - if (ck_a == packet[num_bytes - 2] && ck_b == packet[num_bytes - 1]) - return true; + int num_bytes = packet.size(); + int num_bytes_without_checksum = num_bytes - 2; - return false; - } + for (int i = 2; i < num_bytes_without_checksum; i++) { + ck_a += packet[i]; + ck_b += ck_a; + } + if (ck_a == packet[num_bytes - 2] && ck_b == packet[num_bytes - 1]) + return true; - class UbloxMessageParser - { - public: + return false; + } - UbloxMessageParser(std::function)> packet_callback) : _packet_callback(packet_callback) - {} - void parse_bytes(uint8_t* buffer, size_t num_input_bytes) + class UbloxMessageParser { - // Copy into parser buffer - for (size_t i = 0; i)> packet_callback) : _packet_callback( + packet_callback) + {} - // Wait for header bytes - while (_buffer.size() >= 2) + void parse_bytes(uint8_t *buffer, size_t num_input_bytes) { - if (header_found()) - break; + // Copy into parser buffer + for (size_t i = 0; i < num_input_bytes; i++) { + _buffer.emplace_back(buffer[i]); + } - _buffer.pop_front(); - } + // Wait for header bytes + while (_buffer.size() >= 2) { + if (header_found()) + break; - // Check if header is valid - if (!header_found()) - return; + _buffer.pop_front(); + } - // Check if buffer has full message header - if (_buffer.size() < 6) - return; + // Check if header is valid + if (!header_found()) + return; - // Get message length - uint8_t payload_length_bytes[2] = {_buffer[4], _buffer[5]}; - uint16_t payload_length; - memcpy(&payload_length, payload_length_bytes, sizeof(uint16_t)); + // Check if buffer has full message header + if (_buffer.size() < 6) + return; - int total_message_length = HEADER_SIZE + payload_length + CHECKSUM_SIZE; + // Get message length + uint8_t payload_length_bytes[2] = {_buffer[4], _buffer[5]}; + uint16_t payload_length; + memcpy(&payload_length, payload_length_bytes, sizeof(uint16_t)); - // Check if buffer contains full packet size - if (_buffer.size() < total_message_length) - return; + int total_message_length = HEADER_SIZE + payload_length + CHECKSUM_SIZE; - // Extract packet - std::vector packet(total_message_length); - for (int i = 0; i packet(total_message_length); + for (int i = 0; i < total_message_length; i++) + packet[i] = _buffer[i]; - // Clear packet from buffer - for (int i = 0; i)> _packet_callback; + std::function)> _packet_callback; - std::deque _buffer; - }; + std::deque _buffer; + }; + } } diff --git a/examples/CV7_INS/ublox_device.hpp b/examples/CV7_INS/ublox_device.hpp index d617435d9..5efca92de 100644 --- a/examples/CV7_INS/ublox_device.hpp +++ b/examples/CV7_INS/ublox_device.hpp @@ -28,157 +28,164 @@ #define PVT_PAYLOAD_SIZE 92 -namespace mip::ublox +namespace mip { - struct UbloxPVTMessageRaw { - uint32_t iTOW; - uint16_t utc_year; - uint8_t utc_month; - uint8_t utc_day; - uint8_t utc_hour; - uint8_t utc_minute; - uint8_t utc_second; - uint8_t time_valid_flag; - uint32_t time_accuracy; - int32_t nano_second; - uint8_t fix_type; - uint8_t fix_valid_flags; - uint8_t confirmed_time_date_flags; - uint8_t number_of_satellites; - int32_t longitude; - int32_t latitude; - int32_t height_above_ellipsoid; - int32_t height_above_sea_level; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; - int32_t north_velocity; - int32_t east_velocity; - int32_t down_velocity; - int32_t ground_speed; - int32_t heading_of_motion_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; - uint16_t pDOP; - uint8_t llh_invalid_flag; - uint8_t reserved_bytes[5]; - int32_t heading_of_vehicle; - int16_t magnetic_declination; - uint16_t magnetic_declination_accuracy; - }; - - - struct UbloxPVTMessage + namespace ublox { - // Time - uint16_t utc_year = 0; - uint8_t utc_month = 0; - uint8_t utc_day = 0; - float time_of_week = 0; - bool time_valid = false; - - // LLH position - double latitude = 0; - double longitude = 0; - double height_above_ellipsoid = 0; - float llh_position_uncertainty[3] = {0,0,0}; - bool llh_position_valid = false; - - // NED velocity - float ned_velocity[3] = {0,0,0}; - float ned_velocity_uncertainty[3] = {0,0,0}; - }; - - - UbloxPVTMessage extract_pvt_message(const uint8_t payload[PVT_PAYLOAD_SIZE]) - { - // Unpack raw UBlox message data - UbloxPVTMessageRaw ublox_message_raw; - std::memcpy(&ublox_message_raw, payload, sizeof(ublox_message_raw)); - - // Build output message with properly scaled units - UbloxPVTMessage ublox_message; - - // Time - ublox_message.utc_year = ublox_message_raw.utc_year; - ublox_message.utc_month = ublox_message_raw.utc_month; - ublox_message.utc_day = ublox_message_raw.utc_day; - ublox_message.time_of_week = ublox_message_raw.iTOW * 1e-3; - ublox_message.time_valid = ublox_message_raw.time_valid_flag; - - // LLH position - ublox_message.latitude = ublox_message_raw.latitude * 1e-7; - ublox_message.longitude = ublox_message_raw.longitude * 1e-7;; - ublox_message.height_above_ellipsoid = ublox_message_raw.height_above_ellipsoid * 1e-3; - ublox_message.llh_position_uncertainty[0] = ublox_message_raw.horizontal_accuracy * 1e-3; - ublox_message.llh_position_uncertainty[1] = ublox_message_raw.horizontal_accuracy * 1e-3; - ublox_message.llh_position_uncertainty[2] = ublox_message_raw.vertical_accuracy * 1e-3; - ublox_message.llh_position_valid = !ublox_message_raw.llh_invalid_flag; - - // NED velocity - ublox_message.ned_velocity[0] = ublox_message_raw.north_velocity * 1e-3; - ublox_message.ned_velocity[1] = ublox_message_raw.east_velocity * 1e-3; - ublox_message.ned_velocity[2] = ublox_message_raw.down_velocity * 1e-3; - for (int i = 0; i<3; i++) - ublox_message.ned_velocity_uncertainty[i] = ublox_message_raw.speed_accuracy * 1e-3; - - return ublox_message; - } - - - class UbloxDevice - { - public: - - UbloxDevice(std::unique_ptr connection) : _connection(std::move(connection)), - _message_parser([this](const std::vector& packet){ handle_packet(packet);}) - {} - - void handle_packet(const std::vector& packet) + struct UbloxPVTMessageRaw { - bool is_pvt_message = (packet[2] == 0x01) && (packet[3] == 0x07); - if (!is_pvt_message) - return; - - // Should never happen - size_t expected_packet_size = HEADER_SIZE + PVT_PAYLOAD_SIZE + CHECKSUM_SIZE; - if (packet.size() != expected_packet_size) - return; - - // Extract message payload - uint8_t payload_bytes[PVT_PAYLOAD_SIZE]; - for (int i = 0; i < PVT_PAYLOAD_SIZE; i++) - payload_bytes[i] = packet[i + HEADER_SIZE]; - - // Parse message payload - _current_message = extract_pvt_message(payload_bytes); - - // Mark flag indicating a new message has been received - _new_pvt_message_received = true; - } - - std::pair update() + uint32_t iTOW; + uint16_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + uint8_t utc_second; + uint8_t time_valid_flag; + uint32_t time_accuracy; + int32_t nano_second; + uint8_t fix_type; + uint8_t fix_valid_flags; + uint8_t confirmed_time_date_flags; + uint8_t number_of_satellites; + int32_t longitude; + int32_t latitude; + int32_t height_above_ellipsoid; + int32_t height_above_sea_level; + uint32_t horizontal_accuracy; + uint32_t vertical_accuracy; + int32_t north_velocity; + int32_t east_velocity; + int32_t down_velocity; + int32_t ground_speed; + int32_t heading_of_motion_2d; + uint32_t speed_accuracy; + uint32_t heading_accuracy; + uint16_t pDOP; + uint8_t llh_invalid_flag; + uint8_t reserved_bytes[5]; + int32_t heading_of_vehicle; + int16_t magnetic_declination; + uint16_t magnetic_declination_accuracy; + }; + + + struct UbloxPVTMessage { - // Reset new message indicator flag - _new_pvt_message_received = false; - - // Get incoming bytes from serial port - uint8_t input_bytes[1024]; - size_t num_input_bytes; - mip::Timestamp timestamp_out; - _connection->recvFromDevice(input_bytes, 1024, 1, &num_input_bytes, ×tamp_out); - - // Spin message parser - _message_parser.parse_bytes(input_bytes, num_input_bytes); - - return {_new_pvt_message_received, _current_message}; + // Time + uint16_t utc_year = 0; + uint8_t utc_month = 0; + uint8_t utc_day = 0; + float time_of_week = 0; + bool time_valid = false; + + // LLH position + double latitude = 0; + double longitude = 0; + double height_above_ellipsoid = 0; + float llh_position_uncertainty[3] = {0, 0, 0}; + bool llh_position_valid = false; + + // NED velocity + float ned_velocity[3] = {0, 0, 0}; + float ned_velocity_uncertainty[3] = {0, 0, 0}; + }; + + + UbloxPVTMessage extract_pvt_message(const uint8_t payload[PVT_PAYLOAD_SIZE]) + { + // Unpack raw UBlox message data + UbloxPVTMessageRaw ublox_message_raw; + std::memcpy(&ublox_message_raw, payload, sizeof(ublox_message_raw)); + + // Build output message with properly scaled units + UbloxPVTMessage ublox_message; + + // Time + ublox_message.utc_year = ublox_message_raw.utc_year; + ublox_message.utc_month = ublox_message_raw.utc_month; + ublox_message.utc_day = ublox_message_raw.utc_day; + ublox_message.time_of_week = ublox_message_raw.iTOW * 1e-3; + ublox_message.time_valid = ublox_message_raw.time_valid_flag; + + // LLH position + ublox_message.latitude = ublox_message_raw.latitude * 1e-7; + ublox_message.longitude = ublox_message_raw.longitude * 1e-7;; + ublox_message.height_above_ellipsoid = ublox_message_raw.height_above_ellipsoid * 1e-3; + ublox_message.llh_position_uncertainty[0] = ublox_message_raw.horizontal_accuracy * 1e-3; + ublox_message.llh_position_uncertainty[1] = ublox_message_raw.horizontal_accuracy * 1e-3; + ublox_message.llh_position_uncertainty[2] = ublox_message_raw.vertical_accuracy * 1e-3; + ublox_message.llh_position_valid = !ublox_message_raw.llh_invalid_flag; + + // NED velocity + ublox_message.ned_velocity[0] = ublox_message_raw.north_velocity * 1e-3; + ublox_message.ned_velocity[1] = ublox_message_raw.east_velocity * 1e-3; + ublox_message.ned_velocity[2] = ublox_message_raw.down_velocity * 1e-3; + for (int i = 0; i < 3; i++) + ublox_message.ned_velocity_uncertainty[i] = ublox_message_raw.speed_accuracy * 1e-3; + + return ublox_message; } - protected: - - std::unique_ptr _connection; - UbloxMessageParser _message_parser; - bool _new_pvt_message_received = false; - UbloxPVTMessage _current_message; - }; + class UbloxDevice + { + public: + + UbloxDevice(std::unique_ptr connection) : _connection(std::move(connection)), + _message_parser( + [this](const std::vector& packet) { + handle_packet(packet); + }) + {} + + void handle_packet(const std::vector& packet) + { + bool is_pvt_message = (packet[2] == 0x01) && (packet[3] == 0x07); + if (!is_pvt_message) + return; + + // Should never happen + size_t expected_packet_size = HEADER_SIZE + PVT_PAYLOAD_SIZE + CHECKSUM_SIZE; + if (packet.size() != expected_packet_size) + return; + + // Extract message payload + uint8_t payload_bytes[PVT_PAYLOAD_SIZE]; + for (int i = 0; i < PVT_PAYLOAD_SIZE; i++) + payload_bytes[i] = packet[i + HEADER_SIZE]; + + // Parse message payload + _current_message = extract_pvt_message(payload_bytes); + + // Mark flag indicating a new message has been received + _new_pvt_message_received = true; + } + + std::pair update() + { + // Reset new message indicator flag + _new_pvt_message_received = false; + + // Get incoming bytes from serial port + uint8_t input_bytes[1024]; + size_t num_input_bytes; + mip::Timestamp timestamp_out; + _connection->recvFromDevice(input_bytes, 1024, 1, &num_input_bytes, ×tamp_out); + + // Spin message parser + _message_parser.parse_bytes(input_bytes, num_input_bytes); + + return {_new_pvt_message_received, _current_message}; + } + + protected: + + std::unique_ptr _connection; + UbloxMessageParser _message_parser; + + bool _new_pvt_message_received = false; + UbloxPVTMessage _current_message; + }; + } } \ No newline at end of file From 71d4b4694adaa059868ad911acf8a420db5d15cc Mon Sep 17 00:00:00 2001 From: David Robbins Date: Mon, 16 Oct 2023 13:52:54 -0400 Subject: [PATCH 152/252] Added pragma pack around UBlox message definition Added array include --- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 2 +- examples/CV7_INS/ublox_device.hpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index f97809e30..8a252c540 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -26,11 +26,11 @@ #include "mip/mip_all.hpp" #include "../example_utils.hpp" #include "ublox_device.hpp" -#include #include #include #include #include +#include using namespace mip; diff --git a/examples/CV7_INS/ublox_device.hpp b/examples/CV7_INS/ublox_device.hpp index 5efca92de..6ba8f5d6c 100644 --- a/examples/CV7_INS/ublox_device.hpp +++ b/examples/CV7_INS/ublox_device.hpp @@ -32,6 +32,8 @@ namespace mip { namespace ublox { + +#pragma pack(1) struct UbloxPVTMessageRaw { uint32_t iTOW; @@ -68,6 +70,7 @@ namespace mip int16_t magnetic_declination; uint16_t magnetic_declination_accuracy; }; +#pragma pack() struct UbloxPVTMessage From a99f5a90b776074aea7d9409190736728fb0733d Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 2 Nov 2023 14:51:32 -0400 Subject: [PATCH 153/252] Move files from IC into mip sdk. --- CMakeLists.txt | 10 +- src/mip/extras/composite_result.hpp | 127 ++++++++++++++++++ src/mip/extras/descriptor_id.hpp | 45 +++++++ src/mip/extras/device_models.c | 92 +++++++++++++ .../{mip_models.h => extras/device_models.h} | 17 +-- src/mip/extras/index.hpp | 113 ++++++++++++++++ src/mip/extras/version.cpp | 74 ++++++++++ src/mip/extras/version.hpp | 69 ++++++++++ src/mip/mip_models.c | 91 ------------- src/mip/platform/serial_connection.hpp | 17 ++- src/mip/utils/version.hpp | 94 ------------- 11 files changed, 549 insertions(+), 200 deletions(-) create mode 100644 src/mip/extras/composite_result.hpp create mode 100644 src/mip/extras/descriptor_id.hpp create mode 100644 src/mip/extras/device_models.c rename src/mip/{mip_models.h => extras/device_models.h} (85%) create mode 100644 src/mip/extras/index.hpp create mode 100644 src/mip/extras/version.cpp create mode 100644 src/mip/extras/version.hpp delete mode 100644 src/mip/mip_models.c delete mode 100644 src/mip/utils/version.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f5b311f49..4e6d99555 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,7 +103,6 @@ set(UTILS_SOURCES "${UTILS_DIR}/byte_ring.h" "${UTILS_DIR}/serialization.c" "${UTILS_DIR}/serialization.h" - "${UTILS_DIR}/version.hpp" ) # @@ -118,8 +117,6 @@ set(MIP_SOURCES "${MIP_DIR}/mip_dispatch.h" "${MIP_DIR}/mip_field.c" "${MIP_DIR}/mip_field.h" - "${MIP_DIR}/mip_models.c" - "${MIP_DIR}/mip_models.h" "${MIP_DIR}/mip_offsets.h" "${MIP_DIR}/mip_packet.c" "${MIP_DIR}/mip_packet.h" @@ -198,10 +195,17 @@ if(MIP_USE_EXTRAS) list(APPEND MIP_DEFINES "MIP_USE_EXTRAS") set(MIP_EXTRAS_DIR "${MIP_DIR}/extras") set(MIP_EXTRA_SOURCES + "${MIP_EXTRAS_DIR}/composite_result.hpp" + "${MIP_EXTRAS_DIR}/descriptor_id.hpp" + "${MIP_EXTRAS_DIR}/device_models.h" + "${MIP_EXTRAS_DIR}/device_models.c" + "${MIP_EXTRAS_DIR}/index.hpp" "${MIP_EXTRAS_DIR}/recording_connection.hpp" "${MIP_EXTRAS_DIR}/recording_connection.cpp" "${MIP_EXTRAS_DIR}/scope_helper.hpp" "${MIP_EXTRAS_DIR}/scope_helper.cpp" + "${MIP_EXTRAS_DIR}/version.hpp" + "${MIP_EXTRAS_DIR}/version.cpp" ) endif() diff --git a/src/mip/extras/composite_result.hpp b/src/mip/extras/composite_result.hpp new file mode 100644 index 000000000..334ccb79b --- /dev/null +++ b/src/mip/extras/composite_result.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include "descriptor_id.hpp" + +#include "../mip_device.hpp" +#include "../mip_result.h" +#include "../definitions/descriptors.h" + + +#include + +namespace mip +{ + + //////////////////////////////////////////////////////////////////////////////// + ///@brief Represents a list of zero or more actions and their results. + /// + class CompositeResult + { + public: + struct Entry + { + CmdResult result; ///< Result of action. + DescriptorId descriptor; ///< Command or action that was executed. + + operator bool() const { return result; } + bool operator!() const { return !result; } + + Entry(bool r, DescriptorId d={}) : result(r ? CmdResult::ACK_OK : CmdResult::STATUS_ERROR), descriptor(d) {} + Entry(CmdResult r, DescriptorId d={}) : result(r), descriptor(d) {} + Entry(C::mip_cmd_result r, DescriptorId d={}) : result(r), descriptor(d) {} + //Entry(CmdResult r, CompositeDescriptor c) : result(r), descriptor(c) {} + }; + + public: + CompositeResult() {} + CompositeResult(bool success) : m_results{Entry{success, 0x0000}} {} + CompositeResult(CmdResult result) : m_results{result} {} + CompositeResult(CompositeDescriptor cmd, CmdResult result) : m_results{{result, cmd}} {} + CompositeResult(const Entry& result) : m_results{result} {} + + bool isEmpty() const { return m_results.empty(); } + bool notEmpty() const { return !m_results.empty(); } + size_t count() const { return m_results.size(); } + + bool allSuccessful() const { return std::all_of (m_results.begin(), m_results.end(), [](const Entry& r){ return r.result.isAck(); }); } + bool allFailed() const { return std::all_of (m_results.begin(), m_results.end(), [](const Entry& r){ return !r.result.isAck(); }); } + bool anySuccessful() const { return std::any_of (m_results.begin(), m_results.end(), [](const Entry& r){ return r.result.isAck(); }); } + bool anyFailed() const { return std::any_of (m_results.begin(), m_results.end(), [](const Entry& r){ return !r.result.isAck(); }); } + bool noneSuccessful() const { return std::none_of(m_results.begin(), m_results.end(), [](const Entry& r){ return r.result.isAck(); }); } + bool noneFailed() const { return std::none_of(m_results.begin(), m_results.end(), [](const Entry& r){ return !r.result.isAck(); }); } + + bool allMatch (CmdResult result) const { return std::all_of (m_results.begin(), m_results.end(), [result](const Entry& r){ return r.result == result; }); } + bool anyMatch (CmdResult result) const { return std::any_of (m_results.begin(), m_results.end(), [result](const Entry& r){ return r.result == result; }); } + bool noneMatch(CmdResult result) const { return std::none_of(m_results.begin(), m_results.end(), [result](const Entry& r){ return r.result == result; }); } + + operator bool() const { return noneFailed(); } + bool operator!() const { return anyFailed(); } + + CmdResult summary() const + { + if (isEmpty()) return CmdResult::STATUS_NONE; + if (count() == 1) return m_results.front().result; + if (allSuccessful()) return CmdResult::ACK_OK; + if (anyMatch(CmdResult::STATUS_TIMEDOUT)) return CmdResult::STATUS_TIMEDOUT; + else return CmdResult::STATUS_ERROR; + } + + void clear() { m_results.clear(); } + + // Append result to the list. + //void append(bool success, uint8_t id=0) { m_results.push_back({success ? CmdResult::ACK_OK : CmdResult::STATUS_ERROR, 0x0000}); } + void append(CmdResult result, CompositeDescriptor desc=0x0000) { m_results.push_back({result, desc}); } + void append(Entry result) { m_results.push_back(result); } + + // Append multiple results. + void extend(const CompositeResult& other) { m_results.insert(m_results.end(), other.m_results.begin(), other.m_results.end()); } + + // Same as append but returns *this. + CompositeResult& operator+=(bool result) { append(result); return *this; } + CompositeResult& operator+=(CmdResult result) { append(result); return *this; } + CompositeResult& operator+=(Entry result) { append(result); return *this; } + + // Same as append but returns the result. + // Useful for code like `if( !results.appendAndCheckThis( doCommand() ) { return results; /* This specific command failed */ })` + bool appendAndCheckThisCmd(bool result, uint32_t id) { append({result, id}); return result; } + bool appendAndCheckThisCmd(CmdResult result, CompositeDescriptor desc) { append({result, desc}); return result; } + bool appendAndCheckThisCmd(Entry result) { append(result); return result; } + + // Filter results (these would be a lot easier to implement with C++20's ranges filtering stuff) + // Returns a new CompositeResult (or C++20 filter view) with only the filtered results. + // Todo: Implement these as necessary. + //auto filterByStatus(CmdResult result) const {} + //auto filterByStatusNot(CmdResult result) const {} + //auto filterSuccessful() {} + //auto filterFailed() {} + //auto filterById(CompositeDescriptor) {} + + const Entry& first() const { return m_results.front(); } + Entry& first() { return m_results.front(); } + + const Entry& last() const { return m_results.back(); } + Entry& last() { return m_results.back(); } + + CmdResult firstResult() const { return m_results.front().result; } + CmdResult lastResult() const { return m_results.back().result; } + + auto begin() { return m_results.begin(); } + auto end() { return m_results.end(); } + + auto begin() const { return m_results.begin(); } + auto end() const { return m_results.end(); } + + private: + std::vector m_results; + }; + + + template + CompositeResult::Entry runCommand(DeviceInterface& device, const Cmd& cmd, Args&&... args) + { + CmdResult result = device.runCommand(cmd, std::forward(args)...); + + return {result, {Cmd::DESCRIPTOR_SET, Cmd::FIELD_DESCRIPTOR}}; + } + +} // namespace mip diff --git a/src/mip/extras/descriptor_id.hpp b/src/mip/extras/descriptor_id.hpp new file mode 100644 index 000000000..087318ccd --- /dev/null +++ b/src/mip/extras/descriptor_id.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include "../definitions/descriptors.h" + +#include + + +namespace mip +{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief A combination of a MIP descriptor pair and u16 ID value. +/// +/// Used by CompositeResult as a way to identify both MIP and non-MIP entries. +/// +class DescriptorId +{ +public: + DescriptorId() : m_key(0) {} + DescriptorId(uint8_t desc_set, uint8_t field_desc, uint16_t index=0) : DescriptorId({desc_set, field_desc}, index) {} + DescriptorId(CompositeDescriptor desc, uint16_t index=0) : m_key(uint32_t(desc.as_u16() << 16) | index) {} + //DescriptorId(uint16_t index) : m_key(index) {} + DescriptorId(uint32_t id) : m_key(id) {} + + bool isNull() const { return m_key == 0x00000000; } + + bool isMip() const { return descriptor().as_u16() != 0x0000; } + bool isNonMip() const { return descriptor().as_u16() == 0x0000; } + + CompositeDescriptor descriptor() const { return m_key >> 16; } + uint16_t index() const { return m_key & 0xFFFF; } + uint32_t asU32() const { return m_key; } + + bool operator==(const DescriptorId& other) const { return m_key == other.m_key; } + bool operator!=(const DescriptorId& other) const { return m_key != other.m_key; } + bool operator<=(const DescriptorId& other) const { return m_key <= other.m_key; } + bool operator>=(const DescriptorId& other) const { return m_key >= other.m_key; } + bool operator< (const DescriptorId& other) const { return m_key < other.m_key; } + bool operator> (const DescriptorId& other) const { return m_key > other.m_key; } + +private: + uint32_t m_key; +}; + +} // namespace mip diff --git a/src/mip/extras/device_models.c b/src/mip/extras/device_models.c new file mode 100644 index 000000000..fe04a6484 --- /dev/null +++ b/src/mip/extras/device_models.c @@ -0,0 +1,92 @@ + +#include "device_models.h" + +#include +#include +#include + +#ifdef __cplusplus +namespace mip { +#endif // __cplusplus + +mip_model_number getModelFromSerial(const char* serial) +{ + char model_str[16]; + bool found = false; + // The model number is just the portion of the serial or model string before the dot or dash + // serial and model number fields are 16 chars + for (uint8_t i = 0; i < 16; i++) + { + model_str[i] = serial[i]; + if (serial[i] == '.' + || serial[i] == '-') + { + found = true; + break; + } + } + + if (!found) + { + return MODEL_UNKNOWN; + } + + return atoi(model_str); +} + +mip_model_number getModelFromModelString(const char* model) +{ + // same logic can be used on either serial or model number string + // serial: model.deviceid + // model: model-specifier + return getModelFromSerial(model); +} + +const char* getModelNameFromSerial(const char* serial) +{ + switch(getModelFromSerial(serial)) + { + case MODEL_3DM_DH3 : return "3DM-DH3"; + case MODEL_3DM_GX3_15 : return "3DM-GX3-15"; + case MODEL_3DM_GX3_25 : return "3DM-GX3-25"; + case MODEL_3DM_GX3_35 : return "3DM-GX3-35"; + case MODEL_3DM_GX3_45 : return "3DM-GX3-45"; + case MODEL_3DM_RQ1_45_LT: return "3DM-RQ1-45_LT"; + case MODEL_3DM_GX4_15 : return "3DM-GX4-15"; + case MODEL_3DM_GX4_25 : return "3DM-GX4-25"; + case MODEL_3DM_GX4_45 : return "3DM-GX4-45"; + case MODEL_3DM_RQ1_45_ST: return "3DM-RQ1-45_ST"; + case MODEL_3DM_GX5_10 : return "3DM-GX5-10"; + case MODEL_3DM_GX5_15 : return "3DM-GX5-15"; + case MODEL_3DM_GX5_25 : return "3DM-GX5-25"; + case MODEL_3DM_GX5_35 : return "3DM-GX5-35"; + case MODEL_3DM_GX5_45 : return "3DM-GX5-45"; + case MODEL_3DM_CV5_10 : return "3DM-CV5-10"; + case MODEL_3DM_CV5_15 : return "3DM-CV5-15"; + case MODEL_3DM_CV5_25 : return "3DM-CV5-25"; + case MODEL_3DM_CV5_45 : return "3DM-CV5-45"; + case MODEL_3DM_GQ4_45 : return "3DM-GQ4-45"; + case MODEL_3DM_CX5_45 : return "3DM-CX5-45"; + case MODEL_3DM_CX5_35 : return "3DM-CX5-35"; + case MODEL_3DM_CX5_25 : return "3DM-CX5-25"; + case MODEL_3DM_CX5_15 : return "3DM-CX5-15"; + case MODEL_3DM_CX5_10 : return "3DM-CX5-10"; + case MODEL_3DM_CL5_15 : return "3DM-CL5-15"; + case MODEL_3DM_CL5_25 : return "3DM-CL5-25"; + case MODEL_3DM_GQ7 : return "3DM-GQ7"; + case MODEL_3DM_RTK : return "3DM-RTK"; + case MODEL_3DM_CV7_AHRS : return "3DM-CV7-AHRS"; + case MODEL_3DM_CV7_AR : return "3DM-CV7-AR"; + case MODEL_3DM_GV7_AHRS : return "3DM-GV7-AHRS"; + case MODEL_3DM_GV7_AR : return "3DM-GV7-AR"; + case MODEL_3DM_GV7_INS : return "3DM-GV7-INS"; + case MODEL_3DM_CV7_INS : return "3DM-CV7-INS"; + + default: + case MODEL_UNKNOWN : return ""; + } +} + +#ifdef __cplusplus +} // namespace mip +#endif // __cplusplus diff --git a/src/mip/mip_models.h b/src/mip/extras/device_models.h similarity index 85% rename from src/mip/mip_models.h rename to src/mip/extras/device_models.h index a069e1da4..0ad052577 100644 --- a/src/mip/mip_models.h +++ b/src/mip/extras/device_models.h @@ -1,13 +1,10 @@ #pragma once #ifdef __cplusplus -#include -#include - namespace mip { #endif // __cplusplus - enum Model + enum mip_model_number { MODEL_UNKNOWN = 0, MODEL_3DM_DH3 = 6219, // 3DM-DH3 @@ -46,11 +43,15 @@ namespace mip { MODEL_3DM_GV7_INS = 6290, // 3DM-GV7-INS MODEL_3DM_CV7_INS = 6291 // 3DM-CV7-INS }; +#ifndef __cplusplus + typedef enum mip_model_number mip_model_number; +#endif // __cplusplus - enum Model getModelFromSerial(const char* serial); - enum Model getModelFromModelString(const char* model); + mip_model_number getModelFromSerial(const char* serial); + mip_model_number getModelFromModelString(const char* model); + + const char* getModelNameFromSerial(const char* serial); #ifdef __cplusplus - std::string getModelNameFromSerial(const char* serial); } // namespace mip -#endif // __cplusplus \ No newline at end of file +#endif // __cplusplus diff --git a/src/mip/extras/index.hpp b/src/mip/extras/index.hpp new file mode 100644 index 000000000..fecb5a846 --- /dev/null +++ b/src/mip/extras/index.hpp @@ -0,0 +1,113 @@ +#pragma once + +namespace mip +{ + class Id; + + //////////////////////////////////////////////////////////////////////////////// + ///@brief Represents an index ranging from 0..N excluding N. + /// + /// Use this to help avoid off-by-one errors when using indices into arrays at + /// the same time as referring to MIP values which typically start at 1. + /// + /// Use the .index() or .id() methods to get the actual value, depending on + /// what you need. The index() method returns an unsigned int in the range [0,N-1] + /// while the id() method returns an unsigned int in the range [1,N] + /// + /// isAssigned() can be used to see if the value is valid, i.e. initialized. + /// isValid(N) can be used to check if the value is within a maximum count. + /// + /// The default value with no initialization is INVALID. + /// + /// This is interchangeable with the Id class below. + /// + class Index + { + private: + unsigned int INVALID = -1; + + public: + constexpr explicit Index(unsigned int index) : m_index(index) {} + constexpr Index() : m_index(INVALID) {} + + constexpr Index& setFromIndex(unsigned int index) { m_index = index; return *this; } + constexpr Index& setFromId(unsigned int id) { m_index = id-1; return *this; } + + constexpr unsigned int index() const { return m_index; } + constexpr unsigned int id() const { return m_index+1; } + + constexpr bool operator==(const Index& other) const { return m_index == other.m_index; } + constexpr bool operator!=(const Index& other) const { return m_index != other.m_index; } + + constexpr bool isAssigned() const { return m_index != INVALID; } + constexpr bool isValid(unsigned int max_count) const { return m_index < max_count; } + + constexpr void clear() { m_index = INVALID; } + + Index& operator++() { m_index++; return *this; } + Index& operator--() { m_index--; return *this; } + Index operator++(int) { Index tmp(*this); m_index++; return tmp; } + Index operator--(int) { Index tmp(*this); m_index--; return tmp; } + + constexpr Index(const Id& other); + constexpr Index& operator=(const Id& other); + + private: + unsigned int m_index; + }; + + //////////////////////////////////////////////////////////////////////////////// + ///@brief Representes an ID number ranging from 1..N including N. + /// + /// This is interchangeable with the Index class above. + /// + class Id + { + private: + unsigned int INVALID = 0; + + public: + constexpr explicit Id(unsigned int id) : m_id(id) {} + constexpr Id() : m_id(INVALID) {} + + constexpr Id& setFromIndex(unsigned int index) { m_id = index+1; return *this; } + constexpr Id& setFromId(unsigned int id) { m_id = id; return *this; } + + constexpr unsigned int index() const { return m_id-1; } + constexpr unsigned int id() const { return m_id; } + + constexpr bool operator==(const Id& other) const { return m_id == other.m_id; } + constexpr bool operator!=(const Id& other) const { return m_id != other.m_id; } + + constexpr bool isAssigned() const { return m_id != INVALID; } + constexpr bool isValid(unsigned int max_count) const { return index() < max_count; } + + constexpr void clear() { m_id = INVALID; } + + Id& operator++() { m_id++; return *this; } + Id& operator--() { m_id--; return *this; } + Id operator++(int) { Id tmp(*this); m_id++; return tmp; } + Id operator--(int) { Id tmp(*this); m_id--; return tmp; } + + constexpr Id(const Index& other) : m_id(other.id()) {} + constexpr Id& operator=(const Index& other) { return setFromIndex(other.index()); } + + private: + unsigned int m_id; + }; + + inline constexpr Index::Index(const Id& other) : m_index(other.index()) {} + inline constexpr Index& Index::operator=(const Id& other) { return setFromIndex(other.index()); } + + inline constexpr bool operator==(Id l, Index r) { return l.index() == r.index(); } + inline constexpr bool operator==(Index l, Id r) { return l.index() == r.index(); } + + // Get a pointer to a value in the container at index, or NULL if index out of range. + template + auto* indexOrNull(Container& container, Index index) { return index.isValid(container.size()) ? &container[index.index()] : nullptr; } + + // Get the value in the container at index, or a default value if index out of range. + template + auto indexOrDefault(Container& container, Index index, Value default_) { return index.isValid(container.size()) ? container[index.index()] : default_; } + +} // namespace mip diff --git a/src/mip/extras/version.cpp b/src/mip/extras/version.cpp new file mode 100644 index 000000000..d5b63c57c --- /dev/null +++ b/src/mip/extras/version.cpp @@ -0,0 +1,74 @@ + +#include "version.hpp" + +#include + + +namespace mip +{ + +//////////////////////////////////////////////////////////////////////////////// +///@brief Convert the version to a string in the standard X.Y.ZZ format. +/// +/// Note: The result is unspecified for invalid version numbers. +/// +///@param buffer Character buffer to write into. +///@param buffer_size Length (including space for null terminator) of buffer. +/// +void FirmwareVersion::toString(char* buffer, size_t buffer_size) const +{ + std::snprintf(buffer, buffer_size, "%u.%u.%02u", major(), minor(), patch()); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Reads a standard-format string (X.Y.ZZ\0 or XYZZ\0). +/// +///@param str Input string. Can be unterminated if length is specified. +///@param length Length of input string. Assumed to be NULL-terminated if -1. +/// +///@return True if a valid version was parsed. +/// +bool FirmwareVersion::fromString(const char* str, size_t length) +{ + m_version = 0; + + unsigned int digit = 0; + for(unsigned int i=0; i= 4) + break; + } + + return isValid(); +} + +//////////////////////////////////////////////////////////////////////////////// +///@brief Convert a FirmwareVersion to a string separated by periods. +/// +/// This is different from Version::toString in that the patch number uses +/// zero-padding. +/// +std::string FirmwareVersion::toString() const +{ + char buffer[3+1+3+1+3+1]; + + toString(buffer, sizeof(buffer)); + + return buffer; +} + +} // namespace mip diff --git a/src/mip/extras/version.hpp b/src/mip/extras/version.hpp new file mode 100644 index 000000000..ac30499f2 --- /dev/null +++ b/src/mip/extras/version.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include +#include + +#include + +#if __cpp_lib_string_view >= 201606L +#include +#endif + +namespace mip +{ + +//////////////////////////////////////////////////////////////////////////// +///@brief Represents the device firmware version. +/// +/// Device firmware is of the form X.Y.ZZ, where: +/// - X is the major version, exactly 1 digit. +/// - Y is the minor version, exactly 1 digit. +/// - Z is the patch version, exactly 2 digits. +/// +/// Internally this class stores the version as a 16-bit unsigned integer. +/// +class FirmwareVersion +{ +public: + FirmwareVersion() = default; + FirmwareVersion(uint8_t major, uint8_t minor, uint8_t patch) { fromParts(major, minor, patch); } + explicit FirmwareVersion(uint16_t version) : m_version(version) {} + + FirmwareVersion(const FirmwareVersion&) = default; + FirmwareVersion& operator=(const FirmwareVersion&) = default; + FirmwareVersion& operator=(uint16_t version) { m_version = version; return *this; } + + bool isValid() const { return major() < 10 && minor() < 10 && patch() < 100; } + bool isDevVersion() const { return major() == 0; } + bool isReleaseVersion() const { return major() > 0; } + bool isSpecialVersion() const { return major() > 1; } + + uint16_t asU16() const { return m_version; } + uint16_t& asU16() { return m_version; } + + void fromParts(uint8_t major, uint8_t minor, uint8_t patch) { m_version = major * 1000 + minor * 100 + patch; } + + uint8_t major() const { return m_version / 1000; } + uint8_t minor() const { return (m_version / 100) % 10; } + uint8_t patch() const { return m_version % 100; } + + bool operator==(FirmwareVersion other) const { return m_version == other.m_version; } + bool operator!=(FirmwareVersion other) const { return m_version != other.m_version; } + bool operator<=(FirmwareVersion other) const { return m_version <= other.m_version; } + bool operator>=(FirmwareVersion other) const { return m_version <= other.m_version; } + bool operator< (FirmwareVersion other) const { return m_version < other.m_version; } + bool operator> (FirmwareVersion other) const { return m_version > other.m_version; } + + void toString(char* buffer, size_t buffer_size) const; + bool fromString(const char* str, size_t length=-1); + + std::string toString() const; +#if __cpp_lib_string_view >= 201606L + bool fromString(std::string_view str) { return fromString(str.data(), str.size()); } +#endif + +private: + uint16_t m_version = 0; +}; + +} // namespace mip diff --git a/src/mip/mip_models.c b/src/mip/mip_models.c deleted file mode 100644 index 7e674e626..000000000 --- a/src/mip/mip_models.c +++ /dev/null @@ -1,91 +0,0 @@ -#include "mip_models.h" -#include -#include -#include - -#ifdef __cplusplus -namespace mip { -#endif // __cplusplus - - enum Model getModelFromSerial(const char* serial) - { - char model_str[16]; - bool found = false; - // The model number is just the portion of the serial or model string before the dot or dash - // serial and model number fields are 16 chars - for (uint8_t i = 0; i < 16; i++) - { - model_str[i] = serial[i]; - if (serial[i] == '.' - || serial[i] == '-') - { - found = true; - break; - } - } - - if (!found) - { - return MODEL_UNKNOWN; - } - - return atoi(model_str); - } - - enum Model getModelFromModelString(const char* model) - { - // same logic can be used on either serial or model number string - // serial: model.deviceid - // model: model-specifier - return getModelFromSerial(model); - } - -#ifdef __cplusplus - - std::string getModelNameFromSerial(const char* serial) - { - enum Model model = getModelFromSerial(serial); - switch (model) - { - case MODEL_3DM_DH3 : return "3DM-DH3"; - case MODEL_3DM_GX3_15 : return "3DM-GX3-15"; - case MODEL_3DM_GX3_25 : return "3DM-GX3-25"; - case MODEL_3DM_GX3_35 : return "3DM-GX3-35"; - case MODEL_3DM_GX3_45 : return "3DM-GX3-45"; - case MODEL_3DM_RQ1_45_LT: return "3DM-RQ1-45_LT"; - case MODEL_3DM_GX4_15 : return "3DM-GX4-15"; - case MODEL_3DM_GX4_25 : return "3DM-GX4-25"; - case MODEL_3DM_GX4_45 : return "3DM-GX4-45"; - case MODEL_3DM_RQ1_45_ST: return "3DM-RQ1-45_ST"; - case MODEL_3DM_GX5_10 : return "3DM-GX5-10"; - case MODEL_3DM_GX5_15 : return "3DM-GX5-15"; - case MODEL_3DM_GX5_25 : return "3DM-GX5-25"; - case MODEL_3DM_GX5_35 : return "3DM-GX5-35"; - case MODEL_3DM_GX5_45 : return "3DM-GX5-45"; - case MODEL_3DM_CV5_10 : return "3DM-CV5-10"; - case MODEL_3DM_CV5_15 : return "3DM-CV5-15"; - case MODEL_3DM_CV5_25 : return "3DM-CV5-25"; - case MODEL_3DM_CV5_45 : return "3DM-CV5-45"; - case MODEL_3DM_GQ4_45 : return "3DM-GQ4-45"; - case MODEL_3DM_CX5_45 : return "3DM-CX5-45"; - case MODEL_3DM_CX5_35 : return "3DM-CX5-35"; - case MODEL_3DM_CX5_25 : return "3DM-CX5-25"; - case MODEL_3DM_CX5_15 : return "3DM-CX5-15"; - case MODEL_3DM_CX5_10 : return "3DM-CX5-10"; - case MODEL_3DM_CL5_15 : return "3DM-CL5-15"; - case MODEL_3DM_CL5_25 : return "3DM-CL5-25"; - case MODEL_3DM_GQ7 : return "3DM-GQ7"; - case MODEL_3DM_RTK : return "3DM-RTK"; - case MODEL_3DM_CV7_AHRS : return "3DM-CV7-AHRS"; - case MODEL_3DM_CV7_AR : return "3DM-CV7-AR"; - case MODEL_3DM_GV7_AHRS : return "3DM-GV7-AHRS"; - case MODEL_3DM_GV7_AR : return "3DM-GV7-AR"; - case MODEL_3DM_GV7_INS : return "3DM-GV7-INS"; - case MODEL_3DM_CV7_INS : return "3DM-CV7-INS"; - - default: - case MODEL_UNKNOWN : return ""; - } - } -} // namespace mip -#endif // __cplusplus \ No newline at end of file diff --git a/src/mip/platform/serial_connection.hpp b/src/mip/platform/serial_connection.hpp index ef8bae4fd..6ecd6d5f1 100644 --- a/src/mip/platform/serial_connection.hpp +++ b/src/mip/platform/serial_connection.hpp @@ -23,16 +23,15 @@ class SerialConnection : public mip::Connection static constexpr auto TYPE = "Serial"; - SerialConnection() = default; SerialConnection(const std::string& portName, uint32_t baudrate); ~SerialConnection(); bool recvFromDevice(uint8_t* buffer, size_t max_length, Timeout wait_time, size_t* length_out, mip::Timestamp* timestamp) final; bool sendToDevice(const uint8_t* data, size_t length) final; - bool isConnected() const; - bool connect(); - bool disconnect(); + bool isConnected() const override; + bool connect() override; + bool disconnect() override; void connectionInfo(std::string &name, uint32_t &baudrate) const { @@ -51,6 +50,16 @@ class SerialConnection : public mip::Connection }; +//////////////////////////////////////////////////////////////////////////////// +///@brief A serial connection but indicates that it's actually a USB connection. +class UsbSerialConnection : public SerialConnection +{ +public: + static constexpr auto TYPE = "USB"; + + UsbSerialConnection(const std::string& portName, uint32_t baudrate) : SerialConnection(portName, baudrate) { mType = TYPE; } +}; + ///@} //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/utils/version.hpp b/src/mip/utils/version.hpp deleted file mode 100644 index 97860b41f..000000000 --- a/src/mip/utils/version.hpp +++ /dev/null @@ -1,94 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -//////////////////////////////////////////////////////////////////////////////// -///@defgroup mip_serialization MIP Serialization -/// -///@brief Serialization Functions for reading and writing to byte buffers. -///@{ -///@defgroup mip_serialization_c MIP Serialization [C] -///@defgroup mip_serialization_cpp MIP Serialization [CPP] -///@} - -#ifdef __cplusplus -#include - -namespace mip { - - //////////////////////////////////////////////////////////////////////////////// - ///@addtogroup version_cpp - /// - ///@brief Version interpretation in C++. - /// - ///@{ - - //////////////////////////////////////////////////////////////////////////////// - ///@brief Structure used interpretting version strings. - /// - /// - struct Version - { - Version(uint16_t device_value=0) : m_value(device_value) {} - Version(uint8_t major_, uint8_t minor_, uint8_t patch_) : m_value(fromParts(major_, minor_, patch_)) { assert(major_<10 && minor_<10 && patch_<100); } - - uint8_t major() const { return m_value / 100; } - uint8_t minor() const { return (m_value / 10) % 10;} - uint8_t patch() const { return m_value % 100; } - uint16_t bcd() const { return m_value; } - - bool isDevVersion() const { return major() == 0; } - bool isReleaseVersion() const { return major() > 0; } - bool isSpecialVersion() const { return major() > 1; } - - bool operator==(Version other) const { return m_value == other.m_value; } - bool operator!=(Version other) const { return m_value != other.m_value; } - bool operator<=(Version other) const { return m_value <= other.m_value; } - bool operator>=(Version other) const { return m_value <= other.m_value; } - bool operator< (Version other) const { return m_value < other.m_value; } - bool operator> (Version other) const { return m_value < other.m_value; } - - static constexpr size_t STRING_BUFFER_SIZE = 7+1; // Including null - void toString(char* buffer) const { std::snprintf(buffer, STRING_BUFFER_SIZE, "%u.%u.%02u", major(), minor(), patch()); } - std::string toString() const { char buffer[STRING_BUFFER_SIZE]; toString(buffer); return std::string(buffer); } - - bool fromString(std::string str) - { - // Pattern to extract only numbers Major.Minor.Patch. Anything else before or after is ignored - const std::regex pattern(R"((\d+)\.?(\d+)?\.?(\d+)?)"); - - std::smatch match; - - // Pattern will always find a Major number if it exists - if (std::regex_search(str, match, pattern)) - { - uint8_t major = std::stoi(match[1]); - - // Minor is optionally found - uint8_t minor = match[2].matched ? std::stoi(match[2]) : 0; - - // Patch is optionally found - uint8_t patch = match[3].matched ? std::stoi(match[3]) : 0; - - m_value = fromParts(major, minor, patch); - - return true; - } - - return false; - } - - private: - uint16_t m_value = 0; - - private: - static uint16_t fromParts(uint8_t major, uint8_t minor, uint8_t patch) { return major * 100 + minor * 10 + patch; } - }; - -} // namespace mip -#endif // __cplusplus - -//////////////////////////////////////////////////////////////////////////////// \ No newline at end of file From 742c86c5c75cc623f87e31e384bedf39a6a6b3ba Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 6 Nov 2023 17:03:04 -0500 Subject: [PATCH 154/252] Misc fixes & formating. --- CMakeLists.txt | 1 + src/mip/extras/device_models.c | 124 +++++++++++++++++---------------- src/mip/extras/device_models.h | 113 ++++++++++++++++++------------ src/mip/extras/version.hpp | 3 +- 4 files changed, 136 insertions(+), 105 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e6d99555..739c839a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -226,6 +226,7 @@ set(ALL_MIP_SOURCES ${MIP_INTERFACE_SOURCES} ${MIP_EXTRA_SOURCES} ) +message("SRC ${ALL_MIP_SOURCES}") option(MIP_DISABLE_CPP "Excludes all C++ files from the project." OFF) if(MIP_DISABLE_CPP) diff --git a/src/mip/extras/device_models.c b/src/mip/extras/device_models.c index fe04a6484..27a0de9be 100644 --- a/src/mip/extras/device_models.c +++ b/src/mip/extras/device_models.c @@ -1,92 +1,94 @@ #include "device_models.h" +#include #include #include #include #ifdef __cplusplus namespace mip { -#endif // __cplusplus +namespace C { +extern "C" { +#endif// __cplusplus -mip_model_number getModelFromSerial(const char* serial) +mip_model_number get_model_from_string(const char* model_or_serial) { - char model_str[16]; - bool found = false; + unsigned int start_index = 0; + // The model number is just the portion of the serial or model string before the dot or dash // serial and model number fields are 16 chars - for (uint8_t i = 0; i < 16; i++) + unsigned int i = 0; + for (; model_or_serial[i] != '\0'; i++) { - model_str[i] = serial[i]; - if (serial[i] == '.' - || serial[i] == '-') + const char c = model_or_serial[i]; + + if (!isdigit(c)) { - found = true; - break; + if (c == '.' || c == '-') + break; + + if (isspace(c) && (start_index == i)) + start_index++; + else + return MODEL_UNKNOWN; } } - if (!found) - { + // Must get at least 4 digits. + if (i < start_index + 4) return MODEL_UNKNOWN; - } - - return atoi(model_str); -} -mip_model_number getModelFromModelString(const char* model) -{ - // same logic can be used on either serial or model number string - // serial: model.deviceid - // model: model-specifier - return getModelFromSerial(model); + return atoi(model_or_serial + start_index); } -const char* getModelNameFromSerial(const char* serial) +const char* get_model_name_from_number(mip_model_number model) { - switch(getModelFromSerial(serial)) + switch (model) { - case MODEL_3DM_DH3 : return "3DM-DH3"; - case MODEL_3DM_GX3_15 : return "3DM-GX3-15"; - case MODEL_3DM_GX3_25 : return "3DM-GX3-25"; - case MODEL_3DM_GX3_35 : return "3DM-GX3-35"; - case MODEL_3DM_GX3_45 : return "3DM-GX3-45"; - case MODEL_3DM_RQ1_45_LT: return "3DM-RQ1-45_LT"; - case MODEL_3DM_GX4_15 : return "3DM-GX4-15"; - case MODEL_3DM_GX4_25 : return "3DM-GX4-25"; - case MODEL_3DM_GX4_45 : return "3DM-GX4-45"; - case MODEL_3DM_RQ1_45_ST: return "3DM-RQ1-45_ST"; - case MODEL_3DM_GX5_10 : return "3DM-GX5-10"; - case MODEL_3DM_GX5_15 : return "3DM-GX5-15"; - case MODEL_3DM_GX5_25 : return "3DM-GX5-25"; - case MODEL_3DM_GX5_35 : return "3DM-GX5-35"; - case MODEL_3DM_GX5_45 : return "3DM-GX5-45"; - case MODEL_3DM_CV5_10 : return "3DM-CV5-10"; - case MODEL_3DM_CV5_15 : return "3DM-CV5-15"; - case MODEL_3DM_CV5_25 : return "3DM-CV5-25"; - case MODEL_3DM_CV5_45 : return "3DM-CV5-45"; - case MODEL_3DM_GQ4_45 : return "3DM-GQ4-45"; - case MODEL_3DM_CX5_45 : return "3DM-CX5-45"; - case MODEL_3DM_CX5_35 : return "3DM-CX5-35"; - case MODEL_3DM_CX5_25 : return "3DM-CX5-25"; - case MODEL_3DM_CX5_15 : return "3DM-CX5-15"; - case MODEL_3DM_CX5_10 : return "3DM-CX5-10"; - case MODEL_3DM_CL5_15 : return "3DM-CL5-15"; - case MODEL_3DM_CL5_25 : return "3DM-CL5-25"; - case MODEL_3DM_GQ7 : return "3DM-GQ7"; - case MODEL_3DM_RTK : return "3DM-RTK"; - case MODEL_3DM_CV7_AHRS : return "3DM-CV7-AHRS"; - case MODEL_3DM_CV7_AR : return "3DM-CV7-AR"; - case MODEL_3DM_GV7_AHRS : return "3DM-GV7-AHRS"; - case MODEL_3DM_GV7_AR : return "3DM-GV7-AR"; - case MODEL_3DM_GV7_INS : return "3DM-GV7-INS"; - case MODEL_3DM_CV7_INS : return "3DM-CV7-INS"; + case MODEL_3DM_DH3: return "3DM-DH3"; + case MODEL_3DM_GX3_15: return "3DM-GX3-15"; + case MODEL_3DM_GX3_25: return "3DM-GX3-25"; + case MODEL_3DM_GX3_35: return "3DM-GX3-35"; + case MODEL_3DM_GX3_45: return "3DM-GX3-45"; + case MODEL_3DM_RQ1_45_LT: return "3DM-RQ1-45_LT"; + case MODEL_3DM_GX4_15: return "3DM-GX4-15"; + case MODEL_3DM_GX4_25: return "3DM-GX4-25"; + case MODEL_3DM_GX4_45: return "3DM-GX4-45"; + case MODEL_3DM_RQ1_45_ST: return "3DM-RQ1-45_ST"; + case MODEL_3DM_GX5_10: return "3DM-GX5-10"; + case MODEL_3DM_GX5_15: return "3DM-GX5-15"; + case MODEL_3DM_GX5_25: return "3DM-GX5-25"; + case MODEL_3DM_GX5_35: return "3DM-GX5-35"; + case MODEL_3DM_GX5_45: return "3DM-GX5-45"; + case MODEL_3DM_CV5_10: return "3DM-CV5-10"; + case MODEL_3DM_CV5_15: return "3DM-CV5-15"; + case MODEL_3DM_CV5_25: return "3DM-CV5-25"; + case MODEL_3DM_CV5_45: return "3DM-CV5-45"; + case MODEL_3DM_GQ4_45: return "3DM-GQ4-45"; + case MODEL_3DM_CX5_45: return "3DM-CX5-45"; + case MODEL_3DM_CX5_35: return "3DM-CX5-35"; + case MODEL_3DM_CX5_25: return "3DM-CX5-25"; + case MODEL_3DM_CX5_15: return "3DM-CX5-15"; + case MODEL_3DM_CX5_10: return "3DM-CX5-10"; + case MODEL_3DM_CL5_15: return "3DM-CL5-15"; + case MODEL_3DM_CL5_25: return "3DM-CL5-25"; + case MODEL_3DM_GQ7: return "3DM-GQ7"; + case MODEL_3DM_RTK: return "3DM-RTK"; + case MODEL_3DM_CV7_AHRS: return "3DM-CV7-AHRS"; + case MODEL_3DM_CV7_AR: return "3DM-CV7-AR"; + case MODEL_3DM_GV7_AHRS: return "3DM-GV7-AHRS"; + case MODEL_3DM_GV7_AR: return "3DM-GV7-AR"; + case MODEL_3DM_GV7_INS: return "3DM-GV7-INS"; + case MODEL_3DM_CV7_INS: return "3DM-CV7-INS"; - default: - case MODEL_UNKNOWN : return ""; + default: + case MODEL_UNKNOWN: return ""; } } #ifdef __cplusplus +} // extern "C" +} // namespace C } // namespace mip #endif // __cplusplus diff --git a/src/mip/extras/device_models.h b/src/mip/extras/device_models.h index 0ad052577..c7e5aad03 100644 --- a/src/mip/extras/device_models.h +++ b/src/mip/extras/device_models.h @@ -2,56 +2,83 @@ #ifdef __cplusplus namespace mip { +namespace C +{ +extern "C" { #endif // __cplusplus - enum mip_model_number - { - MODEL_UNKNOWN = 0, - MODEL_3DM_DH3 = 6219, // 3DM-DH3 - MODEL_3DM_GX3_15 = 6227, // 3DM-GX3-15 - MODEL_3DM_GX3_25 = 6223, // 3DM-GX3-25 - MODEL_3DM_GX3_35 = 6225, // 3DM-GX3-35 - MODEL_3DM_GX3_45 = 6228, // 3DM-GX3-45 - MODEL_3DM_RQ1_45_LT = 6232, // 3DM-RQ1-45-LT - MODEL_3DM_GX4_15 = 6233, // 3DM-GX4-15 - MODEL_3DM_GX4_25 = 6234, // 3DM-GX4-25 - MODEL_3DM_GX4_45 = 6236, // 3DM-GX4-45 - MODEL_3DM_RQ1_45_ST = 6239, // 3DM-RQ1-45-ST - MODEL_3DM_GX5_10 = 6255, // 3DM-GX5-10 - MODEL_3DM_GX5_15 = 6254, // 3DM-GX5-15 - MODEL_3DM_GX5_25 = 6253, // 3DM-GX5-25 - MODEL_3DM_GX5_35 = 6252, // 3DM-GX5-35 - MODEL_3DM_GX5_45 = 6251, // 3DM-GX5-45 - MODEL_3DM_CV5_10 = 6259, // 3DM-CV5-10 - MODEL_3DM_CV5_15 = 6258, // 3DM-CV5-15 - MODEL_3DM_CV5_25 = 6257, // 3DM-CV5-25 - MODEL_3DM_CV5_45 = 6256, // 3DM-CV5-45 - MODEL_3DM_GQ4_45 = 6250, // 3DM-GQ4-45 - MODEL_3DM_CX5_45 = 6271, // 3DM-CX5-45 - MODEL_3DM_CX5_35 = 6272, // 3DM-CX5-35 - MODEL_3DM_CX5_25 = 6273, // 3DM-CX5-25 - MODEL_3DM_CX5_15 = 6274, // 3DM-CX5-15 - MODEL_3DM_CX5_10 = 6275, // 3DM-CX5-10 - MODEL_3DM_CL5_15 = 6280, // 3DM-CL5-15 - MODEL_3DM_CL5_25 = 6281, // 3DM-CL5-25 - MODEL_3DM_GQ7 = 6284, // 3DM-GQ7 - MODEL_3DM_RTK = 6285, // 3DM-RTK - MODEL_3DM_CV7_AHRS = 6286, // 3DM-CV7-AHRS - MODEL_3DM_CV7_AR = 6287, // 3DM-CV7-AR - MODEL_3DM_GV7_AHRS = 6288, // 3DM-GV7-AHRS - MODEL_3DM_GV7_AR = 6289, // 3DM-GV7-AR - MODEL_3DM_GV7_INS = 6290, // 3DM-GV7-INS - MODEL_3DM_CV7_INS = 6291 // 3DM-CV7-INS - }; +enum mip_model_number +{ + MODEL_UNKNOWN = 0, + MODEL_3DM_DH3 = 6219,// 3DM-DH3 + MODEL_3DM_GX3_15 = 6227,// 3DM-GX3-15 + MODEL_3DM_GX3_25 = 6223,// 3DM-GX3-25 + MODEL_3DM_GX3_35 = 6225,// 3DM-GX3-35 + MODEL_3DM_GX3_45 = 6228,// 3DM-GX3-45 + MODEL_3DM_RQ1_45_LT = 6232,// 3DM-RQ1-45-LT + MODEL_3DM_GX4_15 = 6233,// 3DM-GX4-15 + MODEL_3DM_GX4_25 = 6234,// 3DM-GX4-25 + MODEL_3DM_GX4_45 = 6236,// 3DM-GX4-45 + MODEL_3DM_RQ1_45_ST = 6239,// 3DM-RQ1-45-ST + MODEL_3DM_GX5_10 = 6255,// 3DM-GX5-10 + MODEL_3DM_GX5_15 = 6254,// 3DM-GX5-15 + MODEL_3DM_GX5_25 = 6253,// 3DM-GX5-25 + MODEL_3DM_GX5_35 = 6252,// 3DM-GX5-35 + MODEL_3DM_GX5_45 = 6251,// 3DM-GX5-45 + MODEL_3DM_CV5_10 = 6259,// 3DM-CV5-10 + MODEL_3DM_CV5_15 = 6258,// 3DM-CV5-15 + MODEL_3DM_CV5_25 = 6257,// 3DM-CV5-25 + MODEL_3DM_CV5_45 = 6256,// 3DM-CV5-45 + MODEL_3DM_GQ4_45 = 6250,// 3DM-GQ4-45 + MODEL_3DM_CX5_45 = 6271,// 3DM-CX5-45 + MODEL_3DM_CX5_35 = 6272,// 3DM-CX5-35 + MODEL_3DM_CX5_25 = 6273,// 3DM-CX5-25 + MODEL_3DM_CX5_15 = 6274,// 3DM-CX5-15 + MODEL_3DM_CX5_10 = 6275,// 3DM-CX5-10 + MODEL_3DM_CL5_15 = 6280,// 3DM-CL5-15 + MODEL_3DM_CL5_25 = 6281,// 3DM-CL5-25 + MODEL_3DM_GQ7 = 6284,// 3DM-GQ7 + MODEL_3DM_RTK = 6285,// 3DM-RTK + MODEL_3DM_CV7_AHRS = 6286,// 3DM-CV7-AHRS + MODEL_3DM_CV7_AR = 6287,// 3DM-CV7-AR + MODEL_3DM_GV7_AHRS = 6288,// 3DM-GV7-AHRS + MODEL_3DM_GV7_AR = 6289,// 3DM-GV7-AR + MODEL_3DM_GV7_INS = 6290,// 3DM-GV7-INS + MODEL_3DM_CV7_INS = 6291 // 3DM-CV7-INS +}; #ifndef __cplusplus - typedef enum mip_model_number mip_model_number; +typedef enum mip_model_number mip_model_number; #endif // __cplusplus - mip_model_number getModelFromSerial(const char* serial); - mip_model_number getModelFromModelString(const char* model); +mip_model_number get_model_from_string(const char* model_or_serial); +const char* get_model_name_from_number(mip_model_number model); - const char* getModelNameFromSerial(const char* serial); #ifdef __cplusplus +} // extern "C" +} // namespace C + +using ModelNumber = C::mip_model_number; + +inline ModelNumber getModelFromString(const char* model_or_serial) { return C::get_model_from_string(model_or_serial); } +inline const char* getModelNameFromNumber(ModelNumber model) { return get_model_name_from_number(model); } + + +//class ModelNumber +//{ +// ModelNumber(C::mip_model_number number) : m_number(number) {} +// ModelNumber(const char* model_or_serial) : m_number(C::get_model_from_string(model_or_serial) {} +// +// const char* getModelName() { return C::get_model_name_from_number(m_number); } +// +// using Model = C::mip_model_number; +// +// uint16_t shortNumber() const { return m_number; } +// uint32_t longNumber() const { return m_number * 1000; } +// +//private: +// C::mip_model_number m_number; +//}; + } // namespace mip #endif // __cplusplus diff --git a/src/mip/extras/version.hpp b/src/mip/extras/version.hpp index ac30499f2..655a1a1c3 100644 --- a/src/mip/extras/version.hpp +++ b/src/mip/extras/version.hpp @@ -33,7 +33,8 @@ class FirmwareVersion FirmwareVersion& operator=(const FirmwareVersion&) = default; FirmwareVersion& operator=(uint16_t version) { m_version = version; return *this; } - bool isValid() const { return major() < 10 && minor() < 10 && patch() < 100; } + bool isNull() const { return m_version == 0; } + bool isValid() const { return !isNull() && major() < 10 && minor() < 10 && patch() < 100; } bool isDevVersion() const { return major() == 0; } bool isReleaseVersion() const { return major() > 0; } bool isSpecialVersion() const { return major() > 1; } From 218a594aae34c3163db9b74655dc535740e2ff99 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 8 Nov 2023 19:09:07 -0500 Subject: [PATCH 155/252] Fix bug in FirmwareVersion comparison. --- src/mip/extras/version.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/extras/version.hpp b/src/mip/extras/version.hpp index 655a1a1c3..23467e2b2 100644 --- a/src/mip/extras/version.hpp +++ b/src/mip/extras/version.hpp @@ -51,7 +51,7 @@ class FirmwareVersion bool operator==(FirmwareVersion other) const { return m_version == other.m_version; } bool operator!=(FirmwareVersion other) const { return m_version != other.m_version; } bool operator<=(FirmwareVersion other) const { return m_version <= other.m_version; } - bool operator>=(FirmwareVersion other) const { return m_version <= other.m_version; } + bool operator>=(FirmwareVersion other) const { return m_version >= other.m_version; } bool operator< (FirmwareVersion other) const { return m_version < other.m_version; } bool operator> (FirmwareVersion other) const { return m_version > other.m_version; } From 637bacda4421b99e1baa7eadf4a1718d4c467784 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 9 Nov 2023 17:41:53 -0500 Subject: [PATCH 156/252] Fix Windows MSVC build. --- src/mip/extras/composite_result.hpp | 3 ++- src/mip/extras/index.hpp | 7 +++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/mip/extras/composite_result.hpp b/src/mip/extras/composite_result.hpp index 334ccb79b..82367e4f5 100644 --- a/src/mip/extras/composite_result.hpp +++ b/src/mip/extras/composite_result.hpp @@ -8,6 +8,7 @@ #include +#include namespace mip { @@ -117,7 +118,7 @@ namespace mip template - CompositeResult::Entry runCommand(DeviceInterface& device, const Cmd& cmd, Args&&... args) + CompositeResult::Entry runCommandEx(DeviceInterface& device, const Cmd& cmd, Args&&... args) { CmdResult result = device.runCommand(cmd, std::forward(args)...); diff --git a/src/mip/extras/index.hpp b/src/mip/extras/index.hpp index fecb5a846..3f31deb79 100644 --- a/src/mip/extras/index.hpp +++ b/src/mip/extras/index.hpp @@ -1,5 +1,8 @@ #pragma once +#include +#include + namespace mip { class Id; @@ -40,7 +43,7 @@ namespace mip constexpr bool operator!=(const Index& other) const { return m_index != other.m_index; } constexpr bool isAssigned() const { return m_index != INVALID; } - constexpr bool isValid(unsigned int max_count) const { return m_index < max_count; } + constexpr bool isValid(size_t max_count) const { return m_index < max_count; } constexpr void clear() { m_index = INVALID; } @@ -80,7 +83,7 @@ namespace mip constexpr bool operator!=(const Id& other) const { return m_id != other.m_id; } constexpr bool isAssigned() const { return m_id != INVALID; } - constexpr bool isValid(unsigned int max_count) const { return index() < max_count; } + constexpr bool isValid(size_t max_count) const { return index() < max_count; } constexpr void clear() { m_id = INVALID; } From 675897053c7171bbf1a84df04b6c2a843a222a8a Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 13 Nov 2023 13:14:23 -0500 Subject: [PATCH 157/252] Add diagnostics to help fix Jenkins build. --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 739c839a2..ee5279f6b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,6 +191,7 @@ if(MIP_USE_TCP) "${MIP_DIR}/platform/tcp_connection.cpp" ) endif() +message(STATUS "mip_sdk: MIP_USE_EXTRAS=${MIP_USE_EXTRAS}") if(MIP_USE_EXTRAS) list(APPEND MIP_DEFINES "MIP_USE_EXTRAS") set(MIP_EXTRAS_DIR "${MIP_DIR}/extras") @@ -226,7 +227,7 @@ set(ALL_MIP_SOURCES ${MIP_INTERFACE_SOURCES} ${MIP_EXTRA_SOURCES} ) -message("SRC ${ALL_MIP_SOURCES}") +message(STATUS "mip_sdk: ALL_MIP_SOURCES=${ALL_MIP_SOURCES}") option(MIP_DISABLE_CPP "Excludes all C++ files from the project." OFF) if(MIP_DISABLE_CPP) From 93cc27fba48f843db96127be49d057fdec617964 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 13 Nov 2023 14:06:44 -0500 Subject: [PATCH 158/252] Remove CMake diagnostics. --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee5279f6b..4e6d99555 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,7 +191,6 @@ if(MIP_USE_TCP) "${MIP_DIR}/platform/tcp_connection.cpp" ) endif() -message(STATUS "mip_sdk: MIP_USE_EXTRAS=${MIP_USE_EXTRAS}") if(MIP_USE_EXTRAS) list(APPEND MIP_DEFINES "MIP_USE_EXTRAS") set(MIP_EXTRAS_DIR "${MIP_DIR}/extras") @@ -227,7 +226,6 @@ set(ALL_MIP_SOURCES ${MIP_INTERFACE_SOURCES} ${MIP_EXTRA_SOURCES} ) -message(STATUS "mip_sdk: ALL_MIP_SOURCES=${ALL_MIP_SOURCES}") option(MIP_DISABLE_CPP "Excludes all C++ files from the project." OFF) if(MIP_DISABLE_CPP) From 68203e18d9c0f310b21531bbec6745d78e28ebfe Mon Sep 17 00:00:00 2001 From: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Date: Mon, 4 Dec 2023 14:13:58 -0500 Subject: [PATCH 159/252] Added custom user CmdResult constructor (#81) --- src/mip/mip_result.h | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/mip/mip_result.h b/src/mip/mip_result.h index cd4a2fbd5..8ffca0748 100644 --- a/src/mip/mip_result.h +++ b/src/mip/mip_result.h @@ -91,22 +91,24 @@ struct CmdResult C::mip_cmd_result value = C::MIP_STATUS_NONE; - CmdResult() : value(C::MIP_ACK_OK) {} - CmdResult(C::mip_cmd_result result) : value(result) {} + constexpr CmdResult() : value(C::MIP_ACK_OK) {} + constexpr CmdResult(C::mip_cmd_result result) : value(result) {} + ~CmdResult() = default; CmdResult& operator=(const CmdResult& other) = default; CmdResult& operator=(C::mip_cmd_result other) { value = other; return *this; } - static CmdResult fromAckNack(uint8_t code) { return CmdResult(static_cast(code)); } + static constexpr CmdResult userResult(uint32_t n) { return CmdResult(static_cast(STATUS_USER - n)); } + static constexpr CmdResult fromAckNack(uint8_t code) { return CmdResult(static_cast(code)); } operator const void*() const { return isAck() ? this : nullptr; } bool operator!() const { return !isAck(); } - bool operator==(CmdResult other) const { return value == other.value; } - bool operator!=(CmdResult other) const { return value != other.value; } + constexpr bool operator==(CmdResult other) const { return value == other.value; } + constexpr bool operator!=(CmdResult other) const { return value != other.value; } - bool operator==(C::mip_cmd_result other) const { return value == other; } - bool operator!=(C::mip_cmd_result other) const { return value != other; } + constexpr bool operator==(C::mip_cmd_result other) const { return value == other; } + constexpr bool operator!=(C::mip_cmd_result other) const { return value != other; } const char* name() const { return C::mip_cmd_result_to_string(value); } From ff81e9b180e2439e03962ddbe90c2c403228125d Mon Sep 17 00:00:00 2001 From: David Robbins Date: Mon, 4 Dec 2023 16:54:15 -0500 Subject: [PATCH 160/252] Updated reference frame configuration message to support new Rotation object --- examples/CV7_INS/CV7_INS_simple_example.cpp | 15 +++++++++------ examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 5 +++-- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index b4e6bb1fa..60a43ee68 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -111,30 +111,33 @@ int main(int argc, const char* argv[]) // //External heading sensor reference frame. // - float external_heading_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // External heading sensor is aligned with vehicle frame + commands_aiding::ReferenceFrame::Rotation external_heading_sensor_to_vehicle_frame_rotation; + external_heading_sensor_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // External heading sensor is aligned with vehicle frame float external_heading_sensor_to_vehicle_frame_translation[3] = {0.0, 0.0, 0.0}; // Heading measurements are agnostic to translation, translation set to zero if(commands_aiding::writeReferenceFrame(*device, external_heading_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external heading sensor frame ID"); // //External GNSS antenna reference frame // - float external_gnss_antenna_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero + commands_aiding::ReferenceFrame::Rotation external_gnss_antenna_to_vehicle_frame_rotation; + external_gnss_antenna_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero float external_gnss_antenna_to_vehicle_frame_translation[3] = {0.0, 1.0, 0.0}; // Antenna is translated 1 meter in vehicle frame Y direction if(commands_aiding::writeReferenceFrame(*device, gnss_antenna_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); // //External bodyframe velocity reference frame // - float external_velocity_sensor_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 1.57, 0.0}; // Rotated 90 deg around yaw axis + commands_aiding::ReferenceFrame::Rotation external_velocity_sensor_to_vehicle_frame_rotation; + external_gnss_antenna_to_vehicle_frame_rotation.euler= mip::Vector3f(0.0f, 0.0f, 1.57f); // Rotated 90 deg around yaw axis float external_velocity_sensor_to_vehicle_frame_translation[3] = {1.0, 0.0, 0.0}; // Sensor is translated 1 meter in X direction if(commands_aiding::writeReferenceFrame(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external vehicle frame velocity sensor ID"); diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 8a252c540..ed92e9fc9 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -139,9 +139,10 @@ int main(int argc, const char* argv[]) // //External GNSS antenna reference frame // - float external_gnss_antenna_to_vehicle_frame_rotation_euler[4] = {0.0, 0.0, 0.0, 0.0}; // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero + commands_aiding::ReferenceFrame::Rotation external_gnss_antenna_to_vehicle_frame_rotation; + external_gnss_antenna_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero if(commands_aiding::writeReferenceFrame(*device, gnss_antenna_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - input_arguments.gnss_antenna_lever_arm, external_gnss_antenna_to_vehicle_frame_rotation_euler) != CmdResult::ACK_OK) + input_arguments.gnss_antenna_lever_arm, external_gnss_antenna_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); From d5a383f8f996233c9fd26ea50350597010cc9a31 Mon Sep 17 00:00:00 2001 From: Rob Fisher Date: Wed, 13 Dec 2023 16:03:02 -0500 Subject: [PATCH 161/252] Updates Jenkinsfile for new Jenkins setup --- Jenkinsfile | 38 ++++++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 4b0051ceb..6cba57559 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -17,7 +17,7 @@ def checkoutRepo() { branches: [ [name: 'refs/heads/' + BRANCH_NAME_REAL] ], - userRemoteConfigs: [[credentialsId: 'ffc94480-3383-4390-82e6-af2fb5e6c76d', url: 'https://github.com/LORD-MicroStrain/libmip.git']], + userRemoteConfigs: [[credentialsId: 'Github_User_And_Token', url: 'https://github.com/LORD-MicroStrain/libmip.git']], extensions: [ ] ]) @@ -39,6 +39,7 @@ pipeline { cd build_Win32 cmake .. ` -A "Win32" ` + -T "v142" ` -DBUILD_DOCUMENTATION=ON ` -DBUILD_PACKAGE=ON cmake --build . -j @@ -58,6 +59,7 @@ pipeline { cd build_x64 cmake .. ` -A "x64" ` + -T "v142" ` -DBUILD_PACKAGE=ON cmake --build . -j cmake --build . --target package @@ -66,25 +68,41 @@ pipeline { } } stage('Ubuntu amd64') { - agent { label 'linux-amd64' } + agent { label 'aws-amd64' } options { skipDefaultCheckout() } steps { checkoutRepo() - sh "cp /etc/pki/ca-trust/source/anchors/ZScaler.crt ./.devcontainer/extra_cas/" sh "./.devcontainer/docker_build.sh --os ubuntu --arch amd64" archiveArtifacts artifacts: 'build_ubuntu_amd64/mipsdk_*' } } stage('Centos amd64') { - agent { label 'linux-amd64' } + agent { label 'aws-amd64' } options { skipDefaultCheckout() } steps { checkoutRepo() - sh "cp /etc/pki/ca-trust/source/anchors/ZScaler.crt ./.devcontainer/extra_cas/" sh "./.devcontainer/docker_build.sh --os centos --arch amd64" archiveArtifacts artifacts: 'build_centos_amd64/mipsdk_*' } } + stage('Ubuntu arm64') { + agent { label 'aws-arm64' } + options { skipDefaultCheckout() } + steps { + checkoutRepo() + sh "./.devcontainer/docker_build.sh --os ubuntu --arch arm64v8" + archiveArtifacts artifacts: 'build_ubuntu_arm64v8/mipsdk_*' + } + } + stage('Ubuntu arm32') { + agent { label 'aws-arm64' } + options { skipDefaultCheckout() } + steps { + checkoutRepo() + sh "./.devcontainer/docker_build.sh --os ubuntu --arch arm32v7" + archiveArtifacts artifacts: 'build_ubuntu_arm32v7/mipsdk_*' + } + } } } } @@ -92,11 +110,11 @@ pipeline { success { script { if (BRANCH_NAME && BRANCH_NAME == 'develop') { - node("linux-amd64") { - withCredentials([string(credentialsId: 'MICROSTRAIN_BUILD_GH_TOKEN', variable: 'GH_TOKEN')]) { + node("master") { + withCredentials([string(credentialsId: 'Github_Token', variable: 'GH_TOKEN')]) { sh ''' # Release to github - archive_dir="${WORKSPACE}/../builds/${BUILD_NUMBER}/archive/" + archive_dir="${WORKSPACE}/../../jobs/LORD-MicroStrain/jobs/mip_sdk/branches/${BRANCH_NAME}/builds/${BUILD_NUMBER}/archive/" ./scripts/release.sh \ --artifacts "$(find "${archive_dir}" -type f)" \ --target "${BRANCH_NAME}" \ @@ -107,11 +125,11 @@ pipeline { } } } else if (BRANCH_NAME && BRANCH_NAME == 'master') { - node("linux-amd64") { + node("master") { withCredentials([string(credentialsId: 'MICROSTRAIN_BUILD_GH_TOKEN', variable: 'GH_TOKEN')]) { sh ''' # Release to the latest version if the master commit matches up with the commit of that version - archive_dir="${WORKSPACE}/../builds/${BUILD_NUMBER}/archive/" + archive_dir="${WORKSPACE}/../../jobs/LORD-MicroStrain/jobs/mip_sdk/branches/${BRANCH_NAME}/builds/${BUILD_NUMBER}/archive/" if git describe --exact-match --tags HEAD &> /dev/null; then # Publish a release ./scripts/release.sh \ From 817094c6bb5b918cc342c59caa221b4adf8b3581 Mon Sep 17 00:00:00 2001 From: Rob Fisher Date: Wed, 13 Dec 2023 17:24:56 -0500 Subject: [PATCH 162/252] Adds MSVC version to produced packages --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a9b380f6a..469a99d35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -499,8 +499,8 @@ if(BUILD_PACKAGE) # Zip specific configuration # Build different zip packages for each target - if(WIN32) - set(CPACK_ARCHIVE_MIP_FILE_NAME "${MIP_FILE_NAME_PREFIX}_Windows") + if(MSVC) + set(CPACK_ARCHIVE_MIP_FILE_NAME "${MIP_FILE_NAME_PREFIX}_MSVC_v${MSVC_TOOLSET_VERSION}") elseif(APPLE) set(CPACK_ARCHIVE_MIP_FILE_NAME "${MIP_FILE_NAME_PREFIX}_OSX") elseif(UNIX) From 23091d87512ba465df74e8c7b7bc4c5d9237d14c Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 14 Dec 2023 13:49:19 +0000 Subject: [PATCH 163/252] Generate MIP definitions from branches/dev@55704. --- src/mip/definitions/commands_aiding.c | 163 ++++++++++++++++++++++ src/mip/definitions/commands_aiding.cpp | 146 ++++++++++++++++++++ src/mip/definitions/commands_aiding.h | 106 +++++++++++++- src/mip/definitions/commands_aiding.hpp | 175 +++++++++++++++++++++++- src/mip/definitions/data_gnss.h | 12 +- src/mip/definitions/data_gnss.hpp | 12 +- 6 files changed, 600 insertions(+), 14 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index fd6c965a8..1f9327b6d 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -665,6 +665,102 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_aiding_height_command(mip_serializer* serializer, const mip_aiding_height_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_float(serializer, self->height); + + insert_float(serializer, self->uncertainty); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_aiding_height_command(mip_serializer* serializer, mip_aiding_height_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_float(serializer, &self->height); + + extract_float(serializer, &self->uncertainty); + + extract_u16(serializer, &self->valid_flags); + +} + +mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float height, float uncertainty, uint16_t valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + insert_float(&serializer, height); + + insert_float(&serializer, uncertainty); + + insert_u16(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_aiding_pressure_command(mip_serializer* serializer, const mip_aiding_pressure_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_float(serializer, self->pressure); + + insert_float(serializer, self->uncertainty); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_aiding_pressure_command(mip_serializer* serializer, mip_aiding_pressure_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_float(serializer, &self->pressure); + + extract_float(serializer, &self->uncertainty); + + extract_u16(serializer, &self->valid_flags); + +} + +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float pressure, float uncertainty, uint16_t valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + insert_float(&serializer, pressure); + + insert_float(&serializer, uncertainty); + + insert_u16(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_aiding_ecef_vel_command* self) { insert_mip_time(serializer, &self->time); @@ -914,6 +1010,73 @@ mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_t return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_aiding_magnetic_field_command(mip_serializer* serializer, const mip_aiding_magnetic_field_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_magnetic_field_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_magnetic_field_command(mip_serializer* serializer, mip_aiding_magnetic_field_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_magnetic_field_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_magnetic_field_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, mip_aiding_magnetic_field_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + assert(magnetic_field || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, magnetic_field[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_magnetic_field_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_MAGNETIC_FIELD, buffer, (uint8_t)mip_serializer_length(&serializer)); +} #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 5761de4dc..a5eb60002 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -575,6 +575,98 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const Height& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.height); + + insert(serializer, self.uncertainty); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, Height& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.height); + + extract(serializer, self.uncertainty); + + extract(serializer, self.valid_flags); + +} + +CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + insert(serializer, height); + + insert(serializer, uncertainty); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const Pressure& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.pressure); + + insert(serializer, self.uncertainty); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, Pressure& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.pressure); + + extract(serializer, self.uncertainty); + + extract(serializer, self.valid_flags); + +} + +CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + insert(serializer, pressure); + + insert(serializer, uncertainty); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const EcefVel& self) { insert(serializer, self.time); @@ -783,6 +875,60 @@ CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensor return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const MagneticField& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, MagneticField& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + assert(magneticField || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, magneticField[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_FIELD, buffer, (uint8_t)mip_serializer_length(&serializer)); +} } // namespace commands_aiding } // namespace mip diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index 2befa76cb..f8386599d 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -42,11 +42,13 @@ enum MIP_CMD_DESC_AIDING_POS_LLH = 0x22, MIP_CMD_DESC_AIDING_HEIGHT_ABS = 0x23, MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x24, + MIP_CMD_DESC_AIDING_PRESSURE = 0x25, MIP_CMD_DESC_AIDING_VEL_ECEF = 0x28, MIP_CMD_DESC_AIDING_VEL_NED = 0x29, MIP_CMD_DESC_AIDING_VEL_ODOM = 0x2A, MIP_CMD_DESC_AIDING_WHEELSPEED = 0x2B, MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x31, + MIP_CMD_DESC_AIDING_MAGNETIC_FIELD = 0x32, MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x38, MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x39, MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x3A, @@ -120,6 +122,28 @@ mip_cmd_result mip_aiding_default_sensor_frame_mapping(struct mip_interface* dev /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_reference_frame (0x13,0x01) Reference Frame [C] +/// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command +/// should mirror the frame ID used in the aiding command (if that aiding measurement is measured in this reference frame) +/// +/// This transform satisfies the following relationship: +/// +/// EQSTART p^{veh} = R p^{sensor_frame} + t EQEND
+/// +/// Where:
+/// EQSTART R EQEND is rotation matrix defined by the rotation component and EQSTART t EQEND is the translation vector

+/// EQSTART p^{sensor_frame} EQEND is a 3-element position vector expressed in the external sensor frame
+/// EQSTART p^{veh} EQEND is a 3-element position vector expressed in the vehicle frame
+/// +/// Rotation can be defined using Euler angles OR quaternions. If Format selector is set to Euler Angles, the fourth element +/// in the rotation vector is ignored and should be set to 0. +/// +/// Example: GNSS antenna lever arm +/// +/// Frame ID: 1 +/// Format: 1 (Euler) +/// Translation: [0,1,] (GNSS with a 1 meter Y offset in the vehicle frame) +/// Rotation: [0,0,0,0] (Rotational component is not relevant for GNSS measurements, set to zero) +/// /// ///@{ @@ -172,6 +196,7 @@ mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [C] +/// Controls command response behavior to external aiding commands /// ///@{ @@ -277,6 +302,52 @@ void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* seria mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_height (0x13,0x23) Height [C] +/// Estimated value of height. +/// +///@{ + +struct mip_aiding_height_command +{ + mip_time time; + uint8_t sensor_id; + float height; ///< [m] + float uncertainty; ///< [m] + uint16_t valid_flags; + +}; +typedef struct mip_aiding_height_command mip_aiding_height_command; +void insert_mip_aiding_height_command(struct mip_serializer* serializer, const mip_aiding_height_command* self); +void extract_mip_aiding_height_command(struct mip_serializer* serializer, mip_aiding_height_command* self); + +mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float height, float uncertainty, uint16_t valid_flags); + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_pressure (0x13,0x25) Pressure [C] +/// Estimated value of air pressure. +/// +///@{ + +struct mip_aiding_pressure_command +{ + mip_time time; + uint8_t sensor_id; + float pressure; ///< [mbar] + float uncertainty; ///< [mbar] + uint16_t valid_flags; + +}; +typedef struct mip_aiding_pressure_command mip_aiding_pressure_command; +void insert_mip_aiding_pressure_command(struct mip_serializer* serializer, const mip_aiding_pressure_command* self); +void extract_mip_aiding_pressure_command(struct mip_serializer* serializer, mip_aiding_pressure_command* self); + +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float pressure, float uncertainty, uint16_t valid_flags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -364,7 +435,7 @@ struct mip_aiding_vehicle_fixed_frame_velocity_command mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) mip_vector3f velocity; ///< [m/s] - mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; }; @@ -399,6 +470,39 @@ void extract_mip_aiding_true_heading_command(struct mip_serializer* serializer, mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_magnetic_field (0x13,0x32) Magnetic Field [C] +/// Estimate of magnetic field in the frame associated with the given sensor ID. +/// +///@{ + +typedef uint16_t mip_aiding_magnetic_field_command_valid_flags; +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_ALL = 0x0007; + +struct mip_aiding_magnetic_field_command +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + mip_vector3f magnetic_field; ///< [G] + mip_vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) + mip_aiding_magnetic_field_command_valid_flags valid_flags; + +}; +typedef struct mip_aiding_magnetic_field_command mip_aiding_magnetic_field_command; +void insert_mip_aiding_magnetic_field_command(struct mip_serializer* serializer, const mip_aiding_magnetic_field_command* self); +void extract_mip_aiding_magnetic_field_command(struct mip_serializer* serializer, mip_aiding_magnetic_field_command* self); + +void insert_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_magnetic_field_command_valid_flags self); +void extract_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, mip_aiding_magnetic_field_command_valid_flags* self); + +mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags); + ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 0ab72d5ba..5a72202c9 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -41,11 +41,13 @@ enum CMD_POS_LLH = 0x22, CMD_HEIGHT_ABS = 0x23, CMD_HEIGHT_REL = 0x24, + CMD_PRESSURE = 0x25, CMD_VEL_ECEF = 0x28, CMD_VEL_NED = 0x29, CMD_VEL_ODOM = 0x2A, CMD_WHEELSPEED = 0x2B, CMD_HEADING_TRUE = 0x31, + CMD_MAGNETIC_FIELD = 0x32, CMD_DELTA_POSITION = 0x38, CMD_DELTA_ATTITUDE = 0x39, CMD_LOCAL_ANGULAR_RATE = 0x3A, @@ -156,6 +158,28 @@ CmdResult defaultSensorFrameMapping(C::mip_interface& device); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_reference_frame (0x13,0x01) Reference Frame [CPP] +/// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command +/// should mirror the frame ID used in the aiding command (if that aiding measurement is measured in this reference frame) +/// +/// This transform satisfies the following relationship: +/// +/// EQSTART p^{veh} = R p^{sensor_frame} + t EQEND
+/// +/// Where:
+/// EQSTART R EQEND is rotation matrix defined by the rotation component and EQSTART t EQEND is the translation vector

+/// EQSTART p^{sensor_frame} EQEND is a 3-element position vector expressed in the external sensor frame
+/// EQSTART p^{veh} EQEND is a 3-element position vector expressed in the vehicle frame
+/// +/// Rotation can be defined using Euler angles OR quaternions. If Format selector is set to Euler Angles, the fourth element +/// in the rotation vector is ignored and should be set to 0. +/// +/// Example: GNSS antenna lever arm +/// +/// Frame ID: 1 +/// Format: 1 (Euler) +/// Translation: [0,1,] (GNSS with a 1 meter Y offset in the vehicle frame) +/// Rotation: [0,0,0,0] (Rotational component is not relevant for GNSS measurements, set to zero) +/// /// ///@{ @@ -247,6 +271,7 @@ CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [CPP] +/// Controls command response behavior to external aiding commands /// ///@{ @@ -464,6 +489,84 @@ void extract(Serializer& serializer, LlhPos& self); CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_height (0x13,0x23) Height [CPP] +/// Estimated value of height. +/// +///@{ + +struct Height +{ + Time time; + uint8_t sensor_id = 0; + float height = 0; ///< [m] + float uncertainty = 0; ///< [m] + uint16_t valid_flags = 0; + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEIGHT_ABS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,height,uncertainty,valid_flags); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); + } + typedef void Response; +}; +void insert(Serializer& serializer, const Height& self); +void extract(Serializer& serializer, Height& self); + +CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_pressure (0x13,0x25) Pressure [CPP] +/// Estimated value of air pressure. +/// +///@{ + +struct Pressure +{ + Time time; + uint8_t sensor_id = 0; + float pressure = 0; ///< [mbar] + float uncertainty = 0; ///< [mbar] + uint16_t valid_flags = 0; + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_PRESSURE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,pressure,uncertainty,valid_flags); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(pressure),std::ref(uncertainty),std::ref(valid_flags)); + } + typedef void Response; +}; +void insert(Serializer& serializer, const Pressure& self); +void extract(Serializer& serializer, Pressure& self); + +CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -649,7 +752,7 @@ struct VehicleFixedFrameVelocity Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) Vector3f velocity; ///< [m/s] - Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; @@ -713,6 +816,76 @@ void extract(Serializer& serializer, TrueHeading& self); CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_magnetic_field (0x13,0x32) Magnetic Field [CPP] +/// Estimate of magnetic field in the frame associated with the given sensor ID. +/// +///@{ + +struct MagneticField +{ + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + X = 0x0001, ///< + Y = 0x0002, ///< + Z = 0x0004, ///< + ALL = 0x0007, + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool x() const { return (value & X) > 0; } + void x(bool val) { if(val) value |= X; else value &= ~X; } + bool y() const { return (value & Y) > 0; } + void y(bool val) { if(val) value |= Y; else value &= ~Y; } + bool z() const { return (value & Z) > 0; } + void z(bool val) { if(val) value |= Z; else value &= ~Z; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + Vector3f magnetic_field; ///< [G] + Vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) + ValidFlags valid_flags; + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_MAGNETIC_FIELD; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,magnetic_field,uncertainty,valid_flags); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(magnetic_field),std::ref(uncertainty),std::ref(valid_flags)); + } + typedef void Response; +}; +void insert(Serializer& serializer, const MagneticField& self); +void extract(Serializer& serializer, MagneticField& self); + +CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); + ///@} /// diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index 9417718d7..8ec56edfd 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -368,12 +368,12 @@ static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLA struct mip_gnss_utc_time_data { uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint32_t msec; ///< [Milliseconds] + uint8_t month; ///< Month (1-12) + uint8_t day; ///< Day (1-31) + uint8_t hour; ///< Hour (0-23) + uint8_t min; ///< Minute (0-59) + uint8_t sec; ///< Second (0-59) + uint32_t msec; ///< Millisecond(0-999) mip_gnss_utc_time_data_valid_flags valid_flags; }; diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index b7cc0c6c9..035fbaba7 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -577,12 +577,12 @@ struct UtcTime }; uint16_t year = 0; - uint8_t month = 0; - uint8_t day = 0; - uint8_t hour = 0; - uint8_t min = 0; - uint8_t sec = 0; - uint32_t msec = 0; ///< [Milliseconds] + uint8_t month = 0; ///< Month (1-12) + uint8_t day = 0; ///< Day (1-31) + uint8_t hour = 0; ///< Hour (0-23) + uint8_t min = 0; ///< Minute (0-59) + uint8_t sec = 0; ///< Second (0-59) + uint32_t msec = 0; ///< Millisecond(0-999) ValidFlags valid_flags; static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; From e6374543026b6d633028ec3b993dc8c36dad8a03 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 18 Dec 2023 15:11:02 +0000 Subject: [PATCH 164/252] Generate MIP definitions from branches/dev@55741. --- src/mip/definitions/commands_filter.h | 16 +++++++++------- src/mip/definitions/commands_filter.hpp | 16 +++++++++------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index bb8f91c9f..e8ca1ccb3 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -1757,13 +1757,15 @@ mip_cmd_result mip_filter_default_mag_dip_angle_error_adaptive_measurement(struc ///@{ typedef uint16_t mip_filter_aiding_measurement_enable_command_aiding_source; -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_POS_VEL = 0; ///< GNSS Position and Velocity -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING = 1; ///< GNSS Heading (dual antenna) -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALTIMETER = 2; ///< Altimeter -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_SPEED = 3; ///< Speed sensor / Odometer -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER = 4; ///< Magnetometer -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING = 5; ///< External heading input -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALL = 65535; ///< Save/load/reset all options +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_POS_VEL = 0; ///< GNSS Position and Velocity +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING = 1; ///< GNSS Heading (dual antenna) +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALTIMETER = 2; ///< Pressure altimeter (built-in sensor) +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_SPEED = 3; ///< Speed sensor / Odometer +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER = 4; ///< Magnetometer (built-in sensor) +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING = 5; ///< External heading input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_ALTIMETER = 6; ///< External pressure altimeter input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER = 7; ///< External magnetomer input input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALL = 65535; ///< Save/load/reset all options struct mip_filter_aiding_measurement_enable_command { diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 1451f409c..9d305f2a9 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -3175,13 +3175,15 @@ struct AidingMeasurementEnable { enum class AidingSource : uint16_t { - GNSS_POS_VEL = 0, ///< GNSS Position and Velocity - GNSS_HEADING = 1, ///< GNSS Heading (dual antenna) - ALTIMETER = 2, ///< Altimeter - SPEED = 3, ///< Speed sensor / Odometer - MAGNETOMETER = 4, ///< Magnetometer - EXTERNAL_HEADING = 5, ///< External heading input - ALL = 65535, ///< Save/load/reset all options + GNSS_POS_VEL = 0, ///< GNSS Position and Velocity + GNSS_HEADING = 1, ///< GNSS Heading (dual antenna) + ALTIMETER = 2, ///< Pressure altimeter (built-in sensor) + SPEED = 3, ///< Speed sensor / Odometer + MAGNETOMETER = 4, ///< Magnetometer (built-in sensor) + EXTERNAL_HEADING = 5, ///< External heading input + EXTERNAL_ALTIMETER = 6, ///< External pressure altimeter input + EXTERNAL_MAGNETOMETER = 7, ///< External magnetomer input input + ALL = 65535, ///< Save/load/reset all options }; FunctionSelector function = static_cast(0); From 3a8ad57b8c9a7034b535d5d179135c05eb6cb19b Mon Sep 17 00:00:00 2001 From: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Date: Mon, 8 Jan 2024 14:03:04 -0500 Subject: [PATCH 165/252] Update .gitignore Added .vs directory --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 929cc6600..79868ed81 100644 --- a/.gitignore +++ b/.gitignore @@ -3,5 +3,6 @@ build-doc/ .vscode/ int/ .idea/ +.vs/ -src/mip/mip_version.h \ No newline at end of file +src/mip/mip_version.h From bf4d318a1a383a76b5db7b6b0ff9c72613492b86 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 10:19:36 -0500 Subject: [PATCH 166/252] Fixed typo under 'TCP Client' section in README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 011fde1c0..57a720202 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ Enable it in the CMake configuration with `-DMIP_USE_SERIAL=1`. ### TCP Client The TCP client connection allows you to connect to a MIP device remotely. The MIP device must be connected -via the normal serial or USB cable to a commputer system running a TCP server which forwards data between +via the normal serial or USB cable to a computer system running a TCP server which forwards data between the serial port and TCP clients. Enable it in the CMake configuration with `-DMIP_USE_TCP=1`. From a3825974b4e04c17ecfcacb1093679b824a56e4d Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 10:42:21 -0500 Subject: [PATCH 167/252] Fixed exception when running without args or with the '-h' flag for GQ7_example.cpp --- examples/GQ7/GQ7_example.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index e1390b016..84602e25b 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -82,9 +82,17 @@ bool should_exit(); int main(int argc, const char* argv[]) { - std::unique_ptr utils = handleCommonArgs(argc, argv); - std::unique_ptr& device = utils->device; + std::unique_ptr utils; + try { + utils = handleCommonArgs(argc, argv); + } catch(const std::underflow_error& ex) { + return printCommonUsage(argv); + } catch(const std::exception& ex) { + fprintf(stderr, "Error: %s\n", ex.what()); + return 1; + } + std::unique_ptr& device = utils->device; printf("Connecting to and configuring sensor.\n"); // From 5642b4397261b09fd31d7b06eff8a4ca85bbd9e8 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 10:46:29 -0500 Subject: [PATCH 168/252] Fixed exception when running without args or with the '-h' flag for GQ7_example.cpp --- examples/GQ7/GQ7_example.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index e1390b016..84602e25b 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -82,9 +82,17 @@ bool should_exit(); int main(int argc, const char* argv[]) { - std::unique_ptr utils = handleCommonArgs(argc, argv); - std::unique_ptr& device = utils->device; + std::unique_ptr utils; + try { + utils = handleCommonArgs(argc, argv); + } catch(const std::underflow_error& ex) { + return printCommonUsage(argv); + } catch(const std::exception& ex) { + fprintf(stderr, "Error: %s\n", ex.what()); + return 1; + } + std::unique_ptr& device = utils->device; printf("Connecting to and configuring sensor.\n"); // From a247c20baf19857ad1ac69117c5b6747f883d065 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 11:00:37 -0500 Subject: [PATCH 169/252] Fixed exception when running without args or with the '-h' flag in GX5_45_example.cpp --- examples/GX5_45/GX5_45_example.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index 2305f2e69..9443eb72b 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -82,9 +82,17 @@ bool should_exit(); int main(int argc, const char* argv[]) { - std::unique_ptr utils = handleCommonArgs(argc, argv); - std::unique_ptr& device = utils->device; + std::unique_ptr utils; + try { + utils = handleCommonArgs(argc, argv); + } catch(const std::underflow_error& ex) { + return printCommonUsage(argv); + } catch(const std::exception& ex) { + fprintf(stderr, "Error: %s\n", ex.what()); + return 1; + } + std::unique_ptr& device = utils->device; printf("Connecting to and configuring sensor.\n"); // From 080d375286c212ba8511bd73306e55cf7f66289c Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 11:23:01 -0500 Subject: [PATCH 170/252] Fixed exception when running without args or with the '-h' flag in CV7_example.cpp --- examples/CV7/CV7_example.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index a47a81c21..6a8d14019 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -76,7 +76,16 @@ void handleFilterEventSource(void*, const mip::Field& field, mip::Timestamp time int main(int argc, const char* argv[]) { - std::unique_ptr utils = handleCommonArgs(argc, argv); + std::unique_ptr utils; + try { + utils = handleCommonArgs(argc, argv); + } catch(const std::underflow_error& ex) { + return printCommonUsage(argv); + } catch(const std::exception& ex) { + fprintf(stderr, "Error: %s\n", ex.what()); + return 1; + } + std::unique_ptr& device = utils->device; // From 505a5a4974a665c07eee5a0201eeedfb6978ae98 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 11:27:39 -0500 Subject: [PATCH 171/252] Fixed exception when running without args or with the '-h' flag in CV7_INS_simple_example.cpp --- examples/CV7_INS/CV7_INS_simple_example.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index b4e6bb1fa..b700ef72f 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -67,7 +67,16 @@ bool should_exit(); int main(int argc, const char* argv[]) { - std::unique_ptr utils = handleCommonArgs(argc, argv); + std::unique_ptr utils; + try { + utils = handleCommonArgs(argc, argv); + } catch(const std::underflow_error& ex) { + return printCommonUsage(argv); + } catch(const std::exception& ex) { + fprintf(stderr, "Error: %s\n", ex.what()); + return 1; + } + std::unique_ptr& device = utils->device; // From dd8e63cb36e8bd50a4e66360af3c3e7a0cca4cfd Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 12:14:53 -0500 Subject: [PATCH 172/252] Removed unnecessary help flag check --- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 8a252c540..11625c60f 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -410,20 +410,11 @@ void print_device_information(const commands_base::BaseDeviceInfo& device_info) InputArguments parse_input_arguments(int argc, const char* argv[]) { + // TODO: Set max arg check for this. if (argc < 8) { usage(argv[0]); - exit_gracefully("ERROR: Incorrect input arguments"); - } - - // Look for help flag - for (int i = 1; i < argc; i++) - { - if(strcmp(argv[i], "-h") == 0) - { - usage(argv[0]); - exit_gracefully(""); - } + exit_gracefully(nullptr); } InputArguments input_arguments; From 919551e34343018029809a1de686c84b91e97294 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 12:18:15 -0500 Subject: [PATCH 173/252] Added notification to user to press enter upon exit in the simple ublox example --- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 11625c60f..fe1620c9b 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -482,6 +482,7 @@ void exit_gracefully(const char *message) printf("%s\n", message); #ifdef _WIN32 + std::cout << "Press ENTER to exit..." << std::endl; int dummy = getchar(); #endif From 2b53723016535245f4fe03fb183eb86fe70ecfac Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 17:03:44 -0500 Subject: [PATCH 174/252] Added links to the example code files in README --- README.md | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 57a720202..5ce984a07 100644 --- a/README.md +++ b/README.md @@ -26,12 +26,20 @@ Features Examples -------- -* Get device information [C++] - queries the device strings and prints them to stdout. -* Watch IMU [C, C++] - Configures the IMU for streaming and prints the data to stdout. -* Product-specific examples: - * GQ7 setup [C, C++] - Configures the device for typical usage in a wheeled-vehicle application. - * CV7 setup [C, C++] - Configures the device for typical usage and includes an example of using the event system. - * GX5-45 setup [C, C++] - Configures the device for typical usage in a wheeled-vehicle application. + +* **Get device information** [[C++](./examples/device_info.cpp)]. +* **Watch IMU** [[C](./examples/watch_imu.c) | [C++](./examples/watch_imu.cpp)]. +* **Threading** [[C++](./examples/threading.cpp)]. +* **Ping** [[C++](./examples/ping.cpp)]. +* **Product-specific examples:** + * **GQ7 setup** [[C](./examples/GQ7/GQ7_example.c), [C++](./examples/GQ7/GQ7_example.cpp)]. + * **CV7 setup** [[C](./examples/CV7/CV7_example.c), [C++](./examples/CV7/CV7_example.cpp)]. + * **GX5-45 setup** [[C](./examples/GX5_45/GX5_45_example.c), [C++](./examples/GX5_45/GX5_45_example.cpp)]. + * **CV7_INS setup** [[C++](./examples/CV7_INS/CV7_INS_simple_example.cpp)]. + * **CV7_INS with UBlox setup** [[C++](./examples/CV7_INS/CV7_INS_simple_ublox_example.cpp)]. You'll need to enable at least one of the communications interfaces in the CMake configuration (see below) to use the examples. From 4b88ced3924c3ef70163e8bd290e793dbe6248eb Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 17:06:53 -0500 Subject: [PATCH 175/252] Added hyperlink to the communication interfaces section from the examples section in README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 5ce984a07..43e0cb59d 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ Examples * **CV7_INS setup** [[C++](./examples/CV7_INS/CV7_INS_simple_example.cpp)]. * **CV7_INS with UBlox setup** [[C++](./examples/CV7_INS/CV7_INS_simple_ublox_example.cpp)]. -You'll need to enable at least one of the communications interfaces in the CMake configuration (see below) to use the examples. +You'll need to enable at least one of the [communications interfaces](#communications-interfaces) in the CMake configuration to use the examples. The examples take two parameters for the device connection: * For a serial connection: Port and baudrate. Port must start with `/dev/` on Linux or `COM` on Windows. @@ -51,7 +51,7 @@ The examples take two parameters for the device connection: Documentation ------------- -Documentation for all released versions can be found [here](https://lord-microstrain.github.io/mip_sdk_documentation) +Documentation for all released versions can be found [here](https://lord-microstrain.github.io/mip_sdk_documentation). Communications Interfaces From 03746d963946a44d929706afd7f8d2d302195cc5 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Wed, 10 Jan 2024 17:09:41 -0500 Subject: [PATCH 176/252] Added hyperlinks to the build configuration section from the communication interfaces section in README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 43e0cb59d..48f16c55e 100644 --- a/README.md +++ b/README.md @@ -65,7 +65,7 @@ A basic serial port interface is provided in C and C++ for Linux and Windows. Th The serial port connection will be used in most cases, when the MIP device is connected via a serial or USB cable (the USB connection acts like a virtual serial port). -Enable it in the CMake configuration with `-DMIP_USE_SERIAL=1`. +[Enable it](#build-configuration) in the CMake configuration with `-DMIP_USE_SERIAL=1`. ### TCP Client @@ -73,7 +73,7 @@ The TCP client connection allows you to connect to a MIP device remotely. The MI via the normal serial or USB cable to a computer system running a TCP server which forwards data between the serial port and TCP clients. -Enable it in the CMake configuration with `-DMIP_USE_TCP=1`. +[Enable it](#build-configuration) in the CMake configuration with `-DMIP_USE_TCP=1`. How to Build From 414051b4929b0d259e7c75ec152955a4e60de2ef Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Fri, 12 Jan 2024 16:17:26 -0500 Subject: [PATCH 177/252] Added filter state information output to GQ7_example.cpp --- examples/GQ7/GQ7_example.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index 84602e25b..2310f31e4 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -296,10 +296,34 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter Full Navigation mode.\n"); + std::string current_state = ""; + std::string read_state = ""; while(running) { device->update(); + // Display current state for informational purposes. + switch (filter_status.filter_state) { + case data_filter::FilterMode::INIT: + read_state = "INIT"; + break; + case data_filter::FilterMode::VERT_GYRO: + read_state = "VERT_GYRO"; + break; + case data_filter::FilterMode::AHRS: + read_state = "AHRS"; + break; + case data_filter::FilterMode::FULL_NAV: + read_state = "FULL_NAV"; + break; + default: + read_state = "STARTUP"; + break; + } + if (read_state != current_state) { + std::cout << "Filter state: " << read_state << std::endl; + current_state = read_state; + } //Check GNSS fixes and alert the user when they become valid for(int i=0; i<2; i++) From 3b07669bde5acdd451671cf39a3e3534a4b1ee45 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Fri, 12 Jan 2024 17:36:03 -0500 Subject: [PATCH 178/252] Added 'Enter' message to exit_gracefully in GQ7_example.cpp --- examples/GQ7/GQ7_example.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index 2310f31e4..0ad40814e 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -294,15 +294,15 @@ int main(int argc, const char* argv[]) bool running = true; mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); - printf("Sensor is configured... waiting for filter to enter Full Navigation mode.\n"); + printf("Sensor is configured... waiting for filter to enter Full Navigation mode (FULL_NAV).\n"); - std::string current_state = ""; - std::string read_state = ""; - while(running) - { + auto current_state = ""; + auto read_state = ""; + while(running) { device->update(); // Display current state for informational purposes. + // TODO: Put into utility function. switch (filter_status.filter_state) { case data_filter::FilterMode::INIT: read_state = "INIT"; @@ -389,6 +389,7 @@ void exit_gracefully(const char *message) printf("%s\n", message); #ifdef _WIN32 + std::cout << "Press ENTER to exit..." << std::endl; int dummy = getchar(); #endif From 0812cad185ddd9bc353d31447eb22db03d167bee Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 10:56:16 -0500 Subject: [PATCH 179/252] Moved filter state display logic to utility function --- examples/GQ7/GQ7_example.cpp | 28 ++-------------------------- examples/example_utils.cpp | 24 ++++++++++++++++++++++++ examples/example_utils.hpp | 4 ++++ 3 files changed, 30 insertions(+), 26 deletions(-) diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index 0ad40814e..8319023c4 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -296,34 +296,10 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter Full Navigation mode (FULL_NAV).\n"); - auto current_state = ""; - auto read_state = ""; + auto current_state = std::string{""}; while(running) { device->update(); - - // Display current state for informational purposes. - // TODO: Put into utility function. - switch (filter_status.filter_state) { - case data_filter::FilterMode::INIT: - read_state = "INIT"; - break; - case data_filter::FilterMode::VERT_GYRO: - read_state = "VERT_GYRO"; - break; - case data_filter::FilterMode::AHRS: - read_state = "AHRS"; - break; - case data_filter::FilterMode::FULL_NAV: - read_state = "FULL_NAV"; - break; - default: - read_state = "STARTUP"; - break; - } - if (read_state != current_state) { - std::cout << "Filter state: " << read_state << std::endl; - current_state = read_state; - } + displayFilterState(filter_status.filter_state, current_state); //Check GNSS fixes and alert the user when they become valid for(int i=0; i<2; i++) diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index c4bde773b..6e82f6adc 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -136,3 +136,27 @@ int printCommonUsage(const char* argv[]) return 1; } +void displayFilterState(const mip::data_filter::FilterMode &filter_state, std::string ¤t_state) { + std::string read_state = ""; + switch (filter_state) { + case mip::data_filter::FilterMode::INIT: + read_state = "INIT"; + break; + case mip::data_filter::FilterMode::VERT_GYRO: + read_state = "VERT_GYRO"; + break; + case mip::data_filter::FilterMode::AHRS: + read_state = "AHRS"; + break; + case mip::data_filter::FilterMode::FULL_NAV: + read_state = "FULL_NAV"; + break; + default: + read_state = "STARTUP"; + break; + } + if (read_state != current_state) { + std::cout << "Filter state: " << read_state << std::endl; + current_state = read_state; + } +} diff --git a/examples/example_utils.hpp b/examples/example_utils.hpp index a95376728..ef6b61e57 100644 --- a/examples/example_utils.hpp +++ b/examples/example_utils.hpp @@ -13,6 +13,7 @@ #include +#include #include #include @@ -34,3 +35,6 @@ std::unique_ptr openFromArgs(const std::string& port_or_hostname, std::unique_ptr handleCommonArgs(int argc, const char* argv[], int maxArgs=4); int printCommonUsage(const char* argv[]); + +/// Displays current filter state for the connected device if it has changed. +void displayFilterState(const mip::data_filter::FilterMode &filter_status, std::string ¤t_state); \ No newline at end of file From e31f71abc6262976f8b0a178fc2b212958d83142 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 11:27:02 -0500 Subject: [PATCH 180/252] Moved exit_gracefully function to example_utils.cpp --- examples/GQ7/GQ7_example.cpp | 18 ------------------ examples/example_utils.cpp | 13 +++++++++++++ examples/example_utils.hpp | 5 ++++- 3 files changed, 17 insertions(+), 19 deletions(-) diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index 8319023c4..9038ecb33 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -355,24 +355,6 @@ int usage(const char* argv0) } -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - -#ifdef _WIN32 - std::cout << "Press ENTER to exit..." << std::endl; - int dummy = getchar(); -#endif - - exit(0); -} - - //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index 6e82f6adc..cb6e6577f 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -160,3 +160,16 @@ void displayFilterState(const mip::data_filter::FilterMode &filter_state, std::s current_state = read_state; } } + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + std::cout << "Press ENTER to exit..." << std::endl; + int dummy = getchar(); +#endif + + exit(0); +} \ No newline at end of file diff --git a/examples/example_utils.hpp b/examples/example_utils.hpp index ef6b61e57..686171b26 100644 --- a/examples/example_utils.hpp +++ b/examples/example_utils.hpp @@ -37,4 +37,7 @@ std::unique_ptr handleCommonArgs(int argc, const char* argv[], int int printCommonUsage(const char* argv[]); /// Displays current filter state for the connected device if it has changed. -void displayFilterState(const mip::data_filter::FilterMode &filter_status, std::string ¤t_state); \ No newline at end of file +void displayFilterState(const mip::data_filter::FilterMode &filter_status, std::string ¤t_state); + +/// Exits the example script while displaying an appropriate exit message. +void exit_gracefully(const char *message); \ No newline at end of file From df84f59b5bafcb86f6a0d677bb02515e5f125781 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 11:42:09 -0500 Subject: [PATCH 181/252] Removed old exit_gracefully definiteion from GX5_45_example.cpp --- examples/GX5_45/GX5_45_example.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index 9443eb72b..ddbc371d1 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -319,23 +319,6 @@ int usage(const char* argv0) } -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - -#ifdef _WIN32 - int dummy = getchar(); -#endif - - exit(0); -} - - //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// From 20d8b9208e29909cde20bb2bd1f5460572733215 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 12:35:40 -0500 Subject: [PATCH 182/252] Added filter state display for five series in the examples --- examples/GX5_45/GX5_45_example.cpp | 2 ++ examples/example_utils.cpp | 12 ++++++++---- examples/example_utils.hpp | 5 ++++- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index ddbc371d1..3d520d355 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -266,9 +266,11 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter running mode.\n"); + auto current_state = std::string{""}; while(running) { device->update(); + displayFilterState(filter_status.filter_state, current_state, true); //Check GNSS fixes and alert the user when they become valid if((gnss_fix_info_valid == false) && (gnss_fix_info.fix_type == data_gnss::FixInfo::FixType::FIX_3D) && diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index cb6e6577f..cf22145e3 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -136,17 +136,21 @@ int printCommonUsage(const char* argv[]) return 1; } -void displayFilterState(const mip::data_filter::FilterMode &filter_state, std::string ¤t_state) { +void displayFilterState( + const mip::data_filter::FilterMode &filter_state, + std::string ¤t_state, + bool isFiveSeries +) { std::string read_state = ""; switch (filter_state) { case mip::data_filter::FilterMode::INIT: - read_state = "INIT"; + read_state = isFiveSeries ? "GX5_INIT" : "INIT"; break; case mip::data_filter::FilterMode::VERT_GYRO: - read_state = "VERT_GYRO"; + read_state = isFiveSeries ? "GX5_RUN_SOLUTION_VALID" : "VERT_GYRO"; break; case mip::data_filter::FilterMode::AHRS: - read_state = "AHRS"; + read_state = isFiveSeries ? "GX5_RUN_SOLUTION_ERROR" : "AHRS"; break; case mip::data_filter::FilterMode::FULL_NAV: read_state = "FULL_NAV"; diff --git a/examples/example_utils.hpp b/examples/example_utils.hpp index 686171b26..c8dd9b623 100644 --- a/examples/example_utils.hpp +++ b/examples/example_utils.hpp @@ -37,7 +37,10 @@ std::unique_ptr handleCommonArgs(int argc, const char* argv[], int int printCommonUsage(const char* argv[]); /// Displays current filter state for the connected device if it has changed. -void displayFilterState(const mip::data_filter::FilterMode &filter_status, std::string ¤t_state); +void displayFilterState( + const mip::data_filter::FilterMode &filter_status, + std::string ¤t_state, + bool isFiveSeries = false); /// Exits the example script while displaying an appropriate exit message. void exit_gracefully(const char *message); \ No newline at end of file From c35a0ccb36fd60f47c50b542f255eec0044678d9 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 14:04:49 -0500 Subject: [PATCH 183/252] Removed old exit_gracefully definition from CV7_example.cpp --- examples/CV7/CV7_example.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index 6a8d14019..2c1676adf 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -337,23 +337,6 @@ int usage(const char* argv0) } -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - -#ifdef _WIN32 - int dummy = getchar(); -#endif - - exit(0); -} - - //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// From f3c45759e89bd9b1a00cf03e9c66c3c19b158960 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 14:07:33 -0500 Subject: [PATCH 184/252] Added filter state display to CV7 example --- examples/CV7/CV7_example.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index 2c1676adf..dbb073c9d 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -275,9 +275,11 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter AHRS mode.\n"); + auto current_state = std::string{""}; while(running) { device->update(); + displayFilterState(filter_status.filter_state, current_state); //Check Filter State if((!filter_state_ahrs) && (filter_status.filter_state == data_filter::FilterMode::AHRS)) From 40dc799aecd4c33dae9b8a2cab063ad9e2b7e79b Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 14:16:17 -0500 Subject: [PATCH 185/252] Added numeric code to filter state display in example_utils.cpp --- examples/example_utils.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index cf22145e3..ddef8c68b 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -144,19 +144,22 @@ void displayFilterState( std::string read_state = ""; switch (filter_state) { case mip::data_filter::FilterMode::INIT: - read_state = isFiveSeries ? "GX5_INIT" : "INIT"; + read_state = isFiveSeries ? "GX5_INIT" : "INIT" + + std::string(" (1)"); break; case mip::data_filter::FilterMode::VERT_GYRO: - read_state = isFiveSeries ? "GX5_RUN_SOLUTION_VALID" : "VERT_GYRO"; + read_state = isFiveSeries ? "GX5_RUN_SOLUTION_VALID" : "VERT_GYRO" + + std::string(" (2)"); break; case mip::data_filter::FilterMode::AHRS: - read_state = isFiveSeries ? "GX5_RUN_SOLUTION_ERROR" : "AHRS"; + read_state = isFiveSeries ? "GX5_RUN_SOLUTION_ERROR" : "AHRS" + + std::string(" (3)"); break; case mip::data_filter::FilterMode::FULL_NAV: - read_state = "FULL_NAV"; + read_state = "FULL_NAV (4)"; break; default: - read_state = "STARTUP"; + read_state = "STARTUP (0)"; break; } if (read_state != current_state) { From da0954db3a28bc4b17e471c5734202a81b3a913b Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 14:21:42 -0500 Subject: [PATCH 186/252] Added objective filter state to sensor is configured message in GX5 and CV7 examples --- examples/CV7/CV7_example.cpp | 2 +- examples/GX5_45/GX5_45_example.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index dbb073c9d..cd5d13706 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -273,7 +273,7 @@ int main(int argc, const char* argv[]) bool running = true; mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); - printf("Sensor is configured... waiting for filter to enter AHRS mode.\n"); + printf("Sensor is configured... waiting for filter to enter AHRS mode (AHRS).\n"); auto current_state = std::string{""}; while(running) diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index 3d520d355..bc23c6a0f 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -264,7 +264,7 @@ int main(int argc, const char* argv[]) bool running = true; mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); - printf("Sensor is configured... waiting for filter to enter running mode.\n"); + printf("Sensor is configured... waiting for filter to enter running mode (GX5_RUN_SOLUTION_VALID).\n"); auto current_state = std::string{""}; while(running) From 3c946763eb1723dcaa0bc73b9f90554186920873 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 14:27:50 -0500 Subject: [PATCH 187/252] Added filter state display to CV7_INS example --- examples/CV7_INS/CV7_INS_simple_example.cpp | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index b700ef72f..552c02e02 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -56,7 +56,6 @@ int usage(const char* argv0); void print_device_information(const commands_base::BaseDeviceInfo& device_info); -void exit_gracefully(const char *message); bool should_exit(); //////////////////////////////////////////////////////////////////////////////// @@ -210,9 +209,11 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to initialize...\n"); + auto current_state = std::string{""}; while(running) { device->update(); + displayFilterState(filter_status.filter_state, current_state); //Check for full nav filter state transition if((!filter_state_full_nav) && (filter_status.filter_state == data_filter::FilterMode::FULL_NAV)) @@ -320,23 +321,6 @@ int usage(const char* argv0) } -//////////////////////////////////////////////////////////////////////////////// -// Exit Function -//////////////////////////////////////////////////////////////////////////////// - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - -#ifdef _WIN32 - int dummy = getchar(); -#endif - - exit(0); -} - - //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// From 2191b9d7f8f9c426829277450a6eb7bfc77ff1a2 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 14:28:36 -0500 Subject: [PATCH 188/252] Added objective state message to CV7_INS example --- examples/CV7_INS/CV7_INS_simple_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 552c02e02..bd4ddf8a9 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -207,7 +207,7 @@ int main(int argc, const char* argv[]) mip::Timestamp prev_print_timestamp = getCurrentTimestamp(); mip::Timestamp prev_measurement_update_timestamp = getCurrentTimestamp(); - printf("Sensor is configured... waiting for filter to initialize...\n"); + printf("Sensor is configured... waiting for filter to initialize (FULL_NAV)...\n"); auto current_state = std::string{""}; while(running) From 6540f288cf50aed56c43df9e9c84d30e0f125bef Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 15 Jan 2024 14:32:20 -0500 Subject: [PATCH 189/252] Removed old exit_gracefully prototypes from CV7, GQ7, and GX5_45 examples --- examples/CV7/CV7_example.cpp | 1 - examples/GQ7/GQ7_example.cpp | 1 - examples/GX5_45/GX5_45_example.cpp | 1 - 3 files changed, 3 deletions(-) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index cd5d13706..ad6412798 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -63,7 +63,6 @@ const uint8_t FILTER_PITCH_EVENT_ACTION_ID = 2; int usage(const char* argv0); -void exit_gracefully(const char *message); bool should_exit(); void handleFilterEventSource(void*, const mip::Field& field, mip::Timestamp timestamp); diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index 9038ecb33..9e4fb9b41 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -70,7 +70,6 @@ bool filter_state_full_nav = false; int usage(const char* argv0); -void exit_gracefully(const char *message); bool should_exit(); diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index bc23c6a0f..3c6a24cb8 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -70,7 +70,6 @@ bool filter_state_running = false; int usage(const char* argv0); -void exit_gracefully(const char *message); bool should_exit(); From 4c4f4f8b27a7bd6293727615cd559eef8788e70f Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Tue, 16 Jan 2024 10:49:26 -0500 Subject: [PATCH 190/252] Working filter state display logic for GQ7_example.c (needs refactor) --- examples/GQ7/GQ7_example.c | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index f56765a5a..314756144 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -88,6 +88,12 @@ int usage(const char* argv0); void exit_gracefully(const char *message); bool should_exit(); +void displayFilterState( + const mip_filter_mode filter_status, + char **current_state, + bool isFiveSeries +); + //////////////////////////////////////////////////////////////////////////////// // Main Function @@ -332,10 +338,27 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter Full Navigation mode.\n"); + char *current_state = ""; while(running) { mip_interface_update(&device, false); + char *read_state = ""; + if (filter_status.filter_state == MIP_FILTER_MODE_INIT) { + read_state = false ? "GX5_INIT (1)" : "INIT (1)"; + } else if (filter_status.filter_state == MIP_FILTER_MODE_VERT_GYRO) { + read_state = false ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; + } else if (filter_status.filter_state == MIP_FILTER_MODE_AHRS) { + read_state = false ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; + } else if (filter_status.filter_state == MIP_FILTER_MODE_FULL_NAV) { + read_state = "FULL_NAV (4)"; + } else { + read_state = "STARTUP (0)"; + } + if (strcmp(read_state, current_state) != 0) { + printf("FILTER STATE: %s\n", read_state); + current_state = read_state; + } //Check GNSS fixes and alert the user when they become valid for(int i=0; i<2; i++) @@ -458,3 +481,15 @@ bool should_exit() } + +//////////////////////////////////////////////////////////////////////////////// +// Displays current filter state for the connected device if it has changed. +//////////////////////////////////////////////////////////////////////////////// + +void displayFilterState( + const mip_filter_mode filter_status, + char **current_state, + bool isFiveSeries +) { + +} \ No newline at end of file From e0e556847f6562f32d20ed7f55c457d37cc575d8 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Tue, 16 Jan 2024 11:56:59 -0500 Subject: [PATCH 191/252] Refactored filter state display logic into separate function in GQ7_example.c --- examples/GQ7/GQ7_example.c | 38 ++++++++++++++++++++------------------ examples/example_utils.cpp | 1 + 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index 314756144..7a38cd53a 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -338,27 +338,13 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter Full Navigation mode.\n"); - char *current_state = ""; + char *state_init = ""; + char **current_state = &state_init; while(running) { mip_interface_update(&device, false); + displayFilterState(filter_status.filter_state, current_state, false); - char *read_state = ""; - if (filter_status.filter_state == MIP_FILTER_MODE_INIT) { - read_state = false ? "GX5_INIT (1)" : "INIT (1)"; - } else if (filter_status.filter_state == MIP_FILTER_MODE_VERT_GYRO) { - read_state = false ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; - } else if (filter_status.filter_state == MIP_FILTER_MODE_AHRS) { - read_state = false ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; - } else if (filter_status.filter_state == MIP_FILTER_MODE_FULL_NAV) { - read_state = "FULL_NAV (4)"; - } else { - read_state = "STARTUP (0)"; - } - if (strcmp(read_state, current_state) != 0) { - printf("FILTER STATE: %s\n", read_state); - current_state = read_state; - } //Check GNSS fixes and alert the user when they become valid for(int i=0; i<2; i++) @@ -491,5 +477,21 @@ void displayFilterState( char **current_state, bool isFiveSeries ) { + char *read_state = ""; + if (filter_status == MIP_FILTER_MODE_INIT) { + read_state = false ? "GX5_INIT (1)" : "INIT (1)"; + } else if (filter_status == MIP_FILTER_MODE_VERT_GYRO) { + read_state = false ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; + } else if (filter_status == MIP_FILTER_MODE_AHRS) { + read_state = false ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; + } else if (filter_status == MIP_FILTER_MODE_FULL_NAV) { + read_state = "FULL_NAV (4)"; + } else { + read_state = "STARTUP (0)"; + } -} \ No newline at end of file + if (strcmp(read_state, *current_state) != 0) { + printf("FILTER STATE: %s\n", read_state); + *current_state = read_state; + } +} diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index ddef8c68b..709f04174 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -162,6 +162,7 @@ void displayFilterState( read_state = "STARTUP (0)"; break; } + if (read_state != current_state) { std::cout << "Filter state: " << read_state << std::endl; current_state = read_state; From 3cc103d4770555df797a3af569725d5ffb241859 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Tue, 16 Jan 2024 14:34:07 -0500 Subject: [PATCH 192/252] Added c utility header to CMakeLists.txt --- examples/CMakeLists.txt | 2 ++ examples/GQ7/GQ7_example.c | 37 ++----------------------------------- examples/example_utils.c | 26 ++++++++++++++++++++++++++ examples/example_utils.h | 9 +++++++++ 4 files changed, 39 insertions(+), 35 deletions(-) create mode 100644 examples/example_utils.c create mode 100644 examples/example_utils.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 403976dfe..1dcd90a26 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -3,6 +3,8 @@ set(EXAMPLE_DIR "${CMAKE_CURRENT_LIST_DIR}") set(DEVICE_SOURCES + "${EXAMPLE_DIR}/example_utils.c" + "${EXAMPLE_DIR}/example_utils.h" "${EXAMPLE_DIR}/example_utils.cpp" "${EXAMPLE_DIR}/example_utils.hpp" ) diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index 7a38cd53a..f78990aaa 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -34,6 +34,8 @@ #include #include +#include "../example_utils.h" + //////////////////////////////////////////////////////////////////////////////// // Global Variables @@ -88,12 +90,6 @@ int usage(const char* argv0); void exit_gracefully(const char *message); bool should_exit(); -void displayFilterState( - const mip_filter_mode filter_status, - char **current_state, - bool isFiveSeries -); - //////////////////////////////////////////////////////////////////////////////// // Main Function @@ -466,32 +462,3 @@ bool should_exit() return false; } - - -//////////////////////////////////////////////////////////////////////////////// -// Displays current filter state for the connected device if it has changed. -//////////////////////////////////////////////////////////////////////////////// - -void displayFilterState( - const mip_filter_mode filter_status, - char **current_state, - bool isFiveSeries -) { - char *read_state = ""; - if (filter_status == MIP_FILTER_MODE_INIT) { - read_state = false ? "GX5_INIT (1)" : "INIT (1)"; - } else if (filter_status == MIP_FILTER_MODE_VERT_GYRO) { - read_state = false ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; - } else if (filter_status == MIP_FILTER_MODE_AHRS) { - read_state = false ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; - } else if (filter_status == MIP_FILTER_MODE_FULL_NAV) { - read_state = "FULL_NAV (4)"; - } else { - read_state = "STARTUP (0)"; - } - - if (strcmp(read_state, *current_state) != 0) { - printf("FILTER STATE: %s\n", read_state); - *current_state = read_state; - } -} diff --git a/examples/example_utils.c b/examples/example_utils.c new file mode 100644 index 000000000..b72edea04 --- /dev/null +++ b/examples/example_utils.c @@ -0,0 +1,26 @@ +#include "example_utils.h" + + +void displayFilterState( + const mip_filter_mode filter_status, + char **current_state, + bool isFiveSeries +) { + char *read_state = ""; + if (filter_status == MIP_FILTER_MODE_INIT) { + read_state = false ? "GX5_INIT (1)" : "INIT (1)"; + } else if (filter_status == MIP_FILTER_MODE_VERT_GYRO) { + read_state = false ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; + } else if (filter_status == MIP_FILTER_MODE_AHRS) { + read_state = false ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; + } else if (filter_status == MIP_FILTER_MODE_FULL_NAV) { + read_state = "FULL_NAV (4)"; + } else { + read_state = "STARTUP (0)"; + } + + if (strcmp(read_state, *current_state) != 0) { + printf("FILTER STATE: %s\n", read_state); + *current_state = read_state; + } +} \ No newline at end of file diff --git a/examples/example_utils.h b/examples/example_utils.h new file mode 100644 index 000000000..ce4a5098b --- /dev/null +++ b/examples/example_utils.h @@ -0,0 +1,9 @@ +#include + + +// Displays current filter state for the connected device if it has changed. +void displayFilterState( + const mip_filter_mode filter_status, + char **current_state, + bool isFiveSeries +); From 141646e16d84c550356d50fab9ac2f574b4723e5 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Tue, 16 Jan 2024 14:46:22 -0500 Subject: [PATCH 193/252] Added c utility headers to executable for c examples in CMakeLists.txt --- examples/CMakeLists.txt | 8 ++++---- examples/GQ7/GQ7_example.c | 1 - examples/example_utils.c | 10 +++++----- examples/example_utils.h | 2 +- 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1dcd90a26..d9f5ffbea 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -67,15 +67,15 @@ endif() # C examples need serial support if(MIP_USE_SERIAL) - add_executable(WatchImuC "${EXAMPLE_DIR}/watch_imu.c") + add_executable(WatchImuC "${EXAMPLE_DIR}/watch_imu.c" ${DEVICE_SOURCES}) target_link_libraries(WatchImuC mip) - add_executable(GQ7_ExampleC "${EXAMPLE_DIR}/GQ7/GQ7_example.c") + add_executable(GQ7_ExampleC "${EXAMPLE_DIR}/GQ7/GQ7_example.c" ${DEVICE_SOURCES}) target_link_libraries(GQ7_ExampleC mip) - add_executable(CV7_ExampleC "${EXAMPLE_DIR}/CV7/CV7_example.c") + add_executable(CV7_ExampleC "${EXAMPLE_DIR}/CV7/CV7_example.c" ${DEVICE_SOURCES}) target_link_libraries(CV7_ExampleC mip) - add_executable(GX5_45_ExampleC "${EXAMPLE_DIR}/GX5_45/GX5_45_example.c") + add_executable(GX5_45_ExampleC "${EXAMPLE_DIR}/GX5_45/GX5_45_example.c" ${DEVICE_SOURCES}) target_link_libraries(GX5_45_ExampleC mip) endif() \ No newline at end of file diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index f78990aaa..43c85a876 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -341,7 +341,6 @@ int main(int argc, const char* argv[]) mip_interface_update(&device, false); displayFilterState(filter_status.filter_state, current_state, false); - //Check GNSS fixes and alert the user when they become valid for(int i=0; i<2; i++) { diff --git a/examples/example_utils.c b/examples/example_utils.c index b72edea04..b02d388b5 100644 --- a/examples/example_utils.c +++ b/examples/example_utils.c @@ -2,18 +2,18 @@ void displayFilterState( - const mip_filter_mode filter_status, + const mip_filter_mode filter_state, char **current_state, bool isFiveSeries ) { char *read_state = ""; - if (filter_status == MIP_FILTER_MODE_INIT) { + if (filter_state == MIP_FILTER_MODE_INIT) { read_state = false ? "GX5_INIT (1)" : "INIT (1)"; - } else if (filter_status == MIP_FILTER_MODE_VERT_GYRO) { + } else if (filter_state == MIP_FILTER_MODE_VERT_GYRO) { read_state = false ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; - } else if (filter_status == MIP_FILTER_MODE_AHRS) { + } else if (filter_state == MIP_FILTER_MODE_AHRS) { read_state = false ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; - } else if (filter_status == MIP_FILTER_MODE_FULL_NAV) { + } else if (filter_state == MIP_FILTER_MODE_FULL_NAV) { read_state = "FULL_NAV (4)"; } else { read_state = "STARTUP (0)"; diff --git a/examples/example_utils.h b/examples/example_utils.h index ce4a5098b..68841db3c 100644 --- a/examples/example_utils.h +++ b/examples/example_utils.h @@ -3,7 +3,7 @@ // Displays current filter state for the connected device if it has changed. void displayFilterState( - const mip_filter_mode filter_status, + const mip_filter_mode filter_state, char **current_state, bool isFiveSeries ); From 9048ebc867109297c662ad282ad9c17245f76684 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Tue, 16 Jan 2024 14:57:05 -0500 Subject: [PATCH 194/252] Added filter state display to other c examples --- examples/CV7/CV7_example.c | 4 ++++ examples/GX5_45/GX5_45_example.c | 3 +++ 2 files changed, 7 insertions(+) diff --git a/examples/CV7/CV7_example.c b/examples/CV7/CV7_example.c index a36465a5c..eedfb2240 100644 --- a/examples/CV7/CV7_example.c +++ b/examples/CV7/CV7_example.c @@ -34,6 +34,7 @@ #include #include +#include "../example_utils.h" //////////////////////////////////////////////////////////////////////////////// // Global Variables @@ -318,9 +319,12 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter AHRS mode.\n"); + char *state_init = ""; + char **current_state = &state_init; while(running) { mip_interface_update(&device, false); + displayFilterState(filter_status.filter_state, current_state, false); //Check Filter State if((!filter_state_ahrs) && (filter_status.filter_state == MIP_FILTER_MODE_AHRS)) diff --git a/examples/GX5_45/GX5_45_example.c b/examples/GX5_45/GX5_45_example.c index 745f3b645..af7410e07 100644 --- a/examples/GX5_45/GX5_45_example.c +++ b/examples/GX5_45/GX5_45_example.c @@ -34,6 +34,7 @@ #include #include +#include "../example_utils.h" //////////////////////////////////////////////////////////////////////////////// // Global Variables @@ -306,6 +307,8 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter running mode.\n"); + char *state_init = ""; + char **current_state = &state_init; while(running) { mip_interface_update(&device, false); From 30698960c2807bff1521ca780ad552d68f0ddfeb Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Tue, 16 Jan 2024 15:13:02 -0500 Subject: [PATCH 195/252] Moved exit_gracefully function prototypes and definitions back into individual files for the C++ examples --- examples/CV7/CV7_example.cpp | 19 +++++++++++++++++++ examples/CV7_INS/CV7_INS_simple_example.cpp | 19 +++++++++++++++++++ examples/GQ7/GQ7_example.cpp | 19 +++++++++++++++++++ examples/GX5_45/GX5_45_example.cpp | 19 +++++++++++++++++++ examples/example_utils.cpp | 13 ------------- examples/example_utils.hpp | 3 --- 6 files changed, 76 insertions(+), 16 deletions(-) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index ad6412798..c15f25097 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -63,6 +63,7 @@ const uint8_t FILTER_PITCH_EVENT_ACTION_ID = 2; int usage(const char* argv0); +void exit_gracefully(const char *message); bool should_exit(); void handleFilterEventSource(void*, const mip::Field& field, mip::Timestamp timestamp); @@ -338,6 +339,24 @@ int usage(const char* argv0) } +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + std::cout << "Press ENTER to exit..." << std::endl; + int dummy = getchar(); +#endif + + exit(0); +} + + //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index bd4ddf8a9..019ac9d7d 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -56,6 +56,7 @@ int usage(const char* argv0); void print_device_information(const commands_base::BaseDeviceInfo& device_info); +void exit_gracefully(const char *message); bool should_exit(); //////////////////////////////////////////////////////////////////////////////// @@ -321,6 +322,24 @@ int usage(const char* argv0) } +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + std::cout << "Press ENTER to exit..." << std::endl; + int dummy = getchar(); +#endif + + exit(0); +} + + //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index 9e4fb9b41..8319023c4 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -70,6 +70,7 @@ bool filter_state_full_nav = false; int usage(const char* argv0); +void exit_gracefully(const char *message); bool should_exit(); @@ -354,6 +355,24 @@ int usage(const char* argv0) } +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + std::cout << "Press ENTER to exit..." << std::endl; + int dummy = getchar(); +#endif + + exit(0); +} + + //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index 3c6a24cb8..73fbebada 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -70,6 +70,7 @@ bool filter_state_running = false; int usage(const char* argv0); +void exit_gracefully(const char *message); bool should_exit(); @@ -320,6 +321,24 @@ int usage(const char* argv0) } +//////////////////////////////////////////////////////////////////////////////// +// Exit Function +//////////////////////////////////////////////////////////////////////////////// + +void exit_gracefully(const char *message) +{ + if(message) + printf("%s\n", message); + +#ifdef _WIN32 + std::cout << "Press ENTER to exit..." << std::endl; + int dummy = getchar(); +#endif + + exit(0); +} + + //////////////////////////////////////////////////////////////////////////////// // Check for Exit Condition //////////////////////////////////////////////////////////////////////////////// diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index 709f04174..f0d00df15 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -168,16 +168,3 @@ void displayFilterState( current_state = read_state; } } - -void exit_gracefully(const char *message) -{ - if(message) - printf("%s\n", message); - -#ifdef _WIN32 - std::cout << "Press ENTER to exit..." << std::endl; - int dummy = getchar(); -#endif - - exit(0); -} \ No newline at end of file diff --git a/examples/example_utils.hpp b/examples/example_utils.hpp index c8dd9b623..68a670bd3 100644 --- a/examples/example_utils.hpp +++ b/examples/example_utils.hpp @@ -41,6 +41,3 @@ void displayFilterState( const mip::data_filter::FilterMode &filter_status, std::string ¤t_state, bool isFiveSeries = false); - -/// Exits the example script while displaying an appropriate exit message. -void exit_gracefully(const char *message); \ No newline at end of file From 430136b4499feb5b985d06df4eee21d8dae47b7c Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 26 Jan 2024 18:00:01 -0500 Subject: [PATCH 196/252] Updates per PR feedback: - Also fixed bug in get_model_from_string. --- src/mip/extras/descriptor_id.hpp | 2 +- src/mip/extras/device_models.c | 4 +++- src/mip/extras/device_models.h | 16 ---------------- 3 files changed, 4 insertions(+), 18 deletions(-) diff --git a/src/mip/extras/descriptor_id.hpp b/src/mip/extras/descriptor_id.hpp index 087318ccd..768f97045 100644 --- a/src/mip/extras/descriptor_id.hpp +++ b/src/mip/extras/descriptor_id.hpp @@ -25,7 +25,7 @@ class DescriptorId bool isNull() const { return m_key == 0x00000000; } bool isMip() const { return descriptor().as_u16() != 0x0000; } - bool isNonMip() const { return descriptor().as_u16() == 0x0000; } + bool isNonMip() const { return !isMip(); } CompositeDescriptor descriptor() const { return m_key >> 16; } uint16_t index() const { return m_key & 0xFFFF; } diff --git a/src/mip/extras/device_models.c b/src/mip/extras/device_models.c index 27a0de9be..8566c0958 100644 --- a/src/mip/extras/device_models.c +++ b/src/mip/extras/device_models.c @@ -21,7 +21,9 @@ mip_model_number get_model_from_string(const char* model_or_serial) unsigned int i = 0; for (; model_or_serial[i] != '\0'; i++) { - const char c = model_or_serial[i]; + // Unsigned is important. Passing a negative value to isdigit or + // isspace is undefined behavior and can trigger assertions. + const unsigned char c = model_or_serial[i]; if (!isdigit(c)) { diff --git a/src/mip/extras/device_models.h b/src/mip/extras/device_models.h index c7e5aad03..da22e542e 100644 --- a/src/mip/extras/device_models.h +++ b/src/mip/extras/device_models.h @@ -64,21 +64,5 @@ inline ModelNumber getModelFromString(const char* model_or_serial) { return C::g inline const char* getModelNameFromNumber(ModelNumber model) { return get_model_name_from_number(model); } -//class ModelNumber -//{ -// ModelNumber(C::mip_model_number number) : m_number(number) {} -// ModelNumber(const char* model_or_serial) : m_number(C::get_model_from_string(model_or_serial) {} -// -// const char* getModelName() { return C::get_model_name_from_number(m_number); } -// -// using Model = C::mip_model_number; -// -// uint16_t shortNumber() const { return m_number; } -// uint32_t longNumber() const { return m_number * 1000; } -// -//private: -// C::mip_model_number m_number; -//}; - } // namespace mip #endif // __cplusplus From e30be2d39bb3c0cb236fd5183ef5b21714f1b6ba Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 31 Jan 2024 19:52:35 +0000 Subject: [PATCH 197/252] Generate MIP definitions from branches/dev@55920. --- src/mip/definitions/commands_3dm.h | 11 ++++------- src/mip/definitions/commands_3dm.hpp | 11 ++++------- src/mip/definitions/data_gnss.h | 2 +- src/mip/definitions/data_gnss.hpp | 2 +- 4 files changed, 10 insertions(+), 16 deletions(-) diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index ea75a2a6d..b98e35c98 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -1132,13 +1132,10 @@ static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BE static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_RISING = 1; ///< Rising edges will be timestamped. static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_FALLING = 2; ///< Falling edges will be timestamped. static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_EITHER = 3; ///< Both rising and falling edges will be timestamped. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_DEFAULT = 1; ///< UART transmit line (auto-select port). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_DEFAULT = 2; ///< UART receive line (auto-select port). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_AUX1 = 33; ///< UART transmit line, port 2 (aux port 1). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_AUX1 = 34; ///< UART receive line, port 2 (aux port 1). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_AUX2 = 49; ///< UART transmit line, port 3 (aux port 2). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_AUX2 = 50; ///< UART receive line, port 3 (aux port 2). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_POWER_SHUTDOWN = 1; ///< A logic 1 applied to the pin will place the device in low-power mode. A full restart is executed after the signal is removed. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT2_TX = 33; ///< (0x21) UART port 2 transmit. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT2_RX = 34; ///< (0x22) UART port 2 receive. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT3_TX = 49; ///< (0x31) UART port 3 transmit. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT3_RX = 50; ///< (0x32) UART port 3 receive. typedef uint8_t mip_3dm_gpio_config_command_pin_mode; static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_NONE = 0x00; diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index caedca9e3..dbbe7447a 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -1928,13 +1928,10 @@ struct GpioConfig TIMESTAMP_RISING = 1, ///< Rising edges will be timestamped. TIMESTAMP_FALLING = 2, ///< Falling edges will be timestamped. TIMESTAMP_EITHER = 3, ///< Both rising and falling edges will be timestamped. - UART_TX_DEFAULT = 1, ///< UART transmit line (auto-select port). - UART_RX_DEFAULT = 2, ///< UART receive line (auto-select port). - UART_TX_AUX1 = 33, ///< UART transmit line, port 2 (aux port 1). - UART_RX_AUX1 = 34, ///< UART receive line, port 2 (aux port 1). - UART_TX_AUX2 = 49, ///< UART transmit line, port 3 (aux port 2). - UART_RX_AUX2 = 50, ///< UART receive line, port 3 (aux port 2). - POWER_SHUTDOWN = 1, ///< A logic 1 applied to the pin will place the device in low-power mode. A full restart is executed after the signal is removed. + UART_PORT2_TX = 33, ///< (0x21) UART port 2 transmit. + UART_PORT2_RX = 34, ///< (0x22) UART port 2 receive. + UART_PORT3_TX = 49, ///< (0x31) UART port 3 transmit. + UART_PORT3_RX = 50, ///< (0x32) UART port 3 receive. }; struct PinMode : Bitfield diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index 8ec56edfd..60b9a6035 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -1029,7 +1029,7 @@ struct mip_gnss_rtk_corrections_status_data double time_of_week; ///< GPS Time of week [seconds] uint16_t week_number; ///< GPS Week since 1980 [weeks] mip_gnss_rtk_corrections_status_data_epoch_status epoch_status; ///< Status of the corrections received during this epoch - uint32_t dongle_status; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see MIP_CMD_DESC_RTK_GET_STATUS_FLAGS for details) + uint32_t dongle_status; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see Get RTK Device Status Flags (0x0F,0x01) for details) float gps_correction_latency; ///< Latency of last GPS correction [seconds] float glonass_correction_latency; ///< Latency of last GLONASS correction [seconds] float galileo_correction_latency; ///< Latency of last Galileo correction [seconds] diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 035fbaba7..bab43b3bd 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -1877,7 +1877,7 @@ struct RtkCorrectionsStatus double time_of_week = 0; ///< GPS Time of week [seconds] uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] EpochStatus epoch_status; ///< Status of the corrections received during this epoch - uint32_t dongle_status = 0; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see MIP_CMD_DESC_RTK_GET_STATUS_FLAGS for details) + uint32_t dongle_status = 0; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see Get RTK Device Status Flags (0x0F,0x01) for details) float gps_correction_latency = 0; ///< Latency of last GPS correction [seconds] float glonass_correction_latency = 0; ///< Latency of last GLONASS correction [seconds] float galileo_correction_latency = 0; ///< Latency of last Galileo correction [seconds] From 1196ec4dcb3c0fabd9410976b67e38bf6ed2c2f0 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 5 Feb 2024 14:19:47 -0500 Subject: [PATCH 198/252] Add TypedResult struct. --- src/mip/definitions/descriptors.h | 31 ++++++++++++++++++++++++++----- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index e95ca1294..39d5c5c1f 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -4,6 +4,7 @@ #include #include #include "../utils/serialization.h" +#include "../mip_result.h" #ifdef __cplusplus @@ -68,17 +69,17 @@ struct CompositeDescriptor uint8_t descriptorSet; ///< MIP descriptor set. uint8_t fieldDescriptor; ///< MIP field descriptor. - CompositeDescriptor(uint8_t descSet, uint8_t fieldDesc) : descriptorSet(descSet), fieldDescriptor(fieldDesc) {} - CompositeDescriptor(uint16_t combo) : descriptorSet(combo >> 8), fieldDescriptor(combo & 0xFF) {} + constexpr CompositeDescriptor(uint8_t descSet, uint8_t fieldDesc) : descriptorSet(descSet), fieldDescriptor(fieldDesc) {} + constexpr CompositeDescriptor(uint16_t combo) : descriptorSet(combo >> 8), fieldDescriptor(combo & 0xFF) {} - CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } + constexpr CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } uint16_t as_u16() const { return (uint16_t(descriptorSet) << 8) | fieldDescriptor; } // operator uint16_t() const { return as_u16(); } - bool operator==(const CompositeDescriptor& other) const { return other.descriptorSet == descriptorSet && other.fieldDescriptor == fieldDescriptor; } - bool operator<(const CompositeDescriptor& other) const { return descriptorSet < other.descriptorSet || (!(descriptorSet > other.descriptorSet) && (fieldDescriptor < other.fieldDescriptor)); } + constexpr bool operator==(const CompositeDescriptor& other) const { return other.descriptorSet == descriptorSet && other.fieldDescriptor == fieldDescriptor; } + constexpr bool operator<(const CompositeDescriptor& other) const { return descriptorSet < other.descriptorSet || (!(descriptorSet > other.descriptorSet) && (fieldDescriptor < other.fieldDescriptor)); } }; @@ -115,6 +116,26 @@ inline bool isResponseFieldDescriptor(uint8_t fieldDescriptor) { return C::mip inline bool isReservedFieldDescriptor(uint8_t fieldDescriptor) { return C::mip_is_reserved_cmd_field_descriptor(fieldDescriptor); } inline bool isSharedDataFieldDescriptor(uint8_t fieldDescriptor) { return C::mip_is_shared_data_field_descriptor(fieldDescriptor); } + +//////////////////////////////////////////////////////////////////////////////// +///@brief A CmdResult that knows the corresponding command type. +/// +///@tparam MipCmd Type of the command struct. +/// +template +struct TypedResult : public CmdResult +{ + using Cmd = MipCmd; + + ///@brief The command descriptor. + /// + static constexpr CompositeDescriptor DESCRIPTOR = MipCmd::DESCRIPTOR; + + ///@brief Returns the composite descriptor of the command. + /// + constexpr CompositeDescriptor descriptor() const { return DESCRIPTOR; } +}; + } // namespace mip #endif // __cplusplus From 3f28eecba4206b5a63f5bd9e2e956dddd01cbacc Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 5 Feb 2024 14:26:39 -0500 Subject: [PATCH 199/252] Use CmdResult constructor in TypedResult. --- src/mip/definitions/descriptors.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index 39d5c5c1f..4c718a53a 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -127,6 +127,9 @@ struct TypedResult : public CmdResult { using Cmd = MipCmd; + // Same constructor as CmdResult. + using CmdResult::CmdResult; + ///@brief The command descriptor. /// static constexpr CompositeDescriptor DESCRIPTOR = MipCmd::DESCRIPTOR; From 42d15dbbf8a363b2a6dbe04da507527a3426de06 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 5 Feb 2024 19:31:56 +0000 Subject: [PATCH 200/252] Generate MIP definitions from branches/ResultDescriptor@55936. --- src/mip/definitions/commands_3dm.cpp | 402 ++--- src/mip/definitions/commands_3dm.h | 11 +- src/mip/definitions/commands_3dm.hpp | 1530 ++++++++++--------- src/mip/definitions/commands_aiding.c | 262 +++- src/mip/definitions/commands_aiding.cpp | 289 +++- src/mip/definitions/commands_aiding.h | 121 +- src/mip/definitions/commands_aiding.hpp | 382 +++-- src/mip/definitions/commands_base.cpp | 42 +- src/mip/definitions/commands_base.hpp | 219 +-- src/mip/definitions/commands_filter.cpp | 528 +++---- src/mip/definitions/commands_filter.h | 16 +- src/mip/definitions/commands_filter.hpp | 1835 ++++++++++++----------- src/mip/definitions/commands_gnss.cpp | 28 +- src/mip/definitions/commands_gnss.hpp | 118 +- src/mip/definitions/commands_rtk.cpp | 50 +- src/mip/definitions/commands_rtk.hpp | 255 ++-- src/mip/definitions/commands_system.cpp | 8 +- src/mip/definitions/commands_system.hpp | 44 +- src/mip/definitions/data_filter.c | 64 + src/mip/definitions/data_filter.cpp | 46 + src/mip/definitions/data_filter.h | 68 +- src/mip/definitions/data_filter.hpp | 375 +++-- src/mip/definitions/data_gnss.h | 14 +- src/mip/definitions/data_gnss.hpp | 227 +-- src/mip/definitions/data_sensor.hpp | 119 +- src/mip/definitions/data_shared.hpp | 53 +- src/mip/definitions/data_system.hpp | 20 +- 27 files changed, 4208 insertions(+), 2918 deletions(-) diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index 563119dcc..477119d41 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -72,7 +72,7 @@ void extract(Serializer& serializer, PollImuMessage& self) } -CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -109,7 +109,7 @@ void extract(Serializer& serializer, PollGnssMessage& self) } -CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -146,7 +146,7 @@ void extract(Serializer& serializer, PollFilterMessage& self) } -CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -205,7 +205,7 @@ void extract(Serializer& serializer, ImuMessageFormat::Response& self) } -CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -221,7 +221,7 @@ CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -230,7 +230,7 @@ CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptors assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_IMU_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_IMU_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -246,7 +246,7 @@ CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptors } return result; } -CmdResult saveImuMessageFormat(C::mip_interface& device) +TypedResult saveImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -256,7 +256,7 @@ CmdResult saveImuMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadImuMessageFormat(C::mip_interface& device) +TypedResult loadImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -266,7 +266,7 @@ CmdResult loadImuMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultImuMessageFormat(C::mip_interface& device) +TypedResult defaultImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -318,7 +318,7 @@ void extract(Serializer& serializer, GpsMessageFormat::Response& self) } -CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -334,7 +334,7 @@ CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -343,7 +343,7 @@ CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptors assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -359,7 +359,7 @@ CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptors } return result; } -CmdResult saveGpsMessageFormat(C::mip_interface& device) +TypedResult saveGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -369,7 +369,7 @@ CmdResult saveGpsMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGpsMessageFormat(C::mip_interface& device) +TypedResult loadGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -379,7 +379,7 @@ CmdResult loadGpsMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGpsMessageFormat(C::mip_interface& device) +TypedResult defaultGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -431,7 +431,7 @@ void extract(Serializer& serializer, FilterMessageFormat::Response& self) } -CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -447,7 +447,7 @@ CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescript return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -456,7 +456,7 @@ CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescript assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FILTER_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FILTER_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -472,7 +472,7 @@ CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescript } return result; } -CmdResult saveFilterMessageFormat(C::mip_interface& device) +TypedResult saveFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -482,7 +482,7 @@ CmdResult saveFilterMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadFilterMessageFormat(C::mip_interface& device) +TypedResult loadFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -492,7 +492,7 @@ CmdResult loadFilterMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultFilterMessageFormat(C::mip_interface& device) +TypedResult defaultFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -524,12 +524,12 @@ void extract(Serializer& serializer, ImuGetBaseRate::Response& self) } -CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +TypedResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMU_BASE_RATE, NULL, 0, REPLY_IMU_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMU_BASE_RATE, NULL, 0, REPLY_IMU_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -565,12 +565,12 @@ void extract(Serializer& serializer, GpsGetBaseRate::Response& self) } -CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +TypedResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_GNSS_BASE_RATE, NULL, 0, REPLY_GNSS_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_GNSS_BASE_RATE, NULL, 0, REPLY_GNSS_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -606,12 +606,12 @@ void extract(Serializer& serializer, FilterGetBaseRate::Response& self) } -CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +TypedResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_FILTER_BASE_RATE, NULL, 0, REPLY_FILTER_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_FILTER_BASE_RATE, NULL, 0, REPLY_FILTER_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -649,7 +649,7 @@ void extract(Serializer& serializer, PollData& self) } -CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors) +TypedResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -694,7 +694,7 @@ void extract(Serializer& serializer, GetBaseRate::Response& self) } -CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut) +TypedResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -704,7 +704,7 @@ CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateO assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_BASE_RATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_BASE_RATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -770,7 +770,7 @@ void extract(Serializer& serializer, MessageFormat::Response& self) } -CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -788,7 +788,7 @@ CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -799,7 +799,7 @@ CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -817,7 +817,7 @@ CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* } return result; } -CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) +TypedResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -829,7 +829,7 @@ CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) +TypedResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -841,7 +841,7 @@ CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet) +TypedResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -873,7 +873,7 @@ void extract(Serializer& serializer, NmeaPollData& self) } -CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries) +TypedResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -932,7 +932,7 @@ void extract(Serializer& serializer, NmeaMessageFormat::Response& self) } -CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries) +TypedResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -948,7 +948,7 @@ CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut) +TypedResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -957,7 +957,7 @@ CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uin assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NMEA_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NMEA_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -973,7 +973,7 @@ CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uin } return result; } -CmdResult saveNmeaMessageFormat(C::mip_interface& device) +TypedResult saveNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -983,7 +983,7 @@ CmdResult saveNmeaMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadNmeaMessageFormat(C::mip_interface& device) +TypedResult loadNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -993,7 +993,7 @@ CmdResult loadNmeaMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultNmeaMessageFormat(C::mip_interface& device) +TypedResult defaultNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1014,7 +1014,7 @@ void extract(Serializer& serializer, DeviceSettings& self) } -CmdResult saveDeviceSettings(C::mip_interface& device) +TypedResult saveDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1024,7 +1024,7 @@ CmdResult saveDeviceSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DEVICE_STARTUP_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadDeviceSettings(C::mip_interface& device) +TypedResult loadDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1034,7 +1034,7 @@ CmdResult loadDeviceSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DEVICE_STARTUP_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultDeviceSettings(C::mip_interface& device) +TypedResult defaultDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1076,7 +1076,7 @@ void extract(Serializer& serializer, UartBaudrate::Response& self) } -CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) +TypedResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1088,7 +1088,7 @@ CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) +TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1097,7 +1097,7 @@ CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_UART_BAUDRATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_UART_BAUDRATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1111,7 +1111,7 @@ CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) } return result; } -CmdResult saveUartBaudrate(C::mip_interface& device) +TypedResult saveUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1121,7 +1121,7 @@ CmdResult saveUartBaudrate(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadUartBaudrate(C::mip_interface& device) +TypedResult loadUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1131,7 +1131,7 @@ CmdResult loadUartBaudrate(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultUartBaudrate(C::mip_interface& device) +TypedResult defaultUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1156,7 +1156,7 @@ void extract(Serializer& serializer, FactoryStreaming& self) } -CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved) +TypedResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1209,7 +1209,7 @@ void extract(Serializer& serializer, DatastreamControl::Response& self) } -CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable) +TypedResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1223,7 +1223,7 @@ CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut) +TypedResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1234,7 +1234,7 @@ CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DATASTREAM_ENABLE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DATASTREAM_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1250,7 +1250,7 @@ CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* } return result; } -CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) +TypedResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1262,7 +1262,7 @@ CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) +TypedResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1274,7 +1274,7 @@ CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) +TypedResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1367,7 +1367,7 @@ void extract(Serializer& serializer, ConstellationSettings::Settings& self) } -CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) +TypedResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1385,7 +1385,7 @@ CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChann return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) +TypedResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1394,7 +1394,7 @@ CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChann assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1416,7 +1416,7 @@ CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChann } return result; } -CmdResult saveConstellationSettings(C::mip_interface& device) +TypedResult saveConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1426,7 +1426,7 @@ CmdResult saveConstellationSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadConstellationSettings(C::mip_interface& device) +TypedResult loadConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1436,7 +1436,7 @@ CmdResult loadConstellationSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultConstellationSettings(C::mip_interface& device) +TypedResult defaultConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1504,7 +1504,7 @@ void extract(Serializer& serializer, GnssSbasSettings::Response& self) } -CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns) +TypedResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1524,7 +1524,7 @@ CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, Gn return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut) +TypedResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1533,7 +1533,7 @@ CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SBAS_SETTINGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SBAS_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1555,7 +1555,7 @@ CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, } return result; } -CmdResult saveGnssSbasSettings(C::mip_interface& device) +TypedResult saveGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1565,7 +1565,7 @@ CmdResult saveGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssSbasSettings(C::mip_interface& device) +TypedResult loadGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1575,7 +1575,7 @@ CmdResult loadGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssSbasSettings(C::mip_interface& device) +TypedResult defaultGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1625,7 +1625,7 @@ void extract(Serializer& serializer, GnssAssistedFix::Response& self) } -CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) +TypedResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1639,7 +1639,7 @@ CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::Assist return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) +TypedResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1648,7 +1648,7 @@ CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::Assiste assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1665,7 +1665,7 @@ CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::Assiste } return result; } -CmdResult saveGnssAssistedFix(C::mip_interface& device) +TypedResult saveGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1675,7 +1675,7 @@ CmdResult saveGnssAssistedFix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssAssistedFix(C::mip_interface& device) +TypedResult loadGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1685,7 +1685,7 @@ CmdResult loadGnssAssistedFix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssAssistedFix(C::mip_interface& device) +TypedResult defaultGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1743,7 +1743,7 @@ void extract(Serializer& serializer, GnssTimeAssistance::Response& self) } -CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy) +TypedResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1759,7 +1759,7 @@ CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut) +TypedResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1768,7 +1768,7 @@ CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint1 assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_TIME_ASSISTANCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_TIME_ASSISTANCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1852,7 +1852,7 @@ void extract(Serializer& serializer, ImuLowpassFilter::Response& self) } -CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) +TypedResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1872,7 +1872,7 @@ CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescript return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) +TypedResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1883,7 +1883,7 @@ CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1908,7 +1908,7 @@ CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto } return result; } -CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +TypedResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1920,7 +1920,7 @@ CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +TypedResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1932,7 +1932,7 @@ CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +TypedResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1976,7 +1976,7 @@ void extract(Serializer& serializer, PpsSource::Response& self) } -CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source) +TypedResult writePpsSource(C::mip_interface& device, PpsSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1988,7 +1988,7 @@ CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) +TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1997,7 +1997,7 @@ CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PPS_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PPS_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2011,7 +2011,7 @@ CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) } return result; } -CmdResult savePpsSource(C::mip_interface& device) +TypedResult savePpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2021,7 +2021,7 @@ CmdResult savePpsSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadPpsSource(C::mip_interface& device) +TypedResult loadPpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2031,7 +2031,7 @@ CmdResult loadPpsSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultPpsSource(C::mip_interface& device) +TypedResult defaultPpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2097,7 +2097,7 @@ void extract(Serializer& serializer, GpioConfig::Response& self) } -CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode) +TypedResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2115,7 +2115,7 @@ CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Fea return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut) +TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2126,7 +2126,7 @@ CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feat assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2148,7 +2148,7 @@ CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feat } return result; } -CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin) +TypedResult saveGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2160,7 +2160,7 @@ CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin) +TypedResult loadGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2172,7 +2172,7 @@ CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin) +TypedResult defaultGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2230,7 +2230,7 @@ void extract(Serializer& serializer, GpioState::Response& self) } -CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) +TypedResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2244,7 +2244,7 @@ CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) +TypedResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2255,7 +2255,7 @@ CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_STATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_STATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2319,7 +2319,7 @@ void extract(Serializer& serializer, Odometer::Response& self) } -CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty) +TypedResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2335,7 +2335,7 @@ CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float sca return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut) +TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2344,7 +2344,7 @@ CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOMETER_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOMETER_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2364,7 +2364,7 @@ CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* } return result; } -CmdResult saveOdometer(C::mip_interface& device) +TypedResult saveOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2374,7 +2374,7 @@ CmdResult saveOdometer(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadOdometer(C::mip_interface& device) +TypedResult loadOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2384,7 +2384,7 @@ CmdResult loadOdometer(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultOdometer(C::mip_interface& device) +TypedResult defaultOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2444,7 +2444,7 @@ void extract(Serializer& serializer, GetEventSupport::Info& self) } -CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut) +TypedResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2454,7 +2454,7 @@ CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_SUPPORT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_SUPPORT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_SUPPORT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_SUPPORT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2515,7 +2515,7 @@ void extract(Serializer& serializer, EventControl::Response& self) } -CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode) +TypedResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2529,7 +2529,7 @@ CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventCon return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut) +TypedResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2540,7 +2540,7 @@ CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventCont assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2556,7 +2556,7 @@ CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventCont } return result; } -CmdResult saveEventControl(C::mip_interface& device, uint8_t instance) +TypedResult saveEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2568,7 +2568,7 @@ CmdResult saveEventControl(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEventControl(C::mip_interface& device, uint8_t instance) +TypedResult loadEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2580,7 +2580,7 @@ CmdResult loadEventControl(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance) +TypedResult defaultEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2639,7 +2639,7 @@ void extract(Serializer& serializer, GetEventTriggerStatus::Entry& self) } -CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut) +TypedResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2653,7 +2653,7 @@ CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_STATUS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2716,7 +2716,7 @@ void extract(Serializer& serializer, GetEventActionStatus::Entry& self) } -CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut) +TypedResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2730,7 +2730,7 @@ CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_STATUS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2941,7 +2941,7 @@ void extract(Serializer& serializer, EventTrigger::CombinationParams& self) } -CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters) +TypedResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2970,7 +2970,7 @@ CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTri return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut) +TypedResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2981,7 +2981,7 @@ CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrig assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3012,7 +3012,7 @@ CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrig } return result; } -CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance) +TypedResult saveEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3024,7 +3024,7 @@ CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance) +TypedResult loadEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3036,7 +3036,7 @@ CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance) +TypedResult defaultEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3175,7 +3175,7 @@ void extract(Serializer& serializer, EventAction::MessageParams& self) } -CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters) +TypedResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3201,7 +3201,7 @@ CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut) +TypedResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3212,7 +3212,7 @@ CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* t assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3241,7 +3241,7 @@ CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* t } return result; } -CmdResult saveEventAction(C::mip_interface& device, uint8_t instance) +TypedResult saveEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3253,7 +3253,7 @@ CmdResult saveEventAction(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEventAction(C::mip_interface& device, uint8_t instance) +TypedResult loadEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3265,7 +3265,7 @@ CmdResult loadEventAction(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance) +TypedResult defaultEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3313,7 +3313,7 @@ void extract(Serializer& serializer, AccelBias::Response& self) } -CmdResult writeAccelBias(C::mip_interface& device, const float* bias) +TypedResult writeAccelBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3327,7 +3327,7 @@ CmdResult writeAccelBias(C::mip_interface& device, const float* bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBias(C::mip_interface& device, float* biasOut) +TypedResult readAccelBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3336,7 +3336,7 @@ CmdResult readAccelBias(C::mip_interface& device, float* biasOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3351,7 +3351,7 @@ CmdResult readAccelBias(C::mip_interface& device, float* biasOut) } return result; } -CmdResult saveAccelBias(C::mip_interface& device) +TypedResult saveAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3361,7 +3361,7 @@ CmdResult saveAccelBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelBias(C::mip_interface& device) +TypedResult loadAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3371,7 +3371,7 @@ CmdResult loadAccelBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelBias(C::mip_interface& device) +TypedResult defaultAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3417,7 +3417,7 @@ void extract(Serializer& serializer, GyroBias::Response& self) } -CmdResult writeGyroBias(C::mip_interface& device, const float* bias) +TypedResult writeGyroBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3431,7 +3431,7 @@ CmdResult writeGyroBias(C::mip_interface& device, const float* bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBias(C::mip_interface& device, float* biasOut) +TypedResult readGyroBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3440,7 +3440,7 @@ CmdResult readGyroBias(C::mip_interface& device, float* biasOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3455,7 +3455,7 @@ CmdResult readGyroBias(C::mip_interface& device, float* biasOut) } return result; } -CmdResult saveGyroBias(C::mip_interface& device) +TypedResult saveGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3465,7 +3465,7 @@ CmdResult saveGyroBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGyroBias(C::mip_interface& device) +TypedResult loadGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3475,7 +3475,7 @@ CmdResult loadGyroBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGyroBias(C::mip_interface& device) +TypedResult defaultGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3509,7 +3509,7 @@ void extract(Serializer& serializer, CaptureGyroBias::Response& self) } -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) +TypedResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3519,7 +3519,7 @@ CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, fl assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CAPTURE_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CAPTURE_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3570,7 +3570,7 @@ void extract(Serializer& serializer, MagHardIronOffset::Response& self) } -CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) +TypedResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3584,7 +3584,7 @@ CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) +TypedResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3593,7 +3593,7 @@ CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3608,7 +3608,7 @@ CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveMagHardIronOffset(C::mip_interface& device) +TypedResult saveMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3618,7 +3618,7 @@ CmdResult saveMagHardIronOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagHardIronOffset(C::mip_interface& device) +TypedResult loadMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3628,7 +3628,7 @@ CmdResult loadMagHardIronOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagHardIronOffset(C::mip_interface& device) +TypedResult defaultMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3674,7 +3674,7 @@ void extract(Serializer& serializer, MagSoftIronMatrix::Response& self) } -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) +TypedResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3688,7 +3688,7 @@ CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) +TypedResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3697,7 +3697,7 @@ CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_COMP_MATRIX, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_COMP_MATRIX, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3712,7 +3712,7 @@ CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveMagSoftIronMatrix(C::mip_interface& device) +TypedResult saveMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3722,7 +3722,7 @@ CmdResult saveMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagSoftIronMatrix(C::mip_interface& device) +TypedResult loadMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3732,7 +3732,7 @@ CmdResult loadMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagSoftIronMatrix(C::mip_interface& device) +TypedResult defaultMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3774,7 +3774,7 @@ void extract(Serializer& serializer, ConingScullingEnable::Response& self) } -CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable) +TypedResult writeConingScullingEnable(C::mip_interface& device, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3786,7 +3786,7 @@ CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) +TypedResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3795,7 +3795,7 @@ CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3809,7 +3809,7 @@ CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) } return result; } -CmdResult saveConingScullingEnable(C::mip_interface& device) +TypedResult saveConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3819,7 +3819,7 @@ CmdResult saveConingScullingEnable(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadConingScullingEnable(C::mip_interface& device) +TypedResult loadConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3829,7 +3829,7 @@ CmdResult loadConingScullingEnable(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultConingScullingEnable(C::mip_interface& device) +TypedResult defaultConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3887,7 +3887,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformEuler::Response& sel } -CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw) +TypedResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3903,7 +3903,7 @@ CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) +TypedResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3912,7 +3912,7 @@ CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* roll assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3932,7 +3932,7 @@ CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* roll } return result; } -CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device) +TypedResult saveSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3942,7 +3942,7 @@ CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device) +TypedResult loadSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3952,7 +3952,7 @@ CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device) +TypedResult defaultSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3998,7 +3998,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response } -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) +TypedResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4012,7 +4012,7 @@ CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) +TypedResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4021,7 +4021,7 @@ CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4036,7 +4036,7 @@ CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* } return result; } -CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) +TypedResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4046,7 +4046,7 @@ CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) +TypedResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4056,7 +4056,7 @@ CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device) +TypedResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4102,7 +4102,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self) } -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) +TypedResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4116,7 +4116,7 @@ CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) +TypedResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4125,7 +4125,7 @@ CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4140,7 +4140,7 @@ CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut } return result; } -CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device) +TypedResult saveSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4150,7 +4150,7 @@ CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device) +TypedResult loadSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4160,7 +4160,7 @@ CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device) +TypedResult defaultSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4226,7 +4226,7 @@ void extract(Serializer& serializer, ComplementaryFilter::Response& self) } -CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant) +TypedResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4244,7 +4244,7 @@ CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnabl return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut) +TypedResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4253,7 +4253,7 @@ CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnabl assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LEGACY_COMP_FILTER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LEGACY_COMP_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4276,7 +4276,7 @@ CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnabl } return result; } -CmdResult saveComplementaryFilter(C::mip_interface& device) +TypedResult saveComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4286,7 +4286,7 @@ CmdResult saveComplementaryFilter(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadComplementaryFilter(C::mip_interface& device) +TypedResult loadComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4296,7 +4296,7 @@ CmdResult loadComplementaryFilter(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultComplementaryFilter(C::mip_interface& device) +TypedResult defaultComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4346,7 +4346,7 @@ void extract(Serializer& serializer, SensorRange::Response& self) } -CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting) +TypedResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4360,7 +4360,7 @@ CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uin return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut) +TypedResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4371,7 +4371,7 @@ CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_RANGE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_RANGE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4387,7 +4387,7 @@ CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint } return result; } -CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) +TypedResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4399,7 +4399,7 @@ CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) +TypedResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4411,7 +4411,7 @@ CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor) +TypedResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4469,7 +4469,7 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Entry& self) } -CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut) +TypedResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4479,7 +4479,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CALIBRATED_RANGES, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CALIBRATED_RANGES, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CALIBRATED_RANGES, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CALIBRATED_RANGES, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4561,7 +4561,7 @@ void extract(Serializer& serializer, LowpassFilter::Response& self) } -CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) +TypedResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4581,7 +4581,7 @@ CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) +TypedResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4594,7 +4594,7 @@ CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LOWPASS_FILTER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LOWPASS_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4618,7 +4618,7 @@ CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f } return result; } -CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +TypedResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4632,7 +4632,7 @@ CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +TypedResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4646,7 +4646,7 @@ CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +TypedResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index ea75a2a6d..b98e35c98 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -1132,13 +1132,10 @@ static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BE static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_RISING = 1; ///< Rising edges will be timestamped. static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_FALLING = 2; ///< Falling edges will be timestamped. static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_TIMESTAMP_EITHER = 3; ///< Both rising and falling edges will be timestamped. -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_DEFAULT = 1; ///< UART transmit line (auto-select port). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_DEFAULT = 2; ///< UART receive line (auto-select port). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_AUX1 = 33; ///< UART transmit line, port 2 (aux port 1). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_AUX1 = 34; ///< UART receive line, port 2 (aux port 1). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_TX_AUX2 = 49; ///< UART transmit line, port 3 (aux port 2). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_RX_AUX2 = 50; ///< UART receive line, port 3 (aux port 2). -static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_POWER_SHUTDOWN = 1; ///< A logic 1 applied to the pin will place the device in low-power mode. A full restart is executed after the signal is removed. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT2_TX = 33; ///< (0x21) UART port 2 transmit. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT2_RX = 34; ///< (0x22) UART port 2 receive. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT3_TX = 49; ///< (0x31) UART port 3 transmit. +static const mip_3dm_gpio_config_command_behavior MIP_3DM_GPIO_CONFIG_COMMAND_BEHAVIOR_UART_PORT3_RX = 50; ///< (0x32) UART port 3 receive. typedef uint8_t mip_3dm_gpio_config_command_pin_mode; static const mip_3dm_gpio_config_command_pin_mode MIP_3DM_GPIO_CONFIG_COMMAND_PIN_MODE_NONE = 0x00; diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 76bafd152..9317e4080 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -210,11 +210,12 @@ struct PollImuMessage uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. DescriptorRate descriptors[83]; ///< Descriptor list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -230,7 +231,7 @@ struct PollImuMessage void insert(Serializer& serializer, const PollImuMessage& self); void extract(Serializer& serializer, PollImuMessage& self); -CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -253,11 +254,12 @@ struct PollGnssMessage uint8_t num_descriptors = 0; ///< Number of descriptors in the descriptor list. DescriptorRate descriptors[83]; ///< Descriptor list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -273,7 +275,7 @@ struct PollGnssMessage void insert(Serializer& serializer, const PollGnssMessage& self); void extract(Serializer& serializer, PollGnssMessage& self); -CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -296,11 +298,12 @@ struct PollFilterMessage uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. DescriptorRate descriptors[83]; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -316,7 +319,7 @@ struct PollFilterMessage void insert(Serializer& serializer, const PollFilterMessage& self); void extract(Serializer& serializer, PollFilterMessage& self); -CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -334,17 +337,18 @@ struct ImuMessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -365,11 +369,12 @@ struct ImuMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. @@ -386,11 +391,11 @@ void extract(Serializer& serializer, ImuMessageFormat& self); void insert(Serializer& serializer, const ImuMessageFormat::Response& self); void extract(Serializer& serializer, ImuMessageFormat::Response& self); -CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveImuMessageFormat(C::mip_interface& device); -CmdResult loadImuMessageFormat(C::mip_interface& device); -CmdResult defaultImuMessageFormat(C::mip_interface& device); +TypedResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveImuMessageFormat(C::mip_interface& device); +TypedResult loadImuMessageFormat(C::mip_interface& device); +TypedResult defaultImuMessageFormat(C::mip_interface& device); ///@} /// @@ -408,17 +413,18 @@ struct GpsMessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -439,11 +445,12 @@ struct GpsMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors DescriptorRate descriptors[82]; ///< Descriptor format list. @@ -460,11 +467,11 @@ void extract(Serializer& serializer, GpsMessageFormat& self); void insert(Serializer& serializer, const GpsMessageFormat::Response& self); void extract(Serializer& serializer, GpsMessageFormat::Response& self); -CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveGpsMessageFormat(C::mip_interface& device); -CmdResult loadGpsMessageFormat(C::mip_interface& device); -CmdResult defaultGpsMessageFormat(C::mip_interface& device); +TypedResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveGpsMessageFormat(C::mip_interface& device); +TypedResult loadGpsMessageFormat(C::mip_interface& device); +TypedResult defaultGpsMessageFormat(C::mip_interface& device); ///@} /// @@ -482,17 +489,18 @@ struct FilterMessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) DescriptorRate descriptors[82]; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -513,11 +521,12 @@ struct FilterMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) DescriptorRate descriptors[82]; @@ -534,11 +543,11 @@ void extract(Serializer& serializer, FilterMessageFormat& self); void insert(Serializer& serializer, const FilterMessageFormat::Response& self); void extract(Serializer& serializer, FilterMessageFormat::Response& self); -CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveFilterMessageFormat(C::mip_interface& device); -CmdResult loadFilterMessageFormat(C::mip_interface& device); -CmdResult defaultFilterMessageFormat(C::mip_interface& device); +TypedResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveFilterMessageFormat(C::mip_interface& device); +TypedResult loadFilterMessageFormat(C::mip_interface& device); +TypedResult defaultFilterMessageFormat(C::mip_interface& device); ///@} /// @@ -554,12 +563,13 @@ CmdResult defaultFilterMessageFormat(C::mip_interface& device); struct ImuGetBaseRate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -572,11 +582,12 @@ struct ImuGetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] @@ -592,7 +603,7 @@ void extract(Serializer& serializer, ImuGetBaseRate& self); void insert(Serializer& serializer, const ImuGetBaseRate::Response& self); void extract(Serializer& serializer, ImuGetBaseRate::Response& self); -CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +TypedResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -608,12 +619,13 @@ CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); struct GpsGetBaseRate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -626,11 +638,12 @@ struct GpsGetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] @@ -646,7 +659,7 @@ void extract(Serializer& serializer, GpsGetBaseRate& self); void insert(Serializer& serializer, const GpsGetBaseRate::Response& self); void extract(Serializer& serializer, GpsGetBaseRate::Response& self); -CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +TypedResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -662,12 +675,13 @@ CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); struct FilterGetBaseRate { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -680,11 +694,12 @@ struct FilterGetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint16_t rate = 0; ///< [hz] @@ -700,7 +715,7 @@ void extract(Serializer& serializer, FilterGetBaseRate& self); void insert(Serializer& serializer, const FilterGetBaseRate::Response& self); void extract(Serializer& serializer, FilterGetBaseRate::Response& self); -CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +TypedResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -724,11 +739,12 @@ struct PollData uint8_t num_descriptors = 0; ///< Number of descriptors in the format list. uint8_t descriptors[82] = {0}; ///< Descriptor format list. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; auto as_tuple() const { @@ -744,7 +760,7 @@ struct PollData void insert(Serializer& serializer, const PollData& self); void extract(Serializer& serializer, PollData& self); -CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors); +TypedResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors); ///@} /// @@ -758,12 +774,13 @@ struct GetBaseRate { uint8_t desc_set = 0; ///< This is the data descriptor set. It must be a supported descriptor. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -776,11 +793,12 @@ struct GetBaseRate } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; ///< Echoes the parameter in the command. uint16_t rate = 0; ///< Base rate in Hz (0 = variable, unknown, or user-defined rate. Data will be sent when received). @@ -797,7 +815,7 @@ void extract(Serializer& serializer, GetBaseRate& self); void insert(Serializer& serializer, const GetBaseRate::Response& self); void extract(Serializer& serializer, GetBaseRate::Response& self); -CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut); +TypedResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut); ///@} /// @@ -816,17 +834,18 @@ struct MessageFormat uint8_t num_descriptors = 0; ///< Number of descriptors (limited by payload size) DescriptorRate descriptors[82]; ///< List of descriptors and decimations. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -848,11 +867,12 @@ struct MessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; uint8_t desc_set = 0; ///< Echoes the descriptor set from the command. uint8_t num_descriptors = 0; ///< Number of descriptors in the list. DescriptorRate descriptors[82]; ///< List of descriptors and decimations. @@ -870,11 +890,11 @@ void extract(Serializer& serializer, MessageFormat& self); void insert(Serializer& serializer, const MessageFormat::Response& self); void extract(Serializer& serializer, MessageFormat::Response& self); -CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet); -CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet); -CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); +TypedResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveMessageFormat(C::mip_interface& device, uint8_t descSet); +TypedResult loadMessageFormat(C::mip_interface& device, uint8_t descSet); +TypedResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); ///@} /// @@ -896,11 +916,12 @@ struct NmeaPollData uint8_t count = 0; ///< Number of format entries (limited by payload size) NmeaMessage format_entries[40]; ///< List of format entries. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -916,7 +937,7 @@ struct NmeaPollData void insert(Serializer& serializer, const NmeaPollData& self); void extract(Serializer& serializer, NmeaPollData& self); -CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries); +TypedResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries); ///@} /// @@ -932,17 +953,18 @@ struct NmeaMessageFormat uint8_t count = 0; ///< Number of format entries (limited by payload size) NmeaMessage format_entries[40]; ///< List of format entries. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -963,11 +985,12 @@ struct NmeaMessageFormat struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of format entries (limited by payload size) NmeaMessage format_entries[40]; ///< List of format entries. @@ -984,11 +1007,11 @@ void extract(Serializer& serializer, NmeaMessageFormat& self); void insert(Serializer& serializer, const NmeaMessageFormat::Response& self); void extract(Serializer& serializer, NmeaMessageFormat::Response& self); -CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries); -CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut); -CmdResult saveNmeaMessageFormat(C::mip_interface& device); -CmdResult loadNmeaMessageFormat(C::mip_interface& device); -CmdResult defaultNmeaMessageFormat(C::mip_interface& device); +TypedResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries); +TypedResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut); +TypedResult saveNmeaMessageFormat(C::mip_interface& device); +TypedResult loadNmeaMessageFormat(C::mip_interface& device); +TypedResult defaultNmeaMessageFormat(C::mip_interface& device); ///@} /// @@ -1006,16 +1029,17 @@ struct DeviceSettings { FunctionSelector function = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x0000; - static const uint32_t READ_PARAMS = 0x0000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x0000; + static constexpr const uint32_t READ_PARAMS = 0x0000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1039,9 +1063,9 @@ struct DeviceSettings void insert(Serializer& serializer, const DeviceSettings& self); void extract(Serializer& serializer, DeviceSettings& self); -CmdResult saveDeviceSettings(C::mip_interface& device); -CmdResult loadDeviceSettings(C::mip_interface& device); -CmdResult defaultDeviceSettings(C::mip_interface& device); +TypedResult saveDeviceSettings(C::mip_interface& device); +TypedResult loadDeviceSettings(C::mip_interface& device); +TypedResult defaultDeviceSettings(C::mip_interface& device); ///@} /// @@ -1070,17 +1094,18 @@ struct UartBaudrate FunctionSelector function = static_cast(0); uint32_t baud = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1101,11 +1126,12 @@ struct UartBaudrate struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint32_t baud = 0; @@ -1121,11 +1147,11 @@ void extract(Serializer& serializer, UartBaudrate& self); void insert(Serializer& serializer, const UartBaudrate::Response& self); void extract(Serializer& serializer, UartBaudrate::Response& self); -CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud); -CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut); -CmdResult saveUartBaudrate(C::mip_interface& device); -CmdResult loadUartBaudrate(C::mip_interface& device); -CmdResult defaultUartBaudrate(C::mip_interface& device); +TypedResult writeUartBaudrate(C::mip_interface& device, uint32_t baud); +TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut); +TypedResult saveUartBaudrate(C::mip_interface& device); +TypedResult loadUartBaudrate(C::mip_interface& device); +TypedResult defaultUartBaudrate(C::mip_interface& device); ///@} /// @@ -1150,11 +1176,12 @@ struct FactoryStreaming Action action = static_cast(0); uint8_t reserved = 0; ///< Reserved. Set to 0x00. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1170,7 +1197,7 @@ struct FactoryStreaming void insert(Serializer& serializer, const FactoryStreaming& self); void extract(Serializer& serializer, FactoryStreaming& self); -CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved); +TypedResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved); ///@} /// @@ -1187,25 +1214,26 @@ CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action ac struct DatastreamControl { - static const uint8_t LEGACY_IMU_STREAM = 0x01; - static const uint8_t LEGACY_GNSS_STREAM = 0x02; - static const uint8_t LEGACY_FILTER_STREAM = 0x03; - static const uint8_t ALL_STREAMS = 0x00; + static constexpr const uint8_t LEGACY_IMU_STREAM = 0x01; + static constexpr const uint8_t LEGACY_GNSS_STREAM = 0x02; + static constexpr const uint8_t LEGACY_FILTER_STREAM = 0x03; + static constexpr const uint8_t ALL_STREAMS = 0x00; FunctionSelector function = static_cast(0); uint8_t desc_set = 0; ///< The descriptor set of the stream to control. When function is SAVE, LOAD, or DEFAULT, can be ALL_STREAMS(0) to apply to all descriptor sets. On Generation 5 products, this must be one of the above legacy constants. bool enable = 0; ///< True or false to enable or disable the stream. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1227,11 +1255,12 @@ struct DatastreamControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; bool enabled = 0; @@ -1248,11 +1277,11 @@ void extract(Serializer& serializer, DatastreamControl& self); void insert(Serializer& serializer, const DatastreamControl::Response& self); void extract(Serializer& serializer, DatastreamControl::Response& self); -CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable); -CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut); -CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet); -CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet); -CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); +TypedResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable); +TypedResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut); +TypedResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet); +TypedResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet); +TypedResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); ///@} /// @@ -1305,7 +1334,7 @@ struct ConstellationSettings OptionFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } OptionFlags& operator=(uint16_t val) { value = val; return *this; } - OptionFlags& operator=(int val) { value = val; return *this; } + OptionFlags& operator=(int val) { value = uint16_t(val); return *this; } OptionFlags& operator|=(uint16_t val) { return *this = value | val; } OptionFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1330,17 +1359,18 @@ struct ConstellationSettings uint8_t config_count = 0; Settings settings[42]; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; auto as_tuple() const { @@ -1361,11 +1391,12 @@ struct ConstellationSettings struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; uint16_t max_channels_available = 0; ///< Maximum channels available uint16_t max_channels_use = 0; ///< Maximum channels to use uint8_t config_count = 0; ///< Number of constellation configurations @@ -1387,11 +1418,11 @@ void extract(Serializer& serializer, ConstellationSettings::Settings& self); void insert(Serializer& serializer, const ConstellationSettings::Response& self); void extract(Serializer& serializer, ConstellationSettings::Response& self); -CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); -CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); -CmdResult saveConstellationSettings(C::mip_interface& device); -CmdResult loadConstellationSettings(C::mip_interface& device); -CmdResult defaultConstellationSettings(C::mip_interface& device); +TypedResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); +TypedResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); +TypedResult saveConstellationSettings(C::mip_interface& device); +TypedResult loadConstellationSettings(C::mip_interface& device); +TypedResult defaultConstellationSettings(C::mip_interface& device); ///@} /// @@ -1422,7 +1453,7 @@ struct GnssSbasSettings SBASOptions(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } SBASOptions& operator=(uint16_t val) { value = val; return *this; } - SBASOptions& operator=(int val) { value = val; return *this; } + SBASOptions& operator=(int val) { value = uint16_t(val); return *this; } SBASOptions& operator|=(uint16_t val) { return *this = value | val; } SBASOptions& operator&=(uint16_t val) { return *this = value & val; } @@ -1443,17 +1474,18 @@ struct GnssSbasSettings uint8_t num_included_prns = 0; ///< Number of SBAS PRNs to include in search (0 = include all) uint16_t included_prns[39] = {0}; ///< List of specific SBAS PRNs to search for - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; auto as_tuple() const { @@ -1474,11 +1506,12 @@ struct GnssSbasSettings struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; uint8_t enable_sbas = 0; ///< 0 - SBAS Disabled, 1 - SBAS enabled SBASOptions sbas_options; ///< SBAS options, see definition uint8_t num_included_prns = 0; ///< Number of SBAS PRNs to include in search (0 = include all) @@ -1497,11 +1530,11 @@ void extract(Serializer& serializer, GnssSbasSettings& self); void insert(Serializer& serializer, const GnssSbasSettings::Response& self); void extract(Serializer& serializer, GnssSbasSettings::Response& self); -CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns); -CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut); -CmdResult saveGnssSbasSettings(C::mip_interface& device); -CmdResult loadGnssSbasSettings(C::mip_interface& device); -CmdResult defaultGnssSbasSettings(C::mip_interface& device); +TypedResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns); +TypedResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut); +TypedResult saveGnssSbasSettings(C::mip_interface& device); +TypedResult loadGnssSbasSettings(C::mip_interface& device); +TypedResult defaultGnssSbasSettings(C::mip_interface& device); ///@} /// @@ -1533,17 +1566,18 @@ struct GnssAssistedFix AssistedFixOption option = static_cast(0); ///< Assisted fix options uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1564,11 +1598,12 @@ struct GnssAssistedFix struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AssistedFixOption option = static_cast(0); ///< Assisted fix options uint8_t flags = 0; ///< Assisted fix flags (set to 0xFF) @@ -1585,11 +1620,11 @@ void extract(Serializer& serializer, GnssAssistedFix& self); void insert(Serializer& serializer, const GnssAssistedFix::Response& self); void extract(Serializer& serializer, GnssAssistedFix::Response& self); -CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); -CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); -CmdResult saveGnssAssistedFix(C::mip_interface& device); -CmdResult loadGnssAssistedFix(C::mip_interface& device); -CmdResult defaultGnssAssistedFix(C::mip_interface& device); +TypedResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); +TypedResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); +TypedResult saveGnssAssistedFix(C::mip_interface& device); +TypedResult loadGnssAssistedFix(C::mip_interface& device); +TypedResult defaultGnssAssistedFix(C::mip_interface& device); ///@} /// @@ -1609,17 +1644,18 @@ struct GnssTimeAssistance uint16_t week_number = 0; ///< GPS Weeks since 1980 [weeks] float accuracy = 0; ///< Accuracy of time information [seconds] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1640,11 +1676,12 @@ struct GnssTimeAssistance struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; double tow = 0; ///< GPS Time of week [seconds] uint16_t week_number = 0; ///< GPS Weeks since 1980 [weeks] float accuracy = 0; ///< Accuracy of time information [seconds] @@ -1662,8 +1699,8 @@ void extract(Serializer& serializer, GnssTimeAssistance& self); void insert(Serializer& serializer, const GnssTimeAssistance::Response& self); void extract(Serializer& serializer, GnssTimeAssistance::Response& self); -CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy); -CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut); +TypedResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy); +TypedResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut); ///@} /// @@ -1697,17 +1734,18 @@ struct ImuLowpassFilter uint16_t frequency = 0; ///< -3dB cutoff frequency in Hz. Will not affect filtering if 'manual' is false. uint8_t reserved = 0; ///< Reserved, set to 0x00. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1729,11 +1767,12 @@ struct ImuLowpassFilter struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t target_descriptor = 0; bool enable = 0; ///< True if the filter is currently enabled. bool manual = 0; ///< True if the filter cutoff was manually configured. @@ -1753,11 +1792,11 @@ void extract(Serializer& serializer, ImuLowpassFilter& self); void insert(Serializer& serializer, const ImuLowpassFilter::Response& self); void extract(Serializer& serializer, ImuLowpassFilter::Response& self); -CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); -CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); -CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +TypedResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); +TypedResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); +TypedResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +TypedResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +TypedResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); ///@} /// @@ -1781,17 +1820,18 @@ struct PpsSource FunctionSelector function = static_cast(0); Source source = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1812,11 +1852,12 @@ struct PpsSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); @@ -1832,11 +1873,11 @@ void extract(Serializer& serializer, PpsSource& self); void insert(Serializer& serializer, const PpsSource::Response& self); void extract(Serializer& serializer, PpsSource::Response& self); -CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source); -CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut); -CmdResult savePpsSource(C::mip_interface& device); -CmdResult loadPpsSource(C::mip_interface& device); -CmdResult defaultPpsSource(C::mip_interface& device); +TypedResult writePpsSource(C::mip_interface& device, PpsSource::Source source); +TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut); +TypedResult savePpsSource(C::mip_interface& device); +TypedResult loadPpsSource(C::mip_interface& device); +TypedResult defaultPpsSource(C::mip_interface& device); ///@} /// @@ -1887,13 +1928,10 @@ struct GpioConfig TIMESTAMP_RISING = 1, ///< Rising edges will be timestamped. TIMESTAMP_FALLING = 2, ///< Falling edges will be timestamped. TIMESTAMP_EITHER = 3, ///< Both rising and falling edges will be timestamped. - UART_TX_DEFAULT = 1, ///< UART transmit line (auto-select port). - UART_RX_DEFAULT = 2, ///< UART receive line (auto-select port). - UART_TX_AUX1 = 33, ///< UART transmit line, port 2 (aux port 1). - UART_RX_AUX1 = 34, ///< UART receive line, port 2 (aux port 1). - UART_TX_AUX2 = 49, ///< UART transmit line, port 3 (aux port 2). - UART_RX_AUX2 = 50, ///< UART receive line, port 3 (aux port 2). - POWER_SHUTDOWN = 1, ///< A logic 1 applied to the pin will place the device in low-power mode. A full restart is executed after the signal is removed. + UART_PORT2_TX = 33, ///< (0x21) UART port 2 transmit. + UART_PORT2_RX = 34, ///< (0x22) UART port 2 receive. + UART_PORT3_TX = 49, ///< (0x31) UART port 3 transmit. + UART_PORT3_RX = 50, ///< (0x32) UART port 3 receive. }; struct PinMode : Bitfield @@ -1912,7 +1950,7 @@ struct GpioConfig PinMode(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } PinMode& operator=(uint8_t val) { value = val; return *this; } - PinMode& operator=(int val) { value = val; return *this; } + PinMode& operator=(int val) { value = uint8_t(val); return *this; } PinMode& operator|=(uint8_t val) { return *this = value | val; } PinMode& operator&=(uint8_t val) { return *this = value & val; } @@ -1933,17 +1971,18 @@ struct GpioConfig Behavior behavior = static_cast(0); ///< Select an appropriate value from the enumeration based on the selected feature (e.g. for PPS, select one of the values prefixed with PPS_.) PinMode pin_mode; ///< GPIO configuration. May be restricted depending on device, pin, feature, and behavior. See device user manual. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1965,11 +2004,12 @@ struct GpioConfig struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t pin = 0; ///< GPIO pin number counting from 1. For save, load, and default function selectors, this can be 0 to select all pins. Feature feature = static_cast(0); ///< Determines how the pin will be used. Behavior behavior = static_cast(0); ///< Select an appropriate value from the enumeration based on the selected feature (e.g. for PPS, select one of the values prefixed with PPS_.) @@ -1988,11 +2028,11 @@ void extract(Serializer& serializer, GpioConfig& self); void insert(Serializer& serializer, const GpioConfig::Response& self); void extract(Serializer& serializer, GpioConfig::Response& self); -CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode); -CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut); -CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin); -CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin); -CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); +TypedResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode); +TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut); +TypedResult saveGpioConfig(C::mip_interface& device, uint8_t pin); +TypedResult loadGpioConfig(C::mip_interface& device, uint8_t pin); +TypedResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); ///@} /// @@ -2023,17 +2063,18 @@ struct GpioState uint8_t pin = 0; ///< GPIO pin number counting from 1. Cannot be 0. bool state = 0; ///< The pin state. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2054,11 +2095,12 @@ struct GpioState struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t pin = 0; ///< GPIO pin number counting from 1. Cannot be 0. bool state = 0; ///< The pin state. @@ -2075,8 +2117,8 @@ void extract(Serializer& serializer, GpioState& self); void insert(Serializer& serializer, const GpioState::Response& self); void extract(Serializer& serializer, GpioState::Response& self); -CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state); -CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); +TypedResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state); +TypedResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); ///@} /// @@ -2100,17 +2142,18 @@ struct Odometer float scaling = 0; ///< Encoder pulses per meter of distance traveled [pulses/m]. Distance traveled is computed using the formula d = p / N * 2R * pi, where d is distance, p is the number of pulses received, N is the encoder resolution, and R is the wheel radius. By simplifying all of the parameters into one, the formula d = p / S is obtained, where s is the odometer scaling factor passed to this command. S is equivalent to N / (2R * pi) and has units of pulses / meter. N is in units of "A" pulses per revolution and R is in meters. Make this value negative if the odometer is mounted so that it rotates backwards. float uncertainty = 0; ///< Uncertainty in encoder counts to distance translation (1-sigma value) [m/m]. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2131,11 +2174,12 @@ struct Odometer struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Mode mode = static_cast(0); ///< Mode setting. float scaling = 0; ///< Encoder pulses per meter of distance traveled [pulses/m]. Distance traveled is computed using the formula d = p / N * 2R * pi, where d is distance, p is the number of pulses received, N is the encoder resolution, and R is the wheel radius. By simplifying all of the parameters into one, the formula d = p / S is obtained, where s is the odometer scaling factor passed to this command. S is equivalent to N / (2R * pi) and has units of pulses / meter. N is in units of "A" pulses per revolution and R is in meters. Make this value negative if the odometer is mounted so that it rotates backwards. float uncertainty = 0; ///< Uncertainty in encoder counts to distance translation (1-sigma value) [m/m]. @@ -2153,11 +2197,11 @@ void extract(Serializer& serializer, Odometer& self); void insert(Serializer& serializer, const Odometer::Response& self); void extract(Serializer& serializer, Odometer::Response& self); -CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty); -CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut); -CmdResult saveOdometer(C::mip_interface& device); -CmdResult loadOdometer(C::mip_interface& device); -CmdResult defaultOdometer(C::mip_interface& device); +TypedResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty); +TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut); +TypedResult saveOdometer(C::mip_interface& device); +TypedResult loadOdometer(C::mip_interface& device); +TypedResult defaultOdometer(C::mip_interface& device); ///@} /// @@ -2199,12 +2243,13 @@ struct GetEventSupport }; Query query = static_cast(0); ///< What type of information to retrieve. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2217,11 +2262,12 @@ struct GetEventSupport } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x000000C0; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; Query query = static_cast(0); ///< Query type specified in the command. uint8_t max_instances = 0; ///< Number of slots available. The 'instance' number for the configuration or control commands must be between 1 and this value. uint8_t num_entries = 0; ///< Number of supported types. @@ -2243,7 +2289,7 @@ void extract(Serializer& serializer, GetEventSupport::Info& self); void insert(Serializer& serializer, const GetEventSupport::Response& self); void extract(Serializer& serializer, GetEventSupport::Response& self); -CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut); +TypedResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut); ///@} /// @@ -2276,17 +2322,18 @@ struct EventControl uint8_t instance = 0; ///< Trigger instance to affect. 0 can be used to apply the mode to all configured triggers, except when the function selector is READ. Mode mode = static_cast(0); ///< How to change the trigger state. Except when instance is 0, the corresponding trigger must be configured, i.e. not have type 0. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2308,11 +2355,12 @@ struct EventControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Trigger instance to affect. 0 can be used to apply the mode to all configured triggers, except when the function selector is READ. Mode mode = static_cast(0); ///< How to change the trigger state. Except when instance is 0, the corresponding trigger must be configured, i.e. not have type 0. @@ -2329,11 +2377,11 @@ void extract(Serializer& serializer, EventControl& self); void insert(Serializer& serializer, const EventControl::Response& self); void extract(Serializer& serializer, EventControl::Response& self); -CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode); -CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut); -CmdResult saveEventControl(C::mip_interface& device, uint8_t instance); -CmdResult loadEventControl(C::mip_interface& device, uint8_t instance); -CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance); +TypedResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode); +TypedResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut); +TypedResult saveEventControl(C::mip_interface& device, uint8_t instance); +TypedResult loadEventControl(C::mip_interface& device, uint8_t instance); +TypedResult defaultEventControl(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2360,7 +2408,7 @@ struct GetEventTriggerStatus Status(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } Status& operator=(uint8_t val) { value = val; return *this; } - Status& operator=(int val) { value = val; return *this; } + Status& operator=(int val) { value = uint8_t(val); return *this; } Status& operator|=(uint8_t val) { return *this = value | val; } Status& operator&=(uint8_t val) { return *this = value & val; } @@ -2384,12 +2432,13 @@ struct GetEventTriggerStatus uint8_t requested_count = 0; ///< Number of entries requested. If 0, requests all trigger slots. uint8_t requested_instances[20] = {0}; ///< List of trigger instances to query. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -2402,11 +2451,12 @@ struct GetEventTriggerStatus } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of entries requested. If requested_count was 0, this is the number of supported trigger slots. Entry triggers[20]; ///< A list of the configured triggers. Entries are in the order requested, or in increasing order if count was 0. @@ -2426,7 +2476,7 @@ void extract(Serializer& serializer, GetEventTriggerStatus::Entry& self); void insert(Serializer& serializer, const GetEventTriggerStatus::Response& self); void extract(Serializer& serializer, GetEventTriggerStatus::Response& self); -CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut); +TypedResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut); ///@} /// @@ -2446,12 +2496,13 @@ struct GetEventActionStatus uint8_t requested_count = 0; ///< Number of entries requested. If 0, requests all action slots. uint8_t requested_instances[20] = {0}; ///< List of action instances to query. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; auto as_tuple() const { @@ -2464,11 +2515,12 @@ struct GetEventActionStatus } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t count = 0; ///< Number of entries requested. If requested_count was 0, this is the number of supported action slots. Entry actions[20]; ///< A list of the configured actions. Entries are in the order requested, or in increasing order if count was 0. @@ -2488,7 +2540,7 @@ void extract(Serializer& serializer, GetEventActionStatus::Entry& self); void insert(Serializer& serializer, const GetEventActionStatus::Response& self); void extract(Serializer& serializer, GetEventActionStatus::Response& self); -CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut); +TypedResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut); ///@} /// @@ -2540,19 +2592,19 @@ struct EventTrigger }; struct CombinationParams { - static const uint16_t LOGIC_NEVER = 0x0000; - static const uint16_t LOGIC_ALWAYS = 0xFFFF; - static const uint16_t LOGIC_NONE = 0x0001; - static const uint16_t LOGIC_OR = 0xFFFE; - static const uint16_t LOGIC_NAND = 0x7FFF; - static const uint16_t LOGIC_XOR_ONE = 0x0116; - static const uint16_t LOGIC_ONLY_A = 0x0002; - static const uint16_t LOGIC_ONLY_B = 0x0004; - static const uint16_t LOGIC_ONLY_C = 0x0010; - static const uint16_t LOGIC_ONLY_D = 0x0100; - static const uint16_t LOGIC_AND_AB = 0x8888; - static const uint16_t LOGIC_AB_OR_C = 0xF8F8; - static const uint16_t LOGIC_AND = 0x8000; + static constexpr const uint16_t LOGIC_NEVER = 0x0000; + static constexpr const uint16_t LOGIC_ALWAYS = 0xFFFF; + static constexpr const uint16_t LOGIC_NONE = 0x0001; + static constexpr const uint16_t LOGIC_OR = 0xFFFE; + static constexpr const uint16_t LOGIC_NAND = 0x7FFF; + static constexpr const uint16_t LOGIC_XOR_ONE = 0x0116; + static constexpr const uint16_t LOGIC_ONLY_A = 0x0002; + static constexpr const uint16_t LOGIC_ONLY_B = 0x0004; + static constexpr const uint16_t LOGIC_ONLY_C = 0x0010; + static constexpr const uint16_t LOGIC_ONLY_D = 0x0100; + static constexpr const uint16_t LOGIC_AND_AB = 0x8888; + static constexpr const uint16_t LOGIC_AB_OR_C = 0xF8F8; + static constexpr const uint16_t LOGIC_AND = 0x8000; uint16_t logic_table = 0; ///< The last column of a truth table describing the output given the state of each input. uint8_t input_triggers[4] = {0}; ///< List of trigger IDs for inputs. Use 0 for unused inputs. @@ -2578,17 +2630,18 @@ struct EventTrigger Type type = static_cast(0); ///< Type of trigger to configure. Parameters parameters; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2610,11 +2663,12 @@ struct EventTrigger struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Trigger number. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all instances. Type type = static_cast(0); ///< Type of trigger to configure. Parameters parameters; @@ -2641,11 +2695,11 @@ void extract(Serializer& serializer, EventTrigger::CombinationParams& self); void insert(Serializer& serializer, const EventTrigger::Response& self); void extract(Serializer& serializer, EventTrigger::Response& self); -CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters); -CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut); -CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance); -CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance); -CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); +TypedResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters); +TypedResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut); +TypedResult saveEventTrigger(C::mip_interface& device, uint8_t instance); +TypedResult loadEventTrigger(C::mip_interface& device, uint8_t instance); +TypedResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2701,17 +2755,18 @@ struct EventAction Type type = static_cast(0); ///< Type of action to configure. Parameters parameters; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2733,11 +2788,12 @@ struct EventAction struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t instance = 0; ///< Action number. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all instances. uint8_t trigger = 0; ///< Trigger ID number. Type type = static_cast(0); ///< Type of action to configure. @@ -2762,11 +2818,11 @@ void extract(Serializer& serializer, EventAction::MessageParams& self); void insert(Serializer& serializer, const EventAction::Response& self); void extract(Serializer& serializer, EventAction::Response& self); -CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters); -CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut); -CmdResult saveEventAction(C::mip_interface& device, uint8_t instance); -CmdResult loadEventAction(C::mip_interface& device, uint8_t instance); -CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance); +TypedResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters); +TypedResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut); +TypedResult saveEventAction(C::mip_interface& device, uint8_t instance); +TypedResult loadEventAction(C::mip_interface& device, uint8_t instance); +TypedResult defaultEventAction(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2783,17 +2839,18 @@ struct AccelBias FunctionSelector function = static_cast(0); Vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2814,11 +2871,12 @@ struct AccelBias struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f bias; ///< accelerometer bias in the sensor frame (x,y,z) [g] @@ -2834,11 +2892,11 @@ void extract(Serializer& serializer, AccelBias& self); void insert(Serializer& serializer, const AccelBias::Response& self); void extract(Serializer& serializer, AccelBias::Response& self); -CmdResult writeAccelBias(C::mip_interface& device, const float* bias); -CmdResult readAccelBias(C::mip_interface& device, float* biasOut); -CmdResult saveAccelBias(C::mip_interface& device); -CmdResult loadAccelBias(C::mip_interface& device); -CmdResult defaultAccelBias(C::mip_interface& device); +TypedResult writeAccelBias(C::mip_interface& device, const float* bias); +TypedResult readAccelBias(C::mip_interface& device, float* biasOut); +TypedResult saveAccelBias(C::mip_interface& device); +TypedResult loadAccelBias(C::mip_interface& device); +TypedResult defaultAccelBias(C::mip_interface& device); ///@} /// @@ -2855,17 +2913,18 @@ struct GyroBias FunctionSelector function = static_cast(0); Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2886,11 +2945,12 @@ struct GyroBias struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] @@ -2906,11 +2966,11 @@ void extract(Serializer& serializer, GyroBias& self); void insert(Serializer& serializer, const GyroBias::Response& self); void extract(Serializer& serializer, GyroBias::Response& self); -CmdResult writeGyroBias(C::mip_interface& device, const float* bias); -CmdResult readGyroBias(C::mip_interface& device, float* biasOut); -CmdResult saveGyroBias(C::mip_interface& device); -CmdResult loadGyroBias(C::mip_interface& device); -CmdResult defaultGyroBias(C::mip_interface& device); +TypedResult writeGyroBias(C::mip_interface& device, const float* bias); +TypedResult readGyroBias(C::mip_interface& device, float* biasOut); +TypedResult saveGyroBias(C::mip_interface& device); +TypedResult loadGyroBias(C::mip_interface& device); +TypedResult defaultGyroBias(C::mip_interface& device); ///@} /// @@ -2929,12 +2989,13 @@ struct CaptureGyroBias { uint16_t averaging_time_ms = 0; ///< Averaging time [milliseconds] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2947,11 +3008,12 @@ struct CaptureGyroBias } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f bias; ///< gyro bias in the sensor frame (x,y,z) [radians/second] @@ -2967,7 +3029,7 @@ void extract(Serializer& serializer, CaptureGyroBias& self); void insert(Serializer& serializer, const CaptureGyroBias::Response& self); void extract(Serializer& serializer, CaptureGyroBias::Response& self); -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); +TypedResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); ///@} /// @@ -2988,17 +3050,18 @@ struct MagHardIronOffset FunctionSelector function = static_cast(0); Vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3019,11 +3082,12 @@ struct MagHardIronOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f offset; ///< hard iron offset in the sensor frame (x,y,z) [Gauss] @@ -3039,11 +3103,11 @@ void extract(Serializer& serializer, MagHardIronOffset& self); void insert(Serializer& serializer, const MagHardIronOffset::Response& self); void extract(Serializer& serializer, MagHardIronOffset::Response& self); -CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); -CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); -CmdResult saveMagHardIronOffset(C::mip_interface& device); -CmdResult loadMagHardIronOffset(C::mip_interface& device); -CmdResult defaultMagHardIronOffset(C::mip_interface& device); +TypedResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); +TypedResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); +TypedResult saveMagHardIronOffset(C::mip_interface& device); +TypedResult loadMagHardIronOffset(C::mip_interface& device); +TypedResult defaultMagHardIronOffset(C::mip_interface& device); ///@} /// @@ -3068,17 +3132,18 @@ struct MagSoftIronMatrix FunctionSelector function = static_cast(0); Matrix3f offset; ///< soft iron matrix [dimensionless] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3099,11 +3164,12 @@ struct MagSoftIronMatrix struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f offset; ///< soft iron matrix [dimensionless] @@ -3119,11 +3185,11 @@ void extract(Serializer& serializer, MagSoftIronMatrix& self); void insert(Serializer& serializer, const MagSoftIronMatrix::Response& self); void extract(Serializer& serializer, MagSoftIronMatrix::Response& self); -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); -CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); -CmdResult saveMagSoftIronMatrix(C::mip_interface& device); -CmdResult loadMagSoftIronMatrix(C::mip_interface& device); -CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); +TypedResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); +TypedResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); +TypedResult saveMagSoftIronMatrix(C::mip_interface& device); +TypedResult loadMagSoftIronMatrix(C::mip_interface& device); +TypedResult defaultMagSoftIronMatrix(C::mip_interface& device); ///@} /// @@ -3138,17 +3204,18 @@ struct ConingScullingEnable FunctionSelector function = static_cast(0); bool enable = 0; ///< If true, coning and sculling compensation is enabled. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3169,11 +3236,12 @@ struct ConingScullingEnable struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< If true, coning and sculling compensation is enabled. @@ -3189,11 +3257,11 @@ void extract(Serializer& serializer, ConingScullingEnable& self); void insert(Serializer& serializer, const ConingScullingEnable::Response& self); void extract(Serializer& serializer, ConingScullingEnable::Response& self); -CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable); -CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); -CmdResult saveConingScullingEnable(C::mip_interface& device); -CmdResult loadConingScullingEnable(C::mip_interface& device); -CmdResult defaultConingScullingEnable(C::mip_interface& device); +TypedResult writeConingScullingEnable(C::mip_interface& device, bool enable); +TypedResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); +TypedResult saveConingScullingEnable(C::mip_interface& device); +TypedResult loadConingScullingEnable(C::mip_interface& device); +TypedResult defaultConingScullingEnable(C::mip_interface& device); ///@} /// @@ -3234,17 +3302,18 @@ struct Sensor2VehicleTransformEuler float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3265,11 +3334,12 @@ struct Sensor2VehicleTransformEuler struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] @@ -3287,11 +3357,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformEuler& self); void insert(Serializer& serializer, const Sensor2VehicleTransformEuler::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformEuler::Response& self); -CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw); -CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); -CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device); -CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device); -CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); +TypedResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw); +TypedResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); +TypedResult saveSensor2VehicleTransformEuler(C::mip_interface& device); +TypedResult loadSensor2VehicleTransformEuler(C::mip_interface& device); +TypedResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); ///@} /// @@ -3338,17 +3408,18 @@ struct Sensor2VehicleTransformQuaternion FunctionSelector function = static_cast(0); Quatf q; ///< Unit length quaternion representing transform [w, i, j, k] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3369,11 +3440,12 @@ struct Sensor2VehicleTransformQuaternion struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Quatf q; ///< Unit length quaternion representing transform [w, i, j, k] @@ -3389,11 +3461,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion& self); void insert(Serializer& serializer, const Sensor2VehicleTransformQuaternion::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response& self); -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); -CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); -CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); -CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); +TypedResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); +TypedResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); +TypedResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); +TypedResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); +TypedResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); ///@} /// @@ -3438,17 +3510,18 @@ struct Sensor2VehicleTransformDcm FunctionSelector function = static_cast(0); Matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3469,11 +3542,12 @@ struct Sensor2VehicleTransformDcm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f dcm; ///< 3 x 3 direction cosine matrix, stored in row-major order @@ -3489,11 +3563,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm& self); void insert(Serializer& serializer, const Sensor2VehicleTransformDcm::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self); -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); -CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device); -CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device); -CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); +TypedResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); +TypedResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); +TypedResult saveSensor2VehicleTransformDcm(C::mip_interface& device); +TypedResult loadSensor2VehicleTransformDcm(C::mip_interface& device); +TypedResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); ///@} /// @@ -3515,17 +3589,18 @@ struct ComplementaryFilter float pitch_roll_time_constant = 0; ///< Time constant associated with the pitch/roll corrections [s] float heading_time_constant = 0; ///< Time constant associated with the heading corrections [s] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3546,11 +3621,12 @@ struct ComplementaryFilter struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool pitch_roll_enable = 0; ///< Enable Pitch/Roll corrections bool heading_enable = 0; ///< Enable Heading corrections (only available on devices with magnetometer) float pitch_roll_time_constant = 0; ///< Time constant associated with the pitch/roll corrections [s] @@ -3569,11 +3645,11 @@ void extract(Serializer& serializer, ComplementaryFilter& self); void insert(Serializer& serializer, const ComplementaryFilter::Response& self); void extract(Serializer& serializer, ComplementaryFilter::Response& self); -CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant); -CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut); -CmdResult saveComplementaryFilter(C::mip_interface& device); -CmdResult loadComplementaryFilter(C::mip_interface& device); -CmdResult defaultComplementaryFilter(C::mip_interface& device); +TypedResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant); +TypedResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut); +TypedResult saveComplementaryFilter(C::mip_interface& device); +TypedResult loadComplementaryFilter(C::mip_interface& device); +TypedResult defaultComplementaryFilter(C::mip_interface& device); ///@} /// @@ -3596,17 +3672,18 @@ struct SensorRange SensorRangeType sensor = static_cast(0); ///< Which type of sensor will get the new range value. uint8_t setting = 0; ///< Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine this value. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3628,11 +3705,12 @@ struct SensorRange struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; SensorRangeType sensor = static_cast(0); ///< Which type of sensor will get the new range value. uint8_t setting = 0; ///< Use the 3DM Get Calibrated Sensor Ranges (0x0C,0x53) command to determine this value. @@ -3649,11 +3727,11 @@ void extract(Serializer& serializer, SensorRange& self); void insert(Serializer& serializer, const SensorRange::Response& self); void extract(Serializer& serializer, SensorRange::Response& self); -CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting); -CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut); -CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor); -CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor); -CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); +TypedResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting); +TypedResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut); +TypedResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor); +TypedResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor); +TypedResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); ///@} /// @@ -3676,12 +3754,13 @@ struct CalibratedSensorRanges }; SensorRangeType sensor = static_cast(0); ///< The sensor to query. Cannot be ALL. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3694,11 +3773,12 @@ struct CalibratedSensorRanges } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000030; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; SensorRangeType sensor = static_cast(0); ///< The sensor type from the command. uint8_t num_ranges = 0; ///< Number of supported ranges. Entry ranges[50]; ///< List of possible range settings. @@ -3719,7 +3799,7 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Entry& self); void insert(Serializer& serializer, const CalibratedSensorRanges::Response& self); void extract(Serializer& serializer, CalibratedSensorRanges::Response& self); -CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); +TypedResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); ///@} /// @@ -3751,17 +3831,18 @@ struct LowpassFilter bool manual = 0; ///< If false, the frequency parameter is ignored and the filter will track to half of the configured message format frequency. float frequency = 0; ///< Cutoff frequency in Hz. This will return the actual frequency when read out in automatic mode. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8003; - static const uint32_t SAVE_PARAMS = 0x8003; - static const uint32_t LOAD_PARAMS = 0x8003; - static const uint32_t DEFAULT_PARAMS = 0x8003; - static const uint32_t ECHOED_PARAMS = 0x0003; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8003; + static constexpr const uint32_t SAVE_PARAMS = 0x8003; + static constexpr const uint32_t LOAD_PARAMS = 0x8003; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8003; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3784,11 +3865,12 @@ struct LowpassFilter struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0003; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t desc_set = 0; ///< Descriptor set of the quantity to be filtered. uint8_t field_desc = 0; ///< Field descriptor of the quantity to be filtered. bool enable = 0; ///< The filter will be enabled if this is true. @@ -3808,11 +3890,11 @@ void extract(Serializer& serializer, LowpassFilter& self); void insert(Serializer& serializer, const LowpassFilter::Response& self); void extract(Serializer& serializer, LowpassFilter::Response& self); -CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); -CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); -CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +TypedResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); +TypedResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); +TypedResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +TypedResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +TypedResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); ///@} /// diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index b4333e05b..1f9327b6d 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -184,16 +184,26 @@ void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const insert_u8(serializer, self->frame_id); - if( self->function == MIP_FUNCTION_WRITE ) + if( self->function == MIP_FUNCTION_WRITE || self->function == MIP_FUNCTION_READ ) { insert_mip_aiding_reference_frame_command_format(serializer, self->format); + } + if( self->function == MIP_FUNCTION_WRITE ) + { for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); - for(unsigned int i=0; i < 4; i++) - insert_float(serializer, self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + insert_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + insert_mip_quatf(serializer, &self->rotation.quaternion); + + } } } void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_aiding_reference_frame_command* self) @@ -202,16 +212,26 @@ void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_ extract_u8(serializer, &self->frame_id); - if( self->function == MIP_FUNCTION_WRITE ) + if( self->function == MIP_FUNCTION_WRITE || self->function == MIP_FUNCTION_READ ) { extract_mip_aiding_reference_frame_command_format(serializer, &self->format); + } + if( self->function == MIP_FUNCTION_WRITE ) + { for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); - for(unsigned int i=0; i < 4; i++) - extract_float(serializer, &self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + extract_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + extract_mip_quatf(serializer, &self->rotation.quaternion); + + } } } @@ -224,9 +244,16 @@ void insert_mip_aiding_reference_frame_response(mip_serializer* serializer, cons for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); - for(unsigned int i=0; i < 4; i++) - insert_float(serializer, self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + insert_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + insert_mip_quatf(serializer, &self->rotation.quaternion); + + } } void extract_mip_aiding_reference_frame_response(mip_serializer* serializer, mip_aiding_reference_frame_response* self) { @@ -237,9 +264,16 @@ void extract_mip_aiding_reference_frame_response(mip_serializer* serializer, mip for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); - for(unsigned int i=0; i < 4; i++) - extract_float(serializer, &self->rotation[i]); - + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + extract_mip_vector3f(serializer, &self->rotation.euler); + + } + if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + extract_mip_quatf(serializer, &self->rotation.quaternion); + + } } void insert_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, const mip_aiding_reference_frame_command_format self) @@ -253,7 +287,7 @@ void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* se *self = tmp; } -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation) +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const mip_aiding_reference_frame_command_rotation* rotation) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -269,15 +303,21 @@ mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, ui for(unsigned int i=0; i < 3; i++) insert_float(&serializer, translation[i]); - assert(rotation || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - insert_float(&serializer, rotation[i]); - + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + insert_mip_vector3f(&serializer, &rotation->euler); + + } + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + insert_mip_quatf(&serializer, &rotation->quaternion); + + } assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out) +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, mip_aiding_reference_frame_command_rotation* rotation_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -287,6 +327,8 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin insert_u8(&serializer, frame_id); + insert_mip_aiding_reference_frame_command_format(&serializer, format); + assert(mip_serializer_is_ok(&serializer)); uint8_t responseLength = sizeof(buffer); @@ -299,17 +341,22 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin extract_u8(&deserializer, &frame_id); - assert(format_out); - extract_mip_aiding_reference_frame_command_format(&deserializer, format_out); + extract_mip_aiding_reference_frame_command_format(&deserializer, &format); assert(translation_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract_float(&deserializer, &translation_out[i]); - assert(rotation_out || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - extract_float(&deserializer, &rotation_out[i]); - + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + { + extract_mip_vector3f(&deserializer, &rotation_out->euler); + + } + if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + { + extract_mip_quatf(&deserializer, &rotation_out->quaternion); + + } if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } @@ -618,6 +665,102 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_aiding_height_command(mip_serializer* serializer, const mip_aiding_height_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_float(serializer, self->height); + + insert_float(serializer, self->uncertainty); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_aiding_height_command(mip_serializer* serializer, mip_aiding_height_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_float(serializer, &self->height); + + extract_float(serializer, &self->uncertainty); + + extract_u16(serializer, &self->valid_flags); + +} + +mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float height, float uncertainty, uint16_t valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + insert_float(&serializer, height); + + insert_float(&serializer, uncertainty); + + insert_u16(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert_mip_aiding_pressure_command(mip_serializer* serializer, const mip_aiding_pressure_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + insert_float(serializer, self->pressure); + + insert_float(serializer, self->uncertainty); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_aiding_pressure_command(mip_serializer* serializer, mip_aiding_pressure_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + extract_float(serializer, &self->pressure); + + extract_float(serializer, &self->uncertainty); + + extract_u16(serializer, &self->valid_flags); + +} + +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float pressure, float uncertainty, uint16_t valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + insert_float(&serializer, pressure); + + insert_float(&serializer, uncertainty); + + insert_u16(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_aiding_ecef_vel_command* self) { insert_mip_time(serializer, &self->time); @@ -867,6 +1010,73 @@ mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_t return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_aiding_magnetic_field_command(mip_serializer* serializer, const mip_aiding_magnetic_field_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->uncertainty[i]); + + insert_mip_aiding_magnetic_field_command_valid_flags(serializer, self->valid_flags); + +} +void extract_mip_aiding_magnetic_field_command(mip_serializer* serializer, mip_aiding_magnetic_field_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->uncertainty[i]); + + extract_mip_aiding_magnetic_field_command_valid_flags(serializer, &self->valid_flags); + +} + +void insert_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_magnetic_field_command_valid_flags self) +{ + insert_u16(serializer, (uint16_t)(self)); +} +void extract_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, mip_aiding_magnetic_field_command_valid_flags* self) +{ + uint16_t tmp = 0; + extract_u16(serializer, &tmp); + *self = tmp; +} + +mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, sensor_id); + + assert(magnetic_field || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, magnetic_field[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert_float(&serializer, uncertainty[i]); + + insert_mip_aiding_magnetic_field_command_valid_flags(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_MAGNETIC_FIELD, buffer, (uint8_t)mip_serializer_length(&serializer)); +} #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 1c08df6e2..9981eb802 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -88,7 +88,7 @@ void extract(Serializer& serializer, SensorFrameMapping::Response& self) } -CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) +TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -102,7 +102,7 @@ CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) +TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -111,7 +111,7 @@ CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -128,7 +128,7 @@ CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, } return result; } -CmdResult saveSensorFrameMapping(C::mip_interface& device) +TypedResult saveSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -138,7 +138,7 @@ CmdResult saveSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorFrameMapping(C::mip_interface& device) +TypedResult loadSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -148,7 +148,7 @@ CmdResult loadSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorFrameMapping(C::mip_interface& device) +TypedResult defaultSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -164,16 +164,26 @@ void insert(Serializer& serializer, const ReferenceFrame& self) insert(serializer, self.frame_id); - if( self.function == FunctionSelector::WRITE ) + if( self.function == FunctionSelector::WRITE || self.function == FunctionSelector::READ ) { insert(serializer, self.format); + } + if( self.function == FunctionSelector::WRITE ) + { for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - insert(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + insert(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + insert(serializer, self.rotation.quaternion); + + } } } void extract(Serializer& serializer, ReferenceFrame& self) @@ -182,16 +192,26 @@ void extract(Serializer& serializer, ReferenceFrame& self) extract(serializer, self.frame_id); - if( self.function == FunctionSelector::WRITE ) + if( self.function == FunctionSelector::WRITE || self.function == FunctionSelector::READ ) { extract(serializer, self.format); + } + if( self.function == FunctionSelector::WRITE ) + { for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - extract(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + extract(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + extract(serializer, self.rotation.quaternion); + + } } } @@ -204,9 +224,16 @@ void insert(Serializer& serializer, const ReferenceFrame::Response& self) for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - insert(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + insert(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + insert(serializer, self.rotation.quaternion); + + } } void extract(Serializer& serializer, ReferenceFrame::Response& self) { @@ -217,12 +244,19 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); - for(unsigned int i=0; i < 4; i++) - extract(serializer, self.rotation[i]); - + if( self.format == ReferenceFrame::Format::EULER ) + { + extract(serializer, self.rotation.euler); + + } + if( self.format == ReferenceFrame::Format::QUATERNION ) + { + extract(serializer, self.rotation.quaternion); + + } } -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation) +TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -236,15 +270,21 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen for(unsigned int i=0; i < 3; i++) insert(serializer, translation[i]); - assert(rotation || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - insert(serializer, rotation[i]); - + if( format == ReferenceFrame::Format::EULER ) + { + insert(serializer, rotation.euler); + + } + if( format == ReferenceFrame::Format::QUATERNION ) + { + insert(serializer, rotation.quaternion); + + } assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut) +TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -252,10 +292,12 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc insert(serializer, FunctionSelector::READ); insert(serializer, frameId); + insert(serializer, format); + assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -263,23 +305,28 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc extract(deserializer, frameId); - assert(formatOut); - extract(deserializer, *formatOut); + extract(deserializer, format); assert(translationOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, translationOut[i]); - assert(rotationOut || (4 == 0)); - for(unsigned int i=0; i < 4; i++) - extract(deserializer, rotationOut[i]); - + if( format == ReferenceFrame::Format::EULER ) + { + extract(deserializer, rotationOut->euler); + + } + if( format == ReferenceFrame::Format::QUATERNION ) + { + extract(deserializer, rotationOut->quaternion); + + } if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -291,7 +338,7 @@ CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -303,7 +350,7 @@ CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -347,7 +394,7 @@ void extract(Serializer& serializer, AidingEchoControl::Response& self) } -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) +TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -359,7 +406,7 @@ CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) +TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -368,7 +415,7 @@ CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mod assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -382,7 +429,7 @@ CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mod } return result; } -CmdResult saveAidingEchoControl(C::mip_interface& device) +TypedResult saveAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -392,7 +439,7 @@ CmdResult saveAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAidingEchoControl(C::mip_interface& device) +TypedResult loadAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -402,7 +449,7 @@ CmdResult loadAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAidingEchoControl(C::mip_interface& device) +TypedResult defaultAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -443,7 +490,7 @@ void extract(Serializer& serializer, EcefPos& self) } -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -503,7 +550,7 @@ void extract(Serializer& serializer, LlhPos& self) } -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -528,6 +575,98 @@ CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const Height& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.height); + + insert(serializer, self.uncertainty); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, Height& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.height); + + extract(serializer, self.uncertainty); + + extract(serializer, self.valid_flags); + +} + +TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + insert(serializer, height); + + insert(serializer, uncertainty); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); +} +void insert(Serializer& serializer, const Pressure& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + insert(serializer, self.pressure); + + insert(serializer, self.uncertainty); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, Pressure& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + extract(serializer, self.pressure); + + extract(serializer, self.uncertainty); + + extract(serializer, self.valid_flags); + +} + +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + insert(serializer, pressure); + + insert(serializer, uncertainty); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} void insert(Serializer& serializer, const EcefVel& self) { insert(serializer, self.time); @@ -559,7 +698,7 @@ void extract(Serializer& serializer, EcefVel& self) } -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -613,7 +752,7 @@ void extract(Serializer& serializer, NedVel& self) } -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -667,7 +806,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -717,7 +856,7 @@ void extract(Serializer& serializer, TrueHeading& self) } -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -736,6 +875,60 @@ CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensor return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_TRUE, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const MagneticField& self) +{ + insert(serializer, self.time); + + insert(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.uncertainty[i]); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, MagneticField& self) +{ + extract(serializer, self.time); + + extract(serializer, self.sensor_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.magnetic_field[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.uncertainty[i]); + + extract(serializer, self.valid_flags); + +} + +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, sensorId); + + assert(magneticField || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, magneticField[i]); + + assert(uncertainty || (3 == 0)); + for(unsigned int i=0; i < 3; i++) + insert(serializer, uncertainty[i]); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_FIELD, buffer, (uint8_t)mip_serializer_length(&serializer)); +} } // namespace commands_aiding } // namespace mip diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index b8ac265d9..f8386599d 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -42,11 +42,13 @@ enum MIP_CMD_DESC_AIDING_POS_LLH = 0x22, MIP_CMD_DESC_AIDING_HEIGHT_ABS = 0x23, MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x24, + MIP_CMD_DESC_AIDING_PRESSURE = 0x25, MIP_CMD_DESC_AIDING_VEL_ECEF = 0x28, MIP_CMD_DESC_AIDING_VEL_NED = 0x29, MIP_CMD_DESC_AIDING_VEL_ODOM = 0x2A, MIP_CMD_DESC_AIDING_WHEELSPEED = 0x2B, MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x31, + MIP_CMD_DESC_AIDING_MAGNETIC_FIELD = 0x32, MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x38, MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x39, MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x3A, @@ -120,6 +122,28 @@ mip_cmd_result mip_aiding_default_sensor_frame_mapping(struct mip_interface* dev /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_reference_frame (0x13,0x01) Reference Frame [C] +/// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command +/// should mirror the frame ID used in the aiding command (if that aiding measurement is measured in this reference frame) +/// +/// This transform satisfies the following relationship: +/// +/// EQSTART p^{veh} = R p^{sensor_frame} + t EQEND
+/// +/// Where:
+/// EQSTART R EQEND is rotation matrix defined by the rotation component and EQSTART t EQEND is the translation vector

+/// EQSTART p^{sensor_frame} EQEND is a 3-element position vector expressed in the external sensor frame
+/// EQSTART p^{veh} EQEND is a 3-element position vector expressed in the vehicle frame
+/// +/// Rotation can be defined using Euler angles OR quaternions. If Format selector is set to Euler Angles, the fourth element +/// in the rotation vector is ignored and should be set to 0. +/// +/// Example: GNSS antenna lever arm +/// +/// Frame ID: 1 +/// Format: 1 (Euler) +/// Translation: [0,1,] (GNSS with a 1 meter Y offset in the vehicle frame) +/// Rotation: [0,0,0,0] (Rotational component is not relevant for GNSS measurements, set to zero) +/// /// ///@{ @@ -127,13 +151,20 @@ typedef uint8_t mip_aiding_reference_frame_command_format; static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER = 1; ///< Translation vector followed by euler angles (roll, pitch, yaw). static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION = 2; ///< Translation vector followed by quaternion (w, x, y, z). +union mip_aiding_reference_frame_command_rotation +{ + mip_vector3f euler; + mip_quatf quaternion; +}; +typedef union mip_aiding_reference_frame_command_rotation mip_aiding_reference_frame_command_rotation; + struct mip_aiding_reference_frame_command { mip_function_selector function; uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_reference_frame_command_format format; ///< Format of the transformation. mip_vector3f translation; ///< Translation X, Y, and Z. - mip_quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + mip_aiding_reference_frame_command_rotation rotation; ///< Rotation as specified by format. }; typedef struct mip_aiding_reference_frame_command mip_aiding_reference_frame_command; @@ -148,15 +179,15 @@ struct mip_aiding_reference_frame_response uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_reference_frame_command_format format; ///< Format of the transformation. mip_vector3f translation; ///< Translation X, Y, and Z. - mip_quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + mip_aiding_reference_frame_command_rotation rotation; ///< Rotation as specified by format. }; typedef struct mip_aiding_reference_frame_response mip_aiding_reference_frame_response; void insert_mip_aiding_reference_frame_response(struct mip_serializer* serializer, const mip_aiding_reference_frame_response* self); void extract_mip_aiding_reference_frame_response(struct mip_serializer* serializer, mip_aiding_reference_frame_response* self); -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const float* rotation); -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format* format_out, float* translation_out, float* rotation_out); +mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const mip_aiding_reference_frame_command_rotation* rotation); +mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, mip_aiding_reference_frame_command_rotation* rotation_out); mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id); @@ -165,6 +196,7 @@ mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [C] +/// Controls command response behavior to external aiding commands /// ///@{ @@ -270,6 +302,52 @@ void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* seria mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_height (0x13,0x23) Height [C] +/// Estimated value of height. +/// +///@{ + +struct mip_aiding_height_command +{ + mip_time time; + uint8_t sensor_id; + float height; ///< [m] + float uncertainty; ///< [m] + uint16_t valid_flags; + +}; +typedef struct mip_aiding_height_command mip_aiding_height_command; +void insert_mip_aiding_height_command(struct mip_serializer* serializer, const mip_aiding_height_command* self); +void extract_mip_aiding_height_command(struct mip_serializer* serializer, mip_aiding_height_command* self); + +mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float height, float uncertainty, uint16_t valid_flags); + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_pressure (0x13,0x25) Pressure [C] +/// Estimated value of air pressure. +/// +///@{ + +struct mip_aiding_pressure_command +{ + mip_time time; + uint8_t sensor_id; + float pressure; ///< [mbar] + float uncertainty; ///< [mbar] + uint16_t valid_flags; + +}; +typedef struct mip_aiding_pressure_command mip_aiding_pressure_command; +void insert_mip_aiding_pressure_command(struct mip_serializer* serializer, const mip_aiding_pressure_command* self); +void extract_mip_aiding_pressure_command(struct mip_serializer* serializer, mip_aiding_pressure_command* self); + +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float pressure, float uncertainty, uint16_t valid_flags); + ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -357,7 +435,7 @@ struct mip_aiding_vehicle_fixed_frame_velocity_command mip_time time; ///< Timestamp of the measurement. uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) mip_vector3f velocity; ///< [m/s] - mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; }; @@ -392,6 +470,39 @@ void extract_mip_aiding_true_heading_command(struct mip_serializer* serializer, mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_magnetic_field (0x13,0x32) Magnetic Field [C] +/// Estimate of magnetic field in the frame associated with the given sensor ID. +/// +///@{ + +typedef uint16_t mip_aiding_magnetic_field_command_valid_flags; +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_ALL = 0x0007; + +struct mip_aiding_magnetic_field_command +{ + mip_time time; ///< Timestamp of the measurement. + uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + mip_vector3f magnetic_field; ///< [G] + mip_vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) + mip_aiding_magnetic_field_command_valid_flags valid_flags; + +}; +typedef struct mip_aiding_magnetic_field_command mip_aiding_magnetic_field_command; +void insert_mip_aiding_magnetic_field_command(struct mip_serializer* serializer, const mip_aiding_magnetic_field_command* self); +void extract_mip_aiding_magnetic_field_command(struct mip_serializer* serializer, mip_aiding_magnetic_field_command* self); + +void insert_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_magnetic_field_command_valid_flags self); +void extract_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, mip_aiding_magnetic_field_command_valid_flags* self); + +mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags); + ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 4355c820d..ea8dc7345 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -41,11 +41,13 @@ enum CMD_POS_LLH = 0x22, CMD_HEIGHT_ABS = 0x23, CMD_HEIGHT_REL = 0x24, + CMD_PRESSURE = 0x25, CMD_VEL_ECEF = 0x28, CMD_VEL_NED = 0x29, CMD_VEL_ODOM = 0x2A, CMD_WHEELSPEED = 0x2B, CMD_HEADING_TRUE = 0x31, + CMD_MAGNETIC_FIELD = 0x32, CMD_DELTA_POSITION = 0x38, CMD_DELTA_ATTITUDE = 0x39, CMD_LOCAL_ANGULAR_RATE = 0x3A, @@ -92,17 +94,18 @@ struct SensorFrameMapping uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -123,11 +126,12 @@ struct SensorFrameMapping struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. @@ -144,16 +148,38 @@ void extract(Serializer& serializer, SensorFrameMapping& self); void insert(Serializer& serializer, const SensorFrameMapping::Response& self); void extract(Serializer& serializer, SensorFrameMapping::Response& self); -CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); -CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); -CmdResult saveSensorFrameMapping(C::mip_interface& device); -CmdResult loadSensorFrameMapping(C::mip_interface& device); -CmdResult defaultSensorFrameMapping(C::mip_interface& device); +TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); +TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); +TypedResult saveSensorFrameMapping(C::mip_interface& device); +TypedResult loadSensorFrameMapping(C::mip_interface& device); +TypedResult defaultSensorFrameMapping(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_reference_frame (0x13,0x01) Reference Frame [CPP] +/// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command +/// should mirror the frame ID used in the aiding command (if that aiding measurement is measured in this reference frame) +/// +/// This transform satisfies the following relationship: +/// +/// EQSTART p^{veh} = R p^{sensor_frame} + t EQEND
+/// +/// Where:
+/// EQSTART R EQEND is rotation matrix defined by the rotation component and EQSTART t EQEND is the translation vector

+/// EQSTART p^{sensor_frame} EQEND is a 3-element position vector expressed in the external sensor frame
+/// EQSTART p^{veh} EQEND is a 3-element position vector expressed in the vehicle frame
+/// +/// Rotation can be defined using Euler angles OR quaternions. If Format selector is set to Euler Angles, the fourth element +/// in the rotation vector is ignored and should be set to 0. +/// +/// Example: GNSS antenna lever arm +/// +/// Frame ID: 1 +/// Format: 1 (Euler) +/// Translation: [0,1,] (GNSS with a 1 meter Y offset in the vehicle frame) +/// Rotation: [0,0,0,0] (Rotational component is not relevant for GNSS measurements, set to zero) +/// /// ///@{ @@ -165,23 +191,31 @@ struct ReferenceFrame QUATERNION = 2, ///< Translation vector followed by quaternion (w, x, y, z). }; + union Rotation + { + Rotation() {} + Vector3f euler; + Quatf quaternion; + }; + FunctionSelector function = static_cast(0); uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. Vector3f translation; ///< Translation X, Y, and Z. - Quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + Rotation rotation; ///< Rotation as specified by format. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8003; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -203,15 +237,16 @@ struct ReferenceFrame struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0003; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. Vector3f translation; ///< Translation X, Y, and Z. - Quatf rotation; ///< Depends on the format parameter. Unused values are ignored. + Rotation rotation; ///< Rotation as specified by format. auto as_tuple() @@ -226,16 +261,17 @@ void extract(Serializer& serializer, ReferenceFrame& self); void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const float* rotation); -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format* formatOut, float* translationOut, float* rotationOut); -CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); -CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); -CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); +TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); +TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); ///@} /// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [CPP] +/// Controls command response behavior to external aiding commands /// ///@{ @@ -251,17 +287,18 @@ struct AidingEchoControl FunctionSelector function = static_cast(0); Mode mode = static_cast(0); ///< Controls data echoing. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -282,11 +319,12 @@ struct AidingEchoControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Mode mode = static_cast(0); ///< Controls data echoing. @@ -302,11 +340,11 @@ void extract(Serializer& serializer, AidingEchoControl& self); void insert(Serializer& serializer, const AidingEchoControl::Response& self); void extract(Serializer& serializer, AidingEchoControl::Response& self); -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); -CmdResult saveAidingEchoControl(C::mip_interface& device); -CmdResult loadAidingEchoControl(C::mip_interface& device); -CmdResult defaultAidingEchoControl(C::mip_interface& device); +TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); +TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); +TypedResult saveAidingEchoControl(C::mip_interface& device); +TypedResult loadAidingEchoControl(C::mip_interface& device); +TypedResult defaultAidingEchoControl(C::mip_interface& device); ///@} /// @@ -334,7 +372,7 @@ struct EcefPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -355,11 +393,12 @@ struct EcefPos Vector3f uncertainty; ///< ECEF position uncertainty [m]. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -375,7 +414,7 @@ struct EcefPos void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); ///@} /// @@ -404,7 +443,7 @@ struct LlhPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -427,11 +466,12 @@ struct LlhPos Vector3f uncertainty; ///< NED position uncertainty. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -447,7 +487,85 @@ struct LlhPos void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_height (0x13,0x23) Height [CPP] +/// Estimated value of height. +/// +///@{ + +struct Height +{ + Time time; + uint8_t sensor_id = 0; + float height = 0; ///< [m] + float uncertainty = 0; ///< [m] + uint16_t valid_flags = 0; + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEIGHT_ABS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,height,uncertainty,valid_flags); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); + } + typedef void Response; +}; +void insert(Serializer& serializer, const Height& self); +void extract(Serializer& serializer, Height& self); + +TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_pressure (0x13,0x25) Pressure [CPP] +/// Estimated value of air pressure. +/// +///@{ + +struct Pressure +{ + Time time; + uint8_t sensor_id = 0; + float pressure = 0; ///< [mbar] + float uncertainty = 0; ///< [mbar] + uint16_t valid_flags = 0; + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_PRESSURE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,pressure,uncertainty,valid_flags); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(pressure),std::ref(uncertainty),std::ref(valid_flags)); + } + typedef void Response; +}; +void insert(Serializer& serializer, const Pressure& self); +void extract(Serializer& serializer, Pressure& self); + +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); ///@} /// @@ -475,7 +593,7 @@ struct EcefVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -496,11 +614,12 @@ struct EcefVel Vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -516,7 +635,7 @@ struct EcefVel void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); ///@} /// @@ -544,7 +663,7 @@ struct NedVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -565,11 +684,12 @@ struct NedVel Vector3f uncertainty; ///< NED velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -585,7 +705,7 @@ struct NedVel void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); ///@} /// @@ -614,7 +734,7 @@ struct VehicleFixedFrameVelocity ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -632,14 +752,15 @@ struct VehicleFixedFrameVelocity Time time; ///< Timestamp of the measurement. uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) Vector3f velocity; ///< [m/s] - Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if velocity_uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) + Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -655,7 +776,7 @@ struct VehicleFixedFrameVelocity void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); ///@} /// @@ -672,11 +793,12 @@ struct TrueHeading float uncertainty = 0; uint16_t valid_flags = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -692,7 +814,77 @@ struct TrueHeading void insert(Serializer& serializer, const TrueHeading& self); void extract(Serializer& serializer, TrueHeading& self); -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_magnetic_field (0x13,0x32) Magnetic Field [CPP] +/// Estimate of magnetic field in the frame associated with the given sensor ID. +/// +///@{ + +struct MagneticField +{ + struct ValidFlags : Bitfield + { + enum _enumType : uint16_t + { + NONE = 0x0000, + X = 0x0001, ///< + Y = 0x0002, ///< + Z = 0x0004, ///< + ALL = 0x0007, + }; + uint16_t value = NONE; + + ValidFlags() : value(NONE) {} + ValidFlags(int val) : value((uint16_t)val) {} + operator uint16_t() const { return value; } + ValidFlags& operator=(uint16_t val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator|=(uint16_t val) { return *this = value | val; } + ValidFlags& operator&=(uint16_t val) { return *this = value & val; } + + bool x() const { return (value & X) > 0; } + void x(bool val) { if(val) value |= X; else value &= ~X; } + bool y() const { return (value & Y) > 0; } + void y(bool val) { if(val) value |= Y; else value &= ~Y; } + bool z() const { return (value & Z) > 0; } + void z(bool val) { if(val) value |= Z; else value &= ~Z; } + + bool allSet() const { return value == ALL; } + void setAll() { value |= ALL; } + }; + + Time time; ///< Timestamp of the measurement. + uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + Vector3f magnetic_field; ///< [G] + Vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) + ValidFlags valid_flags; + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_MAGNETIC_FIELD; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,sensor_id,magnetic_field,uncertainty,valid_flags); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(magnetic_field),std::ref(uncertainty),std::ref(valid_flags)); + } + typedef void Response; +}; +void insert(Serializer& serializer, const MagneticField& self); +void extract(Serializer& serializer, MagneticField& self); + +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); ///@} /// diff --git a/src/mip/definitions/commands_base.cpp b/src/mip/definitions/commands_base.cpp index 035d105f7..bdd9de55d 100644 --- a/src/mip/definitions/commands_base.cpp +++ b/src/mip/definitions/commands_base.cpp @@ -81,7 +81,7 @@ void extract(Serializer& serializer, Ping& self) (void)self; } -CmdResult ping(C::mip_interface& device) +TypedResult ping(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PING, NULL, 0); } @@ -96,7 +96,7 @@ void extract(Serializer& serializer, SetIdle& self) (void)self; } -CmdResult setIdle(C::mip_interface& device) +TypedResult setIdle(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SET_TO_IDLE, NULL, 0); } @@ -122,12 +122,12 @@ void extract(Serializer& serializer, GetDeviceInfo::Response& self) } -CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) +TypedResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_INFO, NULL, 0, REPLY_DEVICE_INFO, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_INFO, NULL, 0, REPLY_DEVICE_INFO, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -165,12 +165,12 @@ void extract(Serializer& serializer, GetDeviceDescriptors::Response& self) } -CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) +TypedResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_DESCRIPTORS, NULL, 0, REPLY_DEVICE_DESCRIPTORS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_DESCRIPTORS, NULL, 0, REPLY_DEVICE_DESCRIPTORS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -206,12 +206,12 @@ void extract(Serializer& serializer, BuiltInTest::Response& self) } -CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut) +TypedResult builtInTest(C::mip_interface& device, uint32_t* resultOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_BUILT_IN_TEST, NULL, 0, REPLY_BUILT_IN_TEST, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_BUILT_IN_TEST, NULL, 0, REPLY_BUILT_IN_TEST, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -236,7 +236,7 @@ void extract(Serializer& serializer, Resume& self) (void)self; } -CmdResult resume(C::mip_interface& device) +TypedResult resume(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RESUME, NULL, 0); } @@ -264,12 +264,12 @@ void extract(Serializer& serializer, GetExtendedDescriptors::Response& self) } -CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) +TypedResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_EXTENDED_DESCRIPTORS, NULL, 0, REPLY_GET_EXTENDED_DESCRIPTORS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_EXTENDED_DESCRIPTORS, NULL, 0, REPLY_GET_EXTENDED_DESCRIPTORS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -307,12 +307,12 @@ void extract(Serializer& serializer, ContinuousBit::Response& self) } -CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut) +TypedResult continuousBit(C::mip_interface& device, uint8_t* resultOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTINUOUS_BIT, NULL, 0, REPLY_CONTINUOUS_BIT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTINUOUS_BIT, NULL, 0, REPLY_CONTINUOUS_BIT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -367,7 +367,7 @@ void extract(Serializer& serializer, CommSpeed::Response& self) } -CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) +TypedResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -381,7 +381,7 @@ CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut) +TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -392,7 +392,7 @@ CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COMM_SPEED, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COMM_SPEED, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -408,7 +408,7 @@ CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOu } return result; } -CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port) +TypedResult saveCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -420,7 +420,7 @@ CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port) +TypedResult loadCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -432,7 +432,7 @@ CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port) +TypedResult defaultCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -469,7 +469,7 @@ void extract(Serializer& serializer, GpsTimeUpdate& self) } } -CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value) +TypedResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -494,7 +494,7 @@ void extract(Serializer& serializer, SoftReset& self) (void)self; } -CmdResult softReset(C::mip_interface& device) +TypedResult softReset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_RESET, NULL, 0); } diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index e75c8fd70..2b8574205 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -118,7 +118,7 @@ struct CommandedTestBitsGq7 : Bitfield CommandedTestBitsGq7(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } CommandedTestBitsGq7& operator=(uint32_t val) { value = val; return *this; } - CommandedTestBitsGq7& operator=(int val) { value = val; return *this; } + CommandedTestBitsGq7& operator=(int val) { value = uint32_t(val); return *this; } CommandedTestBitsGq7& operator|=(uint32_t val) { return *this = value | val; } CommandedTestBitsGq7& operator&=(uint32_t val) { return *this = value & val; } @@ -199,11 +199,12 @@ struct CommandedTestBitsGq7 : Bitfield struct Ping { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -219,7 +220,7 @@ struct Ping void insert(Serializer& serializer, const Ping& self); void extract(Serializer& serializer, Ping& self); -CmdResult ping(C::mip_interface& device); +TypedResult ping(C::mip_interface& device); ///@} /// @@ -236,11 +237,12 @@ CmdResult ping(C::mip_interface& device); struct SetIdle { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -256,7 +258,7 @@ struct SetIdle void insert(Serializer& serializer, const SetIdle& self); void extract(Serializer& serializer, SetIdle& self); -CmdResult setIdle(C::mip_interface& device); +TypedResult setIdle(C::mip_interface& device); ///@} /// @@ -269,12 +271,13 @@ CmdResult setIdle(C::mip_interface& device); struct GetDeviceInfo { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -287,11 +290,12 @@ struct GetDeviceInfo } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; BaseDeviceInfo device_info; @@ -307,7 +311,7 @@ void extract(Serializer& serializer, GetDeviceInfo& self); void insert(Serializer& serializer, const GetDeviceInfo::Response& self); void extract(Serializer& serializer, GetDeviceInfo::Response& self); -CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut); +TypedResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut); ///@} /// @@ -323,12 +327,13 @@ CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) struct GetDeviceDescriptors { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -341,11 +346,12 @@ struct GetDeviceDescriptors } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; uint16_t descriptors[253] = {0}; uint8_t descriptors_count = 0; @@ -362,7 +368,7 @@ void extract(Serializer& serializer, GetDeviceDescriptors& self); void insert(Serializer& serializer, const GetDeviceDescriptors::Response& self); void extract(Serializer& serializer, GetDeviceDescriptors::Response& self); -CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); +TypedResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); ///@} /// @@ -380,12 +386,13 @@ CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOu struct BuiltInTest { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -398,11 +405,12 @@ struct BuiltInTest } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint32_t result = 0; @@ -418,7 +426,7 @@ void extract(Serializer& serializer, BuiltInTest& self); void insert(Serializer& serializer, const BuiltInTest::Response& self); void extract(Serializer& serializer, BuiltInTest::Response& self); -CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut); +TypedResult builtInTest(C::mip_interface& device, uint32_t* resultOut); ///@} /// @@ -433,11 +441,12 @@ CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut); struct Resume { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -453,7 +462,7 @@ struct Resume void insert(Serializer& serializer, const Resume& self); void extract(Serializer& serializer, Resume& self); -CmdResult resume(C::mip_interface& device); +TypedResult resume(C::mip_interface& device); ///@} /// @@ -469,12 +478,13 @@ CmdResult resume(C::mip_interface& device); struct GetExtendedDescriptors { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -487,11 +497,12 @@ struct GetExtendedDescriptors } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; uint16_t descriptors[253] = {0}; uint8_t descriptors_count = 0; @@ -508,7 +519,7 @@ void extract(Serializer& serializer, GetExtendedDescriptors& self); void insert(Serializer& serializer, const GetExtendedDescriptors::Response& self); void extract(Serializer& serializer, GetExtendedDescriptors::Response& self); -CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); +TypedResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); ///@} /// @@ -523,12 +534,13 @@ CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptors struct ContinuousBit { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -541,11 +553,12 @@ struct ContinuousBit } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. @@ -561,7 +574,7 @@ void extract(Serializer& serializer, ContinuousBit& self); void insert(Serializer& serializer, const ContinuousBit::Response& self); void extract(Serializer& serializer, ContinuousBit::Response& self); -CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut); +TypedResult continuousBit(C::mip_interface& device, uint8_t* resultOut); ///@} /// @@ -586,22 +599,23 @@ CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut); struct CommSpeed { - static const uint32_t ALL_PORTS = 0; + static constexpr const uint32_t ALL_PORTS = 0; FunctionSelector function = static_cast(0); uint8_t port = 0; ///< Port ID number, starting with 1. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all ports. See the device user manual for details. uint32_t baud = 0; ///< Port baud rate. Must be a supported rate. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -623,11 +637,12 @@ struct CommSpeed struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t port = 0; ///< Port ID number, starting with 1. When function is SAVE, LOAD, or DEFAULT, this can be 0 to apply to all ports. See the device user manual for details. uint32_t baud = 0; ///< Port baud rate. Must be a supported rate. @@ -644,11 +659,11 @@ void extract(Serializer& serializer, CommSpeed& self); void insert(Serializer& serializer, const CommSpeed::Response& self); void extract(Serializer& serializer, CommSpeed::Response& self); -CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud); -CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut); -CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port); -CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port); -CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port); +TypedResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud); +TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut); +TypedResult saveCommSpeed(C::mip_interface& device, uint8_t port); +TypedResult loadCommSpeed(C::mip_interface& device, uint8_t port); +TypedResult defaultCommSpeed(C::mip_interface& device, uint8_t port); ///@} /// @@ -673,16 +688,17 @@ struct GpsTimeUpdate FieldId field_id = static_cast(0); ///< Determines how to interpret value. uint32_t value = 0; ///< Week number or time of week, depending on the field_id. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x0000; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x0000; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -706,7 +722,7 @@ struct GpsTimeUpdate void insert(Serializer& serializer, const GpsTimeUpdate& self); void extract(Serializer& serializer, GpsTimeUpdate& self); -CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value); +TypedResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value); ///@} /// @@ -721,11 +737,12 @@ CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fi struct SoftReset { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -741,7 +758,7 @@ struct SoftReset void insert(Serializer& serializer, const SoftReset& self); void extract(Serializer& serializer, SoftReset& self); -CmdResult softReset(C::mip_interface& device); +TypedResult softReset(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index c88cb0eb8..7792142bf 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -40,7 +40,7 @@ void extract(Serializer& serializer, Reset& self) (void)self; } -CmdResult reset(C::mip_interface& device) +TypedResult reset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RESET_FILTER, NULL, 0); } @@ -63,7 +63,7 @@ void extract(Serializer& serializer, SetInitialAttitude& self) } -CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading) +TypedResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -110,7 +110,7 @@ void extract(Serializer& serializer, EstimationControl::Response& self) } -CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable) +TypedResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -122,7 +122,7 @@ CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::En return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut) +TypedResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -131,7 +131,7 @@ CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::Ena assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ESTIMATION_CONTROL_FLAGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ESTIMATION_CONTROL_FLAGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -145,7 +145,7 @@ CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::Ena } return result; } -CmdResult saveEstimationControl(C::mip_interface& device) +TypedResult saveEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -155,7 +155,7 @@ CmdResult saveEstimationControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEstimationControl(C::mip_interface& device) +TypedResult loadEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -165,7 +165,7 @@ CmdResult loadEstimationControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEstimationControl(C::mip_interface& device) +TypedResult defaultEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -220,7 +220,7 @@ void extract(Serializer& serializer, ExternalGnssUpdate& self) } -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) +TypedResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -270,7 +270,7 @@ void extract(Serializer& serializer, ExternalHeadingUpdate& self) } -CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type) +TypedResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -312,7 +312,7 @@ void extract(Serializer& serializer, ExternalHeadingUpdateWithTime& self) } -CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type) +TypedResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -363,7 +363,7 @@ void extract(Serializer& serializer, TareOrientation::Response& self) } -CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes) +TypedResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -375,7 +375,7 @@ CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTar return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut) +TypedResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -384,7 +384,7 @@ CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTare assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_TARE_ORIENTATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_TARE_ORIENTATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -398,7 +398,7 @@ CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTare } return result; } -CmdResult saveTareOrientation(C::mip_interface& device) +TypedResult saveTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -408,7 +408,7 @@ CmdResult saveTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadTareOrientation(C::mip_interface& device) +TypedResult loadTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -418,7 +418,7 @@ CmdResult loadTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultTareOrientation(C::mip_interface& device) +TypedResult defaultTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -460,7 +460,7 @@ void extract(Serializer& serializer, VehicleDynamicsMode::Response& self) } -CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) +TypedResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -472,7 +472,7 @@ CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) +TypedResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -481,7 +481,7 @@ CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode: assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -495,7 +495,7 @@ CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode: } return result; } -CmdResult saveVehicleDynamicsMode(C::mip_interface& device) +TypedResult saveVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -505,7 +505,7 @@ CmdResult saveVehicleDynamicsMode(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadVehicleDynamicsMode(C::mip_interface& device) +TypedResult loadVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -515,7 +515,7 @@ CmdResult loadVehicleDynamicsMode(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultVehicleDynamicsMode(C::mip_interface& device) +TypedResult defaultVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -573,7 +573,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationEuler::Response& sel } -CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw) +TypedResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -589,7 +589,7 @@ CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) +TypedResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -598,7 +598,7 @@ CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* roll assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_EULER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_EULER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -618,7 +618,7 @@ CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* roll } return result; } -CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device) +TypedResult saveSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -628,7 +628,7 @@ CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device) +TypedResult loadSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -638,7 +638,7 @@ CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device) +TypedResult defaultSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -684,7 +684,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self) } -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) +TypedResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -698,7 +698,7 @@ CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) +TypedResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -707,7 +707,7 @@ CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_DCM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_DCM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -722,7 +722,7 @@ CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut } return result; } -CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device) +TypedResult saveSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -732,7 +732,7 @@ CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device) +TypedResult loadSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -742,7 +742,7 @@ CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device) +TypedResult defaultSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -788,7 +788,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response } -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) +TypedResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -802,7 +802,7 @@ CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) +TypedResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -811,7 +811,7 @@ CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -826,7 +826,7 @@ CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* } return result; } -CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) +TypedResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -836,7 +836,7 @@ CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) +TypedResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -846,7 +846,7 @@ CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device) +TypedResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -892,7 +892,7 @@ void extract(Serializer& serializer, SensorToVehicleOffset::Response& self) } -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) +TypedResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -906,7 +906,7 @@ CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offs return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) +TypedResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -915,7 +915,7 @@ CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_OFFSET, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -930,7 +930,7 @@ CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveSensorToVehicleOffset(C::mip_interface& device) +TypedResult saveSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -940,7 +940,7 @@ CmdResult saveSensorToVehicleOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleOffset(C::mip_interface& device) +TypedResult loadSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -950,7 +950,7 @@ CmdResult loadSensorToVehicleOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleOffset(C::mip_interface& device) +TypedResult defaultSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -996,7 +996,7 @@ void extract(Serializer& serializer, AntennaOffset::Response& self) } -CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) +TypedResult writeAntennaOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1010,7 +1010,7 @@ CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) +TypedResult readAntennaOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1019,7 +1019,7 @@ CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_OFFSET, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1034,7 +1034,7 @@ CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveAntennaOffset(C::mip_interface& device) +TypedResult saveAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1044,7 +1044,7 @@ CmdResult saveAntennaOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAntennaOffset(C::mip_interface& device) +TypedResult loadAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1054,7 +1054,7 @@ CmdResult loadAntennaOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAntennaOffset(C::mip_interface& device) +TypedResult defaultAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1096,7 +1096,7 @@ void extract(Serializer& serializer, GnssSource::Response& self) } -CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) +TypedResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1108,7 +1108,7 @@ CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut) +TypedResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1117,7 +1117,7 @@ CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SOURCE_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SOURCE_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1131,7 +1131,7 @@ CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut } return result; } -CmdResult saveGnssSource(C::mip_interface& device) +TypedResult saveGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1141,7 +1141,7 @@ CmdResult saveGnssSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssSource(C::mip_interface& device) +TypedResult loadGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1151,7 +1151,7 @@ CmdResult loadGnssSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssSource(C::mip_interface& device) +TypedResult defaultGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1193,7 +1193,7 @@ void extract(Serializer& serializer, HeadingSource::Response& self) } -CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source) +TypedResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1205,7 +1205,7 @@ CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source sou return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut) +TypedResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1214,7 +1214,7 @@ CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sou assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_UPDATE_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_UPDATE_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1228,7 +1228,7 @@ CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sou } return result; } -CmdResult saveHeadingSource(C::mip_interface& device) +TypedResult saveHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1238,7 +1238,7 @@ CmdResult saveHeadingSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadHeadingSource(C::mip_interface& device) +TypedResult loadHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1248,7 +1248,7 @@ CmdResult loadHeadingSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultHeadingSource(C::mip_interface& device) +TypedResult defaultHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1290,7 +1290,7 @@ void extract(Serializer& serializer, AutoInitControl::Response& self) } -CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) +TypedResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1302,7 +1302,7 @@ CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) +TypedResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1311,7 +1311,7 @@ CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AUTOINIT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AUTOINIT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1325,7 +1325,7 @@ CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) } return result; } -CmdResult saveAutoInitControl(C::mip_interface& device) +TypedResult saveAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1335,7 +1335,7 @@ CmdResult saveAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoInitControl(C::mip_interface& device) +TypedResult loadAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1345,7 +1345,7 @@ CmdResult loadAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoInitControl(C::mip_interface& device) +TypedResult defaultAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1391,7 +1391,7 @@ void extract(Serializer& serializer, AccelNoise::Response& self) } -CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) +TypedResult writeAccelNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1405,7 +1405,7 @@ CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) +TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1414,7 +1414,7 @@ CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1429,7 +1429,7 @@ CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveAccelNoise(C::mip_interface& device) +TypedResult saveAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1439,7 +1439,7 @@ CmdResult saveAccelNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelNoise(C::mip_interface& device) +TypedResult loadAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1449,7 +1449,7 @@ CmdResult loadAccelNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelNoise(C::mip_interface& device) +TypedResult defaultAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1495,7 +1495,7 @@ void extract(Serializer& serializer, GyroNoise::Response& self) } -CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) +TypedResult writeGyroNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1509,7 +1509,7 @@ CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) +TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1518,7 +1518,7 @@ CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1533,7 +1533,7 @@ CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveGyroNoise(C::mip_interface& device) +TypedResult saveGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1543,7 +1543,7 @@ CmdResult saveGyroNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGyroNoise(C::mip_interface& device) +TypedResult loadGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1553,7 +1553,7 @@ CmdResult loadGyroNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGyroNoise(C::mip_interface& device) +TypedResult defaultGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1611,7 +1611,7 @@ void extract(Serializer& serializer, AccelBiasModel::Response& self) } -CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) +TypedResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1629,7 +1629,7 @@ CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +TypedResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1638,7 +1638,7 @@ CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* no assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1657,7 +1657,7 @@ CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* no } return result; } -CmdResult saveAccelBiasModel(C::mip_interface& device) +TypedResult saveAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1667,7 +1667,7 @@ CmdResult saveAccelBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelBiasModel(C::mip_interface& device) +TypedResult loadAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1677,7 +1677,7 @@ CmdResult loadAccelBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelBiasModel(C::mip_interface& device) +TypedResult defaultAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1735,7 +1735,7 @@ void extract(Serializer& serializer, GyroBiasModel::Response& self) } -CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) +TypedResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1753,7 +1753,7 @@ CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +TypedResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1762,7 +1762,7 @@ CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noi assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1781,7 +1781,7 @@ CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noi } return result; } -CmdResult saveGyroBiasModel(C::mip_interface& device) +TypedResult saveGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1791,7 +1791,7 @@ CmdResult saveGyroBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGyroBiasModel(C::mip_interface& device) +TypedResult loadGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1801,7 +1801,7 @@ CmdResult loadGyroBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGyroBiasModel(C::mip_interface& device) +TypedResult defaultGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1843,7 +1843,7 @@ void extract(Serializer& serializer, AltitudeAiding::Response& self) } -CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) +TypedResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1855,7 +1855,7 @@ CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSe return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) +TypedResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1864,7 +1864,7 @@ CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSel assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1878,7 +1878,7 @@ CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSel } return result; } -CmdResult saveAltitudeAiding(C::mip_interface& device) +TypedResult saveAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1888,7 +1888,7 @@ CmdResult saveAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAltitudeAiding(C::mip_interface& device) +TypedResult loadAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1898,7 +1898,7 @@ CmdResult loadAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAltitudeAiding(C::mip_interface& device) +TypedResult defaultAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1940,7 +1940,7 @@ void extract(Serializer& serializer, PitchRollAiding::Response& self) } -CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) +TypedResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1952,7 +1952,7 @@ CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::Aiding return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) +TypedResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1961,7 +1961,7 @@ CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingS assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1975,7 +1975,7 @@ CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingS } return result; } -CmdResult savePitchRollAiding(C::mip_interface& device) +TypedResult savePitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1985,7 +1985,7 @@ CmdResult savePitchRollAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadPitchRollAiding(C::mip_interface& device) +TypedResult loadPitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1995,7 +1995,7 @@ CmdResult loadPitchRollAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultPitchRollAiding(C::mip_interface& device) +TypedResult defaultPitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2045,7 +2045,7 @@ void extract(Serializer& serializer, AutoZupt::Response& self) } -CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) +TypedResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2059,7 +2059,7 @@ CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshol return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2068,7 +2068,7 @@ CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thre assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2085,7 +2085,7 @@ CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thre } return result; } -CmdResult saveAutoZupt(C::mip_interface& device) +TypedResult saveAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2095,7 +2095,7 @@ CmdResult saveAutoZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoZupt(C::mip_interface& device) +TypedResult loadAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2105,7 +2105,7 @@ CmdResult loadAutoZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoZupt(C::mip_interface& device) +TypedResult defaultAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2155,7 +2155,7 @@ void extract(Serializer& serializer, AutoAngularZupt::Response& self) } -CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) +TypedResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2169,7 +2169,7 @@ CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +TypedResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2178,7 +2178,7 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2195,7 +2195,7 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa } return result; } -CmdResult saveAutoAngularZupt(C::mip_interface& device) +TypedResult saveAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2205,7 +2205,7 @@ CmdResult saveAutoAngularZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoAngularZupt(C::mip_interface& device) +TypedResult loadAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2215,7 +2215,7 @@ CmdResult loadAutoAngularZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoAngularZupt(C::mip_interface& device) +TypedResult defaultAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2236,7 +2236,7 @@ void extract(Serializer& serializer, CommandedZupt& self) (void)self; } -CmdResult commandedZupt(C::mip_interface& device) +TypedResult commandedZupt(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); } @@ -2251,7 +2251,7 @@ void extract(Serializer& serializer, CommandedAngularZupt& self) (void)self; } -CmdResult commandedAngularZupt(C::mip_interface& device) +TypedResult commandedAngularZupt(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); } @@ -2266,7 +2266,7 @@ void extract(Serializer& serializer, MagCaptureAutoCal& self) } -CmdResult writeMagCaptureAutoCal(C::mip_interface& device) +TypedResult writeMagCaptureAutoCal(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2276,7 +2276,7 @@ CmdResult writeMagCaptureAutoCal(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult saveMagCaptureAutoCal(C::mip_interface& device) +TypedResult saveMagCaptureAutoCal(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2322,7 +2322,7 @@ void extract(Serializer& serializer, GravityNoise::Response& self) } -CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) +TypedResult writeGravityNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2336,7 +2336,7 @@ CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) +TypedResult readGravityNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2345,7 +2345,7 @@ CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2360,7 +2360,7 @@ CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveGravityNoise(C::mip_interface& device) +TypedResult saveGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2370,7 +2370,7 @@ CmdResult saveGravityNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGravityNoise(C::mip_interface& device) +TypedResult loadGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2380,7 +2380,7 @@ CmdResult loadGravityNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGravityNoise(C::mip_interface& device) +TypedResult defaultGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2422,7 +2422,7 @@ void extract(Serializer& serializer, PressureAltitudeNoise::Response& self) } -CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) +TypedResult writePressureAltitudeNoise(C::mip_interface& device, float noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2434,7 +2434,7 @@ CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) +TypedResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2443,7 +2443,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2457,7 +2457,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult savePressureAltitudeNoise(C::mip_interface& device) +TypedResult savePressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2467,7 +2467,7 @@ CmdResult savePressureAltitudeNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadPressureAltitudeNoise(C::mip_interface& device) +TypedResult loadPressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2477,7 +2477,7 @@ CmdResult loadPressureAltitudeNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultPressureAltitudeNoise(C::mip_interface& device) +TypedResult defaultPressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2523,7 +2523,7 @@ void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) } -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) +TypedResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2537,7 +2537,7 @@ CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) +TypedResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2546,7 +2546,7 @@ CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2561,7 +2561,7 @@ CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveHardIronOffsetNoise(C::mip_interface& device) +TypedResult saveHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2571,7 +2571,7 @@ CmdResult saveHardIronOffsetNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadHardIronOffsetNoise(C::mip_interface& device) +TypedResult loadHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2581,7 +2581,7 @@ CmdResult loadHardIronOffsetNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) +TypedResult defaultHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2627,7 +2627,7 @@ void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self) } -CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) +TypedResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2641,7 +2641,7 @@ CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) +TypedResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2650,7 +2650,7 @@ CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2665,7 +2665,7 @@ CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) +TypedResult saveSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2675,7 +2675,7 @@ CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) +TypedResult loadSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2685,7 +2685,7 @@ CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device) +TypedResult defaultSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2731,7 +2731,7 @@ void extract(Serializer& serializer, MagNoise::Response& self) } -CmdResult writeMagNoise(C::mip_interface& device, const float* noise) +TypedResult writeMagNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2745,7 +2745,7 @@ CmdResult writeMagNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) +TypedResult readMagNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2754,7 +2754,7 @@ CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2769,7 +2769,7 @@ CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveMagNoise(C::mip_interface& device) +TypedResult saveMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2779,7 +2779,7 @@ CmdResult saveMagNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagNoise(C::mip_interface& device) +TypedResult loadMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2789,7 +2789,7 @@ CmdResult loadMagNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagNoise(C::mip_interface& device) +TypedResult defaultMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2839,7 +2839,7 @@ void extract(Serializer& serializer, InclinationSource::Response& self) } -CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) +TypedResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2853,7 +2853,7 @@ CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) +TypedResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2862,7 +2862,7 @@ CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2879,7 +2879,7 @@ CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* } return result; } -CmdResult saveInclinationSource(C::mip_interface& device) +TypedResult saveInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2889,7 +2889,7 @@ CmdResult saveInclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadInclinationSource(C::mip_interface& device) +TypedResult loadInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2899,7 +2899,7 @@ CmdResult loadInclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultInclinationSource(C::mip_interface& device) +TypedResult defaultInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2949,7 +2949,7 @@ void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) } -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) +TypedResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2963,7 +2963,7 @@ CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagPara return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) +TypedResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2972,7 +2972,7 @@ CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParam assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2989,7 +2989,7 @@ CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParam } return result; } -CmdResult saveMagneticDeclinationSource(C::mip_interface& device) +TypedResult saveMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2999,7 +2999,7 @@ CmdResult saveMagneticDeclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagneticDeclinationSource(C::mip_interface& device) +TypedResult loadMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3009,7 +3009,7 @@ CmdResult loadMagneticDeclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) +TypedResult defaultMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3059,7 +3059,7 @@ void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self) } -CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) +TypedResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3073,7 +3073,7 @@ CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamS return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) +TypedResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3082,7 +3082,7 @@ CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSo assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3099,7 +3099,7 @@ CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSo } return result; } -CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) +TypedResult saveMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3109,7 +3109,7 @@ CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) +TypedResult loadMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3119,7 +3119,7 @@ CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device) +TypedResult defaultMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3185,7 +3185,7 @@ void extract(Serializer& serializer, ReferencePosition::Response& self) } -CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) +TypedResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3203,7 +3203,7 @@ CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double l return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) +TypedResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3212,7 +3212,7 @@ CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, doubl assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3235,7 +3235,7 @@ CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, doubl } return result; } -CmdResult saveReferencePosition(C::mip_interface& device) +TypedResult saveReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3245,7 +3245,7 @@ CmdResult saveReferencePosition(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadReferencePosition(C::mip_interface& device) +TypedResult loadReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3255,7 +3255,7 @@ CmdResult loadReferencePosition(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultReferencePosition(C::mip_interface& device) +TypedResult defaultReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3345,7 +3345,7 @@ void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Res } -CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) +TypedResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3369,7 +3369,7 @@ CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +TypedResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3378,7 +3378,7 @@ CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, F assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3410,7 +3410,7 @@ CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, F } return result; } -CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3420,7 +3420,7 @@ CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3430,7 +3430,7 @@ CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3520,7 +3520,7 @@ void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Respo } -CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) +TypedResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3544,7 +3544,7 @@ CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, Fi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +TypedResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3553,7 +3553,7 @@ CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, Fil assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3585,7 +3585,7 @@ CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, Fil } return result; } -CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3595,7 +3595,7 @@ CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3605,7 +3605,7 @@ CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3679,7 +3679,7 @@ void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Respon } -CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) +TypedResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3699,7 +3699,7 @@ CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, boo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +TypedResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3708,7 +3708,7 @@ CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3734,7 +3734,7 @@ CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool } return result; } -CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3744,7 +3744,7 @@ CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3754,7 +3754,7 @@ CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3804,7 +3804,7 @@ void extract(Serializer& serializer, AidingMeasurementEnable::Response& self) } -CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable) +TypedResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3818,7 +3818,7 @@ CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasureme return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut) +TypedResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3829,7 +3829,7 @@ CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AIDING_MEASUREMENT_ENABLE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AIDING_MEASUREMENT_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3845,7 +3845,7 @@ CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen } return result; } -CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +TypedResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3857,7 +3857,7 @@ CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +TypedResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3869,7 +3869,7 @@ CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +TypedResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3892,7 +3892,7 @@ void extract(Serializer& serializer, Run& self) (void)self; } -CmdResult run(C::mip_interface& device) +TypedResult run(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RUN, NULL, 0); } @@ -3944,7 +3944,7 @@ void extract(Serializer& serializer, KinematicConstraint::Response& self) } -CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection) +TypedResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3960,7 +3960,7 @@ CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t acceleratio return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut) +TypedResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3969,7 +3969,7 @@ CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* acceleratio assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_KINEMATIC_CONSTRAINT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_KINEMATIC_CONSTRAINT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3989,7 +3989,7 @@ CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* acceleratio } return result; } -CmdResult saveKinematicConstraint(C::mip_interface& device) +TypedResult saveKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3999,7 +3999,7 @@ CmdResult saveKinematicConstraint(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadKinematicConstraint(C::mip_interface& device) +TypedResult loadKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4009,7 +4009,7 @@ CmdResult loadKinematicConstraint(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultKinematicConstraint(C::mip_interface& device) +TypedResult defaultKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4123,7 +4123,7 @@ void extract(Serializer& serializer, InitializationConfiguration::Response& self } -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) +TypedResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4155,7 +4155,7 @@ CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t wai return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) +TypedResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4164,7 +4164,7 @@ CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* wai assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INITIALIZATION_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INITIALIZATION_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4204,7 +4204,7 @@ CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* wai } return result; } -CmdResult saveInitializationConfiguration(C::mip_interface& device) +TypedResult saveInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4214,7 +4214,7 @@ CmdResult saveInitializationConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadInitializationConfiguration(C::mip_interface& device) +TypedResult loadInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4224,7 +4224,7 @@ CmdResult loadInitializationConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultInitializationConfiguration(C::mip_interface& device) +TypedResult defaultInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4274,7 +4274,7 @@ void extract(Serializer& serializer, AdaptiveFilterOptions::Response& self) } -CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit) +TypedResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4288,7 +4288,7 @@ CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut) +TypedResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4297,7 +4297,7 @@ CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADAPTIVE_FILTER_OPTIONS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADAPTIVE_FILTER_OPTIONS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4314,7 +4314,7 @@ CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, } return result; } -CmdResult saveAdaptiveFilterOptions(C::mip_interface& device) +TypedResult saveAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4324,7 +4324,7 @@ CmdResult saveAdaptiveFilterOptions(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAdaptiveFilterOptions(C::mip_interface& device) +TypedResult loadAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4334,7 +4334,7 @@ CmdResult loadAdaptiveFilterOptions(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device) +TypedResult defaultAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4388,7 +4388,7 @@ void extract(Serializer& serializer, MultiAntennaOffset::Response& self) } -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) +TypedResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4404,7 +4404,7 @@ CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) +TypedResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4415,7 +4415,7 @@ CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, f assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MULTI_ANTENNA_OFFSET, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MULTI_ANTENNA_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4432,7 +4432,7 @@ CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, f } return result; } -CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +TypedResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4444,7 +4444,7 @@ CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +TypedResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4456,7 +4456,7 @@ CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +TypedResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4520,7 +4520,7 @@ void extract(Serializer& serializer, RelPosConfiguration::Response& self) } -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) +TypedResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4538,7 +4538,7 @@ CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, Fil return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) +TypedResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4547,7 +4547,7 @@ CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REL_POS_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REL_POS_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4568,7 +4568,7 @@ CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, } return result; } -CmdResult saveRelPosConfiguration(C::mip_interface& device) +TypedResult saveRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4578,7 +4578,7 @@ CmdResult saveRelPosConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadRelPosConfiguration(C::mip_interface& device) +TypedResult loadRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4588,7 +4588,7 @@ CmdResult loadRelPosConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultRelPosConfiguration(C::mip_interface& device) +TypedResult defaultRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4642,7 +4642,7 @@ void extract(Serializer& serializer, RefPointLeverArm::Response& self) } -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) +TypedResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4658,7 +4658,7 @@ CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refe return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) +TypedResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4667,7 +4667,7 @@ CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refer assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REF_POINT_LEVER_ARM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REF_POINT_LEVER_ARM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4685,7 +4685,7 @@ CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refer } return result; } -CmdResult saveRefPointLeverArm(C::mip_interface& device) +TypedResult saveRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4695,7 +4695,7 @@ CmdResult saveRefPointLeverArm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadRefPointLeverArm(C::mip_interface& device) +TypedResult loadRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4705,7 +4705,7 @@ CmdResult loadRefPointLeverArm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultRefPointLeverArm(C::mip_interface& device) +TypedResult defaultRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4738,7 +4738,7 @@ void extract(Serializer& serializer, SpeedMeasurement& self) } -CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty) +TypedResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4799,7 +4799,7 @@ void extract(Serializer& serializer, SpeedLeverArm::Response& self) } -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) +TypedResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4815,7 +4815,7 @@ CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const flo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) +TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4826,7 +4826,7 @@ CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* lev assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPEED_LEVER_ARM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPEED_LEVER_ARM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4843,7 +4843,7 @@ CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* lev } return result; } -CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) +TypedResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4855,7 +4855,7 @@ CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) +TypedResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4867,7 +4867,7 @@ CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source) +TypedResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4911,7 +4911,7 @@ void extract(Serializer& serializer, WheeledVehicleConstraintControl::Response& } -CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable) +TypedResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4923,7 +4923,7 @@ CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut) +TypedResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4932,7 +4932,7 @@ CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_CONSTRAINT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_CONSTRAINT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4946,7 +4946,7 @@ CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* } return result; } -CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device) +TypedResult saveWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4956,7 +4956,7 @@ CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device) +TypedResult loadWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4966,7 +4966,7 @@ CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device) +TypedResult defaultWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5008,7 +5008,7 @@ void extract(Serializer& serializer, VerticalGyroConstraintControl::Response& se } -CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable) +TypedResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5020,7 +5020,7 @@ CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t e return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut) +TypedResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5029,7 +5029,7 @@ CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* e assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_CONSTRAINT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_CONSTRAINT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -5043,7 +5043,7 @@ CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* e } return result; } -CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device) +TypedResult saveVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5053,7 +5053,7 @@ CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device) +TypedResult loadVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5063,7 +5063,7 @@ CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device) +TypedResult defaultVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5113,7 +5113,7 @@ void extract(Serializer& serializer, GnssAntennaCalControl::Response& self) } -CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset) +TypedResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5127,7 +5127,7 @@ CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut) +TypedResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5136,7 +5136,7 @@ CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_CALIBRATION_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_CALIBRATION_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -5153,7 +5153,7 @@ CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut } return result; } -CmdResult saveGnssAntennaCalControl(C::mip_interface& device) +TypedResult saveGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5163,7 +5163,7 @@ CmdResult saveGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssAntennaCalControl(C::mip_interface& device) +TypedResult loadGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5173,7 +5173,7 @@ CmdResult loadGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssAntennaCalControl(C::mip_interface& device) +TypedResult defaultGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5194,7 +5194,7 @@ void extract(Serializer& serializer, SetInitialHeading& self) } -CmdResult setInitialHeading(C::mip_interface& device, float heading) +TypedResult setInitialHeading(C::mip_interface& device, float heading) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index bb8f91c9f..e8ca1ccb3 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -1757,13 +1757,15 @@ mip_cmd_result mip_filter_default_mag_dip_angle_error_adaptive_measurement(struc ///@{ typedef uint16_t mip_filter_aiding_measurement_enable_command_aiding_source; -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_POS_VEL = 0; ///< GNSS Position and Velocity -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING = 1; ///< GNSS Heading (dual antenna) -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALTIMETER = 2; ///< Altimeter -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_SPEED = 3; ///< Speed sensor / Odometer -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER = 4; ///< Magnetometer -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING = 5; ///< External heading input -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALL = 65535; ///< Save/load/reset all options +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_POS_VEL = 0; ///< GNSS Position and Velocity +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING = 1; ///< GNSS Heading (dual antenna) +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALTIMETER = 2; ///< Pressure altimeter (built-in sensor) +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_SPEED = 3; ///< Speed sensor / Odometer +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER = 4; ///< Magnetometer (built-in sensor) +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING = 5; ///< External heading input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_ALTIMETER = 6; ///< External pressure altimeter input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER = 7; ///< External magnetomer input input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALL = 65535; ///< Save/load/reset all options struct mip_filter_aiding_measurement_enable_command { diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 9e43c1a1c..e36706b43 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -192,11 +192,12 @@ enum class FilterAdaptiveMeasurement : uint8_t struct Reset { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -212,7 +213,7 @@ struct Reset void insert(Serializer& serializer, const Reset& self); void extract(Serializer& serializer, Reset& self); -CmdResult reset(C::mip_interface& device); +TypedResult reset(C::mip_interface& device); ///@} /// @@ -239,11 +240,12 @@ struct SetInitialAttitude float pitch = 0; ///< [radians] float heading = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -259,7 +261,7 @@ struct SetInitialAttitude void insert(Serializer& serializer, const SetInitialAttitude& self); void extract(Serializer& serializer, SetInitialAttitude& self); -CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading); +TypedResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading); ///@} /// @@ -301,7 +303,7 @@ struct EstimationControl EnableFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } EnableFlags& operator=(uint16_t val) { value = val; return *this; } - EnableFlags& operator=(int val) { value = val; return *this; } + EnableFlags& operator=(int val) { value = uint16_t(val); return *this; } EnableFlags& operator|=(uint16_t val) { return *this = value | val; } EnableFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -327,17 +329,18 @@ struct EstimationControl FunctionSelector function = static_cast(0); EnableFlags enable; ///< See above - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -358,11 +361,12 @@ struct EstimationControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; EnableFlags enable; ///< See above @@ -378,11 +382,11 @@ void extract(Serializer& serializer, EstimationControl& self); void insert(Serializer& serializer, const EstimationControl::Response& self); void extract(Serializer& serializer, EstimationControl::Response& self); -CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable); -CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut); -CmdResult saveEstimationControl(C::mip_interface& device); -CmdResult loadEstimationControl(C::mip_interface& device); -CmdResult defaultEstimationControl(C::mip_interface& device); +TypedResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable); +TypedResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut); +TypedResult saveEstimationControl(C::mip_interface& device); +TypedResult loadEstimationControl(C::mip_interface& device); +TypedResult defaultEstimationControl(C::mip_interface& device); ///@} /// @@ -407,11 +411,12 @@ struct ExternalGnssUpdate Vector3f pos_uncertainty; ///< NED Frame, 1-sigma [meters] Vector3f vel_uncertainty; ///< NED Frame, 1-sigma [meters/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -427,7 +432,7 @@ struct ExternalGnssUpdate void insert(Serializer& serializer, const ExternalGnssUpdate& self); void extract(Serializer& serializer, ExternalGnssUpdate& self); -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); +TypedResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); ///@} /// @@ -455,11 +460,12 @@ struct ExternalHeadingUpdate float heading_uncertainty = 0; ///< 1-sigma [radians] uint8_t type = 0; ///< 1 - True, 2 - Magnetic - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -475,7 +481,7 @@ struct ExternalHeadingUpdate void insert(Serializer& serializer, const ExternalHeadingUpdate& self); void extract(Serializer& serializer, ExternalHeadingUpdate& self); -CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type); +TypedResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type); ///@} /// @@ -509,11 +515,12 @@ struct ExternalHeadingUpdateWithTime float heading_uncertainty = 0; ///< 1-sigma [radians] uint8_t type = 0; ///< 1 - True, 2 - Magnetic - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -529,7 +536,7 @@ struct ExternalHeadingUpdateWithTime void insert(Serializer& serializer, const ExternalHeadingUpdateWithTime& self); void extract(Serializer& serializer, ExternalHeadingUpdateWithTime& self); -CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type); +TypedResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type); ///@} /// @@ -561,7 +568,7 @@ struct TareOrientation MipTareAxes(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } MipTareAxes& operator=(uint8_t val) { value = val; return *this; } - MipTareAxes& operator=(int val) { value = val; return *this; } + MipTareAxes& operator=(int val) { value = uint8_t(val); return *this; } MipTareAxes& operator|=(uint8_t val) { return *this = value | val; } MipTareAxes& operator&=(uint8_t val) { return *this = value & val; } @@ -579,17 +586,18 @@ struct TareOrientation FunctionSelector function = static_cast(0); MipTareAxes axes; ///< Axes to tare - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -610,11 +618,12 @@ struct TareOrientation struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; MipTareAxes axes; ///< Axes to tare @@ -630,11 +639,11 @@ void extract(Serializer& serializer, TareOrientation& self); void insert(Serializer& serializer, const TareOrientation::Response& self); void extract(Serializer& serializer, TareOrientation::Response& self); -CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes); -CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut); -CmdResult saveTareOrientation(C::mip_interface& device); -CmdResult loadTareOrientation(C::mip_interface& device); -CmdResult defaultTareOrientation(C::mip_interface& device); +TypedResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes); +TypedResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut); +TypedResult saveTareOrientation(C::mip_interface& device); +TypedResult loadTareOrientation(C::mip_interface& device); +TypedResult defaultTareOrientation(C::mip_interface& device); ///@} /// @@ -657,17 +666,18 @@ struct VehicleDynamicsMode FunctionSelector function = static_cast(0); DynamicsMode mode = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -688,11 +698,12 @@ struct VehicleDynamicsMode struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; DynamicsMode mode = static_cast(0); @@ -708,11 +719,11 @@ void extract(Serializer& serializer, VehicleDynamicsMode& self); void insert(Serializer& serializer, const VehicleDynamicsMode::Response& self); void extract(Serializer& serializer, VehicleDynamicsMode::Response& self); -CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); -CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); -CmdResult saveVehicleDynamicsMode(C::mip_interface& device); -CmdResult loadVehicleDynamicsMode(C::mip_interface& device); -CmdResult defaultVehicleDynamicsMode(C::mip_interface& device); +TypedResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); +TypedResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); +TypedResult saveVehicleDynamicsMode(C::mip_interface& device); +TypedResult loadVehicleDynamicsMode(C::mip_interface& device); +TypedResult defaultVehicleDynamicsMode(C::mip_interface& device); ///@} /// @@ -751,17 +762,18 @@ struct SensorToVehicleRotationEuler float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -782,11 +794,12 @@ struct SensorToVehicleRotationEuler struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; float roll = 0; ///< [radians] float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] @@ -804,11 +817,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationEuler& self); void insert(Serializer& serializer, const SensorToVehicleRotationEuler::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationEuler::Response& self); -CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw); -CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); -CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device); -CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device); -CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); +TypedResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw); +TypedResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); +TypedResult saveSensorToVehicleRotationEuler(C::mip_interface& device); +TypedResult loadSensorToVehicleRotationEuler(C::mip_interface& device); +TypedResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); ///@} /// @@ -851,17 +864,18 @@ struct SensorToVehicleRotationDcm FunctionSelector function = static_cast(0); Matrix3f dcm; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -882,11 +896,12 @@ struct SensorToVehicleRotationDcm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f dcm; @@ -902,11 +917,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm& self); void insert(Serializer& serializer, const SensorToVehicleRotationDcm::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self); -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); -CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device); -CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device); -CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); +TypedResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); +TypedResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); +TypedResult saveSensorToVehicleRotationDcm(C::mip_interface& device); +TypedResult loadSensorToVehicleRotationDcm(C::mip_interface& device); +TypedResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); ///@} /// @@ -948,17 +963,18 @@ struct SensorToVehicleRotationQuaternion FunctionSelector function = static_cast(0); Quatf quat; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -979,11 +995,12 @@ struct SensorToVehicleRotationQuaternion struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Quatf quat; @@ -999,11 +1016,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion& self); void insert(Serializer& serializer, const SensorToVehicleRotationQuaternion::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response& self); -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); -CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); -CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); -CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); +TypedResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); +TypedResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); +TypedResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); +TypedResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); +TypedResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); ///@} /// @@ -1026,17 +1043,18 @@ struct SensorToVehicleOffset FunctionSelector function = static_cast(0); Vector3f offset; ///< [meters] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1057,11 +1075,12 @@ struct SensorToVehicleOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f offset; ///< [meters] @@ -1077,11 +1096,11 @@ void extract(Serializer& serializer, SensorToVehicleOffset& self); void insert(Serializer& serializer, const SensorToVehicleOffset::Response& self); void extract(Serializer& serializer, SensorToVehicleOffset::Response& self); -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); -CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); -CmdResult saveSensorToVehicleOffset(C::mip_interface& device); -CmdResult loadSensorToVehicleOffset(C::mip_interface& device); -CmdResult defaultSensorToVehicleOffset(C::mip_interface& device); +TypedResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); +TypedResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); +TypedResult saveSensorToVehicleOffset(C::mip_interface& device); +TypedResult loadSensorToVehicleOffset(C::mip_interface& device); +TypedResult defaultSensorToVehicleOffset(C::mip_interface& device); ///@} /// @@ -1101,17 +1120,18 @@ struct AntennaOffset FunctionSelector function = static_cast(0); Vector3f offset; ///< [meters] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1132,11 +1152,12 @@ struct AntennaOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f offset; ///< [meters] @@ -1152,11 +1173,11 @@ void extract(Serializer& serializer, AntennaOffset& self); void insert(Serializer& serializer, const AntennaOffset::Response& self); void extract(Serializer& serializer, AntennaOffset::Response& self); -CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset); -CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut); -CmdResult saveAntennaOffset(C::mip_interface& device); -CmdResult loadAntennaOffset(C::mip_interface& device); -CmdResult defaultAntennaOffset(C::mip_interface& device); +TypedResult writeAntennaOffset(C::mip_interface& device, const float* offset); +TypedResult readAntennaOffset(C::mip_interface& device, float* offsetOut); +TypedResult saveAntennaOffset(C::mip_interface& device); +TypedResult loadAntennaOffset(C::mip_interface& device); +TypedResult defaultAntennaOffset(C::mip_interface& device); ///@} /// @@ -1183,17 +1204,18 @@ struct GnssSource FunctionSelector function = static_cast(0); Source source = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1214,11 +1236,12 @@ struct GnssSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); @@ -1234,11 +1257,11 @@ void extract(Serializer& serializer, GnssSource& self); void insert(Serializer& serializer, const GnssSource::Response& self); void extract(Serializer& serializer, GnssSource::Response& self); -CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source); -CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut); -CmdResult saveGnssSource(C::mip_interface& device); -CmdResult loadGnssSource(C::mip_interface& device); -CmdResult defaultGnssSource(C::mip_interface& device); +TypedResult writeGnssSource(C::mip_interface& device, GnssSource::Source source); +TypedResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut); +TypedResult saveGnssSource(C::mip_interface& device); +TypedResult loadGnssSource(C::mip_interface& device); +TypedResult defaultGnssSource(C::mip_interface& device); ///@} /// @@ -1276,17 +1299,18 @@ struct HeadingSource FunctionSelector function = static_cast(0); Source source = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1307,11 +1331,12 @@ struct HeadingSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Source source = static_cast(0); @@ -1327,11 +1352,11 @@ void extract(Serializer& serializer, HeadingSource& self); void insert(Serializer& serializer, const HeadingSource::Response& self); void extract(Serializer& serializer, HeadingSource::Response& self); -CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source); -CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut); -CmdResult saveHeadingSource(C::mip_interface& device); -CmdResult loadHeadingSource(C::mip_interface& device); -CmdResult defaultHeadingSource(C::mip_interface& device); +TypedResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source); +TypedResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut); +TypedResult saveHeadingSource(C::mip_interface& device); +TypedResult loadHeadingSource(C::mip_interface& device); +TypedResult defaultHeadingSource(C::mip_interface& device); ///@} /// @@ -1354,17 +1379,18 @@ struct AutoInitControl FunctionSelector function = static_cast(0); uint8_t enable = 0; ///< See above - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1385,11 +1411,12 @@ struct AutoInitControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< See above @@ -1405,11 +1432,11 @@ void extract(Serializer& serializer, AutoInitControl& self); void insert(Serializer& serializer, const AutoInitControl::Response& self); void extract(Serializer& serializer, AutoInitControl::Response& self); -CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable); -CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut); -CmdResult saveAutoInitControl(C::mip_interface& device); -CmdResult loadAutoInitControl(C::mip_interface& device); -CmdResult defaultAutoInitControl(C::mip_interface& device); +TypedResult writeAutoInitControl(C::mip_interface& device, uint8_t enable); +TypedResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut); +TypedResult saveAutoInitControl(C::mip_interface& device); +TypedResult loadAutoInitControl(C::mip_interface& device); +TypedResult defaultAutoInitControl(C::mip_interface& device); ///@} /// @@ -1430,17 +1457,18 @@ struct AccelNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1461,11 +1489,12 @@ struct AccelNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] @@ -1481,11 +1510,11 @@ void extract(Serializer& serializer, AccelNoise& self); void insert(Serializer& serializer, const AccelNoise::Response& self); void extract(Serializer& serializer, AccelNoise::Response& self); -CmdResult writeAccelNoise(C::mip_interface& device, const float* noise); -CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveAccelNoise(C::mip_interface& device); -CmdResult loadAccelNoise(C::mip_interface& device); -CmdResult defaultAccelNoise(C::mip_interface& device); +TypedResult writeAccelNoise(C::mip_interface& device, const float* noise); +TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveAccelNoise(C::mip_interface& device); +TypedResult loadAccelNoise(C::mip_interface& device); +TypedResult defaultAccelNoise(C::mip_interface& device); ///@} /// @@ -1506,17 +1535,18 @@ struct GyroNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1537,11 +1567,12 @@ struct GyroNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] @@ -1557,11 +1588,11 @@ void extract(Serializer& serializer, GyroNoise& self); void insert(Serializer& serializer, const GyroNoise::Response& self); void extract(Serializer& serializer, GyroNoise::Response& self); -CmdResult writeGyroNoise(C::mip_interface& device, const float* noise); -CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveGyroNoise(C::mip_interface& device); -CmdResult loadGyroNoise(C::mip_interface& device); -CmdResult defaultGyroNoise(C::mip_interface& device); +TypedResult writeGyroNoise(C::mip_interface& device, const float* noise); +TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveGyroNoise(C::mip_interface& device); +TypedResult loadGyroNoise(C::mip_interface& device); +TypedResult defaultGyroNoise(C::mip_interface& device); ///@} /// @@ -1580,17 +1611,18 @@ struct AccelBiasModel Vector3f beta; ///< Accel Bias Beta [1/second] Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1611,11 +1643,12 @@ struct AccelBiasModel struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f beta; ///< Accel Bias Beta [1/second] Vector3f noise; ///< Accel Noise 1-sigma [meters/second^2] @@ -1632,11 +1665,11 @@ void extract(Serializer& serializer, AccelBiasModel& self); void insert(Serializer& serializer, const AccelBiasModel::Response& self); void extract(Serializer& serializer, AccelBiasModel::Response& self); -CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); -CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); -CmdResult saveAccelBiasModel(C::mip_interface& device); -CmdResult loadAccelBiasModel(C::mip_interface& device); -CmdResult defaultAccelBiasModel(C::mip_interface& device); +TypedResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); +TypedResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); +TypedResult saveAccelBiasModel(C::mip_interface& device); +TypedResult loadAccelBiasModel(C::mip_interface& device); +TypedResult defaultAccelBiasModel(C::mip_interface& device); ///@} /// @@ -1655,17 +1688,18 @@ struct GyroBiasModel Vector3f beta; ///< Gyro Bias Beta [1/second] Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1686,11 +1720,12 @@ struct GyroBiasModel struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f beta; ///< Gyro Bias Beta [1/second] Vector3f noise; ///< Gyro Noise 1-sigma [rad/second] @@ -1707,11 +1742,11 @@ void extract(Serializer& serializer, GyroBiasModel& self); void insert(Serializer& serializer, const GyroBiasModel::Response& self); void extract(Serializer& serializer, GyroBiasModel::Response& self); -CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); -CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); -CmdResult saveGyroBiasModel(C::mip_interface& device); -CmdResult loadGyroBiasModel(C::mip_interface& device); -CmdResult defaultGyroBiasModel(C::mip_interface& device); +TypedResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); +TypedResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); +TypedResult saveGyroBiasModel(C::mip_interface& device); +TypedResult loadGyroBiasModel(C::mip_interface& device); +TypedResult defaultGyroBiasModel(C::mip_interface& device); ///@} /// @@ -1736,17 +1771,18 @@ struct AltitudeAiding FunctionSelector function = static_cast(0); AidingSelector selector = static_cast(0); ///< See above - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1767,11 +1803,12 @@ struct AltitudeAiding struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AidingSelector selector = static_cast(0); ///< See above @@ -1787,11 +1824,11 @@ void extract(Serializer& serializer, AltitudeAiding& self); void insert(Serializer& serializer, const AltitudeAiding::Response& self); void extract(Serializer& serializer, AltitudeAiding::Response& self); -CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); -CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); -CmdResult saveAltitudeAiding(C::mip_interface& device); -CmdResult loadAltitudeAiding(C::mip_interface& device); -CmdResult defaultAltitudeAiding(C::mip_interface& device); +TypedResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); +TypedResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); +TypedResult saveAltitudeAiding(C::mip_interface& device); +TypedResult loadAltitudeAiding(C::mip_interface& device); +TypedResult defaultAltitudeAiding(C::mip_interface& device); ///@} /// @@ -1813,17 +1850,18 @@ struct PitchRollAiding FunctionSelector function = static_cast(0); AidingSource source = static_cast(0); ///< Controls the aiding source - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1844,11 +1882,12 @@ struct PitchRollAiding struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AidingSource source = static_cast(0); ///< Controls the aiding source @@ -1864,11 +1903,11 @@ void extract(Serializer& serializer, PitchRollAiding& self); void insert(Serializer& serializer, const PitchRollAiding::Response& self); void extract(Serializer& serializer, PitchRollAiding::Response& self); -CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); -CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); -CmdResult savePitchRollAiding(C::mip_interface& device); -CmdResult loadPitchRollAiding(C::mip_interface& device); -CmdResult defaultPitchRollAiding(C::mip_interface& device); +TypedResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); +TypedResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); +TypedResult savePitchRollAiding(C::mip_interface& device); +TypedResult loadPitchRollAiding(C::mip_interface& device); +TypedResult defaultPitchRollAiding(C::mip_interface& device); ///@} /// @@ -1885,17 +1924,18 @@ struct AutoZupt uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [meters/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1916,11 +1956,12 @@ struct AutoZupt struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [meters/second] @@ -1937,11 +1978,11 @@ void extract(Serializer& serializer, AutoZupt& self); void insert(Serializer& serializer, const AutoZupt::Response& self); void extract(Serializer& serializer, AutoZupt::Response& self); -CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold); -CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); -CmdResult saveAutoZupt(C::mip_interface& device); -CmdResult loadAutoZupt(C::mip_interface& device); -CmdResult defaultAutoZupt(C::mip_interface& device); +TypedResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold); +TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); +TypedResult saveAutoZupt(C::mip_interface& device); +TypedResult loadAutoZupt(C::mip_interface& device); +TypedResult defaultAutoZupt(C::mip_interface& device); ///@} /// @@ -1959,17 +2000,18 @@ struct AutoAngularZupt uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [radians/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -1990,11 +2032,12 @@ struct AutoAngularZupt struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float threshold = 0; ///< [radians/second] @@ -2011,11 +2054,11 @@ void extract(Serializer& serializer, AutoAngularZupt& self); void insert(Serializer& serializer, const AutoAngularZupt::Response& self); void extract(Serializer& serializer, AutoAngularZupt::Response& self); -CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold); -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); -CmdResult saveAutoAngularZupt(C::mip_interface& device); -CmdResult loadAutoAngularZupt(C::mip_interface& device); -CmdResult defaultAutoAngularZupt(C::mip_interface& device); +TypedResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold); +TypedResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); +TypedResult saveAutoAngularZupt(C::mip_interface& device); +TypedResult loadAutoAngularZupt(C::mip_interface& device); +TypedResult defaultAutoAngularZupt(C::mip_interface& device); ///@} /// @@ -2028,11 +2071,12 @@ CmdResult defaultAutoAngularZupt(C::mip_interface& device); struct CommandedZupt { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2048,7 +2092,7 @@ struct CommandedZupt void insert(Serializer& serializer, const CommandedZupt& self); void extract(Serializer& serializer, CommandedZupt& self); -CmdResult commandedZupt(C::mip_interface& device); +TypedResult commandedZupt(C::mip_interface& device); ///@} /// @@ -2061,11 +2105,12 @@ CmdResult commandedZupt(C::mip_interface& device); struct CommandedAngularZupt { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2081,7 +2126,7 @@ struct CommandedAngularZupt void insert(Serializer& serializer, const CommandedAngularZupt& self); void extract(Serializer& serializer, CommandedAngularZupt& self); -CmdResult commandedAngularZupt(C::mip_interface& device); +TypedResult commandedAngularZupt(C::mip_interface& device); ///@} /// @@ -2097,16 +2142,17 @@ struct MagCaptureAutoCal { FunctionSelector function = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8000; - static const uint32_t READ_PARAMS = 0x0000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8000; + static constexpr const uint32_t READ_PARAMS = 0x0000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2130,8 +2176,8 @@ struct MagCaptureAutoCal void insert(Serializer& serializer, const MagCaptureAutoCal& self); void extract(Serializer& serializer, MagCaptureAutoCal& self); -CmdResult writeMagCaptureAutoCal(C::mip_interface& device); -CmdResult saveMagCaptureAutoCal(C::mip_interface& device); +TypedResult writeMagCaptureAutoCal(C::mip_interface& device); +TypedResult saveMagCaptureAutoCal(C::mip_interface& device); ///@} /// @@ -2151,17 +2197,18 @@ struct GravityNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Gravity Noise 1-sigma [gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2182,11 +2229,12 @@ struct GravityNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Gravity Noise 1-sigma [gauss] @@ -2202,11 +2250,11 @@ void extract(Serializer& serializer, GravityNoise& self); void insert(Serializer& serializer, const GravityNoise::Response& self); void extract(Serializer& serializer, GravityNoise::Response& self); -CmdResult writeGravityNoise(C::mip_interface& device, const float* noise); -CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveGravityNoise(C::mip_interface& device); -CmdResult loadGravityNoise(C::mip_interface& device); -CmdResult defaultGravityNoise(C::mip_interface& device); +TypedResult writeGravityNoise(C::mip_interface& device, const float* noise); +TypedResult readGravityNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveGravityNoise(C::mip_interface& device); +TypedResult loadGravityNoise(C::mip_interface& device); +TypedResult defaultGravityNoise(C::mip_interface& device); ///@} /// @@ -2226,17 +2274,18 @@ struct PressureAltitudeNoise FunctionSelector function = static_cast(0); float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2257,11 +2306,12 @@ struct PressureAltitudeNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; float noise = 0; ///< Pressure Altitude Noise 1-sigma [m] @@ -2277,11 +2327,11 @@ void extract(Serializer& serializer, PressureAltitudeNoise& self); void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self); void extract(Serializer& serializer, PressureAltitudeNoise::Response& self); -CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise); -CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); -CmdResult savePressureAltitudeNoise(C::mip_interface& device); -CmdResult loadPressureAltitudeNoise(C::mip_interface& device); -CmdResult defaultPressureAltitudeNoise(C::mip_interface& device); +TypedResult writePressureAltitudeNoise(C::mip_interface& device, float noise); +TypedResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); +TypedResult savePressureAltitudeNoise(C::mip_interface& device); +TypedResult loadPressureAltitudeNoise(C::mip_interface& device); +TypedResult defaultPressureAltitudeNoise(C::mip_interface& device); ///@} /// @@ -2303,17 +2353,18 @@ struct HardIronOffsetNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2334,11 +2385,12 @@ struct HardIronOffsetNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Hard Iron Offset Noise 1-sigma [gauss] @@ -2354,11 +2406,11 @@ void extract(Serializer& serializer, HardIronOffsetNoise& self); void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveHardIronOffsetNoise(C::mip_interface& device); -CmdResult loadHardIronOffsetNoise(C::mip_interface& device); -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); +TypedResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); +TypedResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveHardIronOffsetNoise(C::mip_interface& device); +TypedResult loadHardIronOffsetNoise(C::mip_interface& device); +TypedResult defaultHardIronOffsetNoise(C::mip_interface& device); ///@} /// @@ -2379,17 +2431,18 @@ struct SoftIronMatrixNoise FunctionSelector function = static_cast(0); Matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2410,11 +2463,12 @@ struct SoftIronMatrixNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Matrix3f noise; ///< Soft Iron Matrix Noise 1-sigma [dimensionless] @@ -2430,11 +2484,11 @@ void extract(Serializer& serializer, SoftIronMatrixNoise& self); void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self); void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self); -CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); -CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveSoftIronMatrixNoise(C::mip_interface& device); -CmdResult loadSoftIronMatrixNoise(C::mip_interface& device); -CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); +TypedResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); +TypedResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveSoftIronMatrixNoise(C::mip_interface& device); +TypedResult loadSoftIronMatrixNoise(C::mip_interface& device); +TypedResult defaultSoftIronMatrixNoise(C::mip_interface& device); ///@} /// @@ -2455,17 +2509,18 @@ struct MagNoise FunctionSelector function = static_cast(0); Vector3f noise; ///< Mag Noise 1-sigma [gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2486,11 +2541,12 @@ struct MagNoise struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Vector3f noise; ///< Mag Noise 1-sigma [gauss] @@ -2506,11 +2562,11 @@ void extract(Serializer& serializer, MagNoise& self); void insert(Serializer& serializer, const MagNoise::Response& self); void extract(Serializer& serializer, MagNoise::Response& self); -CmdResult writeMagNoise(C::mip_interface& device, const float* noise); -CmdResult readMagNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveMagNoise(C::mip_interface& device); -CmdResult loadMagNoise(C::mip_interface& device); -CmdResult defaultMagNoise(C::mip_interface& device); +TypedResult writeMagNoise(C::mip_interface& device, const float* noise); +TypedResult readMagNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveMagNoise(C::mip_interface& device); +TypedResult loadMagNoise(C::mip_interface& device); +TypedResult defaultMagNoise(C::mip_interface& device); ///@} /// @@ -2530,17 +2586,18 @@ struct InclinationSource FilterMagParamSource source = static_cast(0); ///< Inclination Source float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2561,11 +2618,12 @@ struct InclinationSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Inclination Source float inclination = 0; ///< Inclination angle [radians] (only required if source = MANUAL) @@ -2582,11 +2640,11 @@ void extract(Serializer& serializer, InclinationSource& self); void insert(Serializer& serializer, const InclinationSource::Response& self); void extract(Serializer& serializer, InclinationSource::Response& self); -CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); -CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); -CmdResult saveInclinationSource(C::mip_interface& device); -CmdResult loadInclinationSource(C::mip_interface& device); -CmdResult defaultInclinationSource(C::mip_interface& device); +TypedResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); +TypedResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); +TypedResult saveInclinationSource(C::mip_interface& device); +TypedResult loadInclinationSource(C::mip_interface& device); +TypedResult defaultInclinationSource(C::mip_interface& device); ///@} /// @@ -2606,17 +2664,18 @@ struct MagneticDeclinationSource FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2637,11 +2696,12 @@ struct MagneticDeclinationSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Magnetic field declination angle source float declination = 0; ///< Declination angle [radians] (only required if source = MANUAL) @@ -2658,11 +2718,11 @@ void extract(Serializer& serializer, MagneticDeclinationSource& self); void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); -CmdResult saveMagneticDeclinationSource(C::mip_interface& device); -CmdResult loadMagneticDeclinationSource(C::mip_interface& device); -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); +TypedResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); +TypedResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); +TypedResult saveMagneticDeclinationSource(C::mip_interface& device); +TypedResult loadMagneticDeclinationSource(C::mip_interface& device); +TypedResult defaultMagneticDeclinationSource(C::mip_interface& device); ///@} /// @@ -2681,17 +2741,18 @@ struct MagFieldMagnitudeSource FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2712,11 +2773,12 @@ struct MagFieldMagnitudeSource struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterMagParamSource source = static_cast(0); ///< Magnetic Field Magnitude Source float magnitude = 0; ///< Magnitude [gauss] (only required if source = MANUAL) @@ -2733,11 +2795,11 @@ void extract(Serializer& serializer, MagFieldMagnitudeSource& self); void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self); void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self); -CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); -CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); -CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device); -CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device); -CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device); +TypedResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); +TypedResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); +TypedResult saveMagFieldMagnitudeSource(C::mip_interface& device); +TypedResult loadMagFieldMagnitudeSource(C::mip_interface& device); +TypedResult defaultMagFieldMagnitudeSource(C::mip_interface& device); ///@} /// @@ -2758,17 +2820,18 @@ struct ReferencePosition double longitude = 0; ///< [degrees] double altitude = 0; ///< [meters] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x800F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2789,11 +2852,12 @@ struct ReferencePosition struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< enable/disable double latitude = 0; ///< [degrees] double longitude = 0; ///< [degrees] @@ -2812,11 +2876,11 @@ void extract(Serializer& serializer, ReferencePosition& self); void insert(Serializer& serializer, const ReferencePosition::Response& self); void extract(Serializer& serializer, ReferencePosition::Response& self); -CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); -CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); -CmdResult saveReferencePosition(C::mip_interface& device); -CmdResult loadReferencePosition(C::mip_interface& device); -CmdResult defaultReferencePosition(C::mip_interface& device); +TypedResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); +TypedResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); +TypedResult saveReferencePosition(C::mip_interface& device); +TypedResult loadReferencePosition(C::mip_interface& device); +TypedResult defaultReferencePosition(C::mip_interface& device); ///@} /// @@ -2850,17 +2914,18 @@ struct AccelMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x807F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x807F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2881,11 +2946,12 @@ struct AccelMagnitudeErrorAdaptiveMeasurement struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float low_limit = 0; ///< [meters/second^2] @@ -2907,11 +2973,11 @@ void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement& sel void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Response& self); -CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); -CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +TypedResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +TypedResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -2940,17 +3006,18 @@ struct MagMagnitudeErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x807F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x807F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -2971,11 +3038,12 @@ struct MagMagnitudeErrorAdaptiveMeasurement struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; FilterAdaptiveMeasurement adaptive_measurement = static_cast(0); ///< Adaptive measurement selector float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float low_limit = 0; ///< [meters/second^2] @@ -2997,11 +3065,11 @@ void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement& self) void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Response& self); -CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); -CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +TypedResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +TypedResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3030,17 +3098,18 @@ struct MagDipAngleErrorAdaptiveMeasurement float high_limit_uncertainty = 0; ///< 1-Sigma [meters/second^2] float minimum_uncertainty = 0; ///< 1-Sigma [meters/second^2] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3061,11 +3130,12 @@ struct MagDipAngleErrorAdaptiveMeasurement struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool enable = 0; ///< Enable/Disable float frequency = 0; ///< Low-pass filter curoff frequency [hertz] float high_limit = 0; ///< [meters/second^2] @@ -3085,11 +3155,11 @@ void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement& self); void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Response& self); -CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); -CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); +TypedResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +TypedResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3105,30 +3175,33 @@ struct AidingMeasurementEnable { enum class AidingSource : uint16_t { - GNSS_POS_VEL = 0, ///< GNSS Position and Velocity - GNSS_HEADING = 1, ///< GNSS Heading (dual antenna) - ALTIMETER = 2, ///< Altimeter - SPEED = 3, ///< Speed sensor / Odometer - MAGNETOMETER = 4, ///< Magnetometer - EXTERNAL_HEADING = 5, ///< External heading input - ALL = 65535, ///< Save/load/reset all options + GNSS_POS_VEL = 0, ///< GNSS Position and Velocity + GNSS_HEADING = 1, ///< GNSS Heading (dual antenna) + ALTIMETER = 2, ///< Pressure altimeter (built-in sensor) + SPEED = 3, ///< Speed sensor / Odometer + MAGNETOMETER = 4, ///< Magnetometer (built-in sensor) + EXTERNAL_HEADING = 5, ///< External heading input + EXTERNAL_ALTIMETER = 6, ///< External pressure altimeter input + EXTERNAL_MAGNETOMETER = 7, ///< External magnetomer input input + ALL = 65535, ///< Save/load/reset all options }; FunctionSelector function = static_cast(0); AidingSource aiding_source = static_cast(0); ///< Aiding measurement source bool enable = 0; ///< Controls the aiding source - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3150,11 +3223,12 @@ struct AidingMeasurementEnable struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; AidingSource aiding_source = static_cast(0); ///< Aiding measurement source bool enable = 0; ///< Controls the aiding source @@ -3171,11 +3245,11 @@ void extract(Serializer& serializer, AidingMeasurementEnable& self); void insert(Serializer& serializer, const AidingMeasurementEnable::Response& self); void extract(Serializer& serializer, AidingMeasurementEnable::Response& self); -CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable); -CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut); -CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); -CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); -CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +TypedResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable); +TypedResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut); +TypedResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +TypedResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +TypedResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); ///@} /// @@ -3190,11 +3264,12 @@ CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasure struct Run { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3210,7 +3285,7 @@ struct Run void insert(Serializer& serializer, const Run& self); void extract(Serializer& serializer, Run& self); -CmdResult run(C::mip_interface& device); +TypedResult run(C::mip_interface& device); ///@} /// @@ -3229,17 +3304,18 @@ struct KinematicConstraint uint8_t velocity_constraint_selection = 0; ///< 0=None (default),
1=Zero-velocity,
2=Wheeled-vehicle.
uint8_t angular_constraint_selection = 0; ///< 0=None (default), 1=Zero-angular rate (ZUPT). - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3260,11 +3336,12 @@ struct KinematicConstraint struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t acceleration_constraint_selection = 0; ///< Acceleration constraint:
0=None (default),
1=Zero-acceleration. uint8_t velocity_constraint_selection = 0; ///< 0=None (default),
1=Zero-velocity,
2=Wheeled-vehicle.
uint8_t angular_constraint_selection = 0; ///< 0=None (default), 1=Zero-angular rate (ZUPT). @@ -3282,11 +3359,11 @@ void extract(Serializer& serializer, KinematicConstraint& self); void insert(Serializer& serializer, const KinematicConstraint::Response& self); void extract(Serializer& serializer, KinematicConstraint::Response& self); -CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection); -CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut); -CmdResult saveKinematicConstraint(C::mip_interface& device); -CmdResult loadKinematicConstraint(C::mip_interface& device); -CmdResult defaultKinematicConstraint(C::mip_interface& device); +TypedResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection); +TypedResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut); +TypedResult saveKinematicConstraint(C::mip_interface& device); +TypedResult loadKinematicConstraint(C::mip_interface& device); +TypedResult defaultKinematicConstraint(C::mip_interface& device); ///@} /// @@ -3319,7 +3396,7 @@ struct InitializationConfiguration AlignmentSelector(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } AlignmentSelector& operator=(uint8_t val) { value = val; return *this; } - AlignmentSelector& operator=(int val) { value = val; return *this; } + AlignmentSelector& operator=(int val) { value = uint8_t(val); return *this; } AlignmentSelector& operator|=(uint8_t val) { return *this = value | val; } AlignmentSelector& operator&=(uint8_t val) { return *this = value & val; } @@ -3355,17 +3432,18 @@ struct InitializationConfiguration Vector3f initial_velocity; ///< User-specified initial platform velocity (units determined by reference frame selector, see note.) FilterReferenceFrame reference_frame_selector = static_cast(0); ///< User-specified initial position/velocity reference frames - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x81FF; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x81FF; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3386,11 +3464,12 @@ struct InitializationConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t wait_for_run_command = 0; ///< Initialize filter only after receiving "run" command InitialConditionSource initial_cond_src = static_cast(0); ///< Initial condition source: AlignmentSelector auto_heading_alignment_selector; ///< Bitfield specifying the allowed automatic heading alignment methods for automatic initial conditions. Bits are set to 1 to enable, and the correspond to the following:
@@ -3414,11 +3493,11 @@ void extract(Serializer& serializer, InitializationConfiguration& self); void insert(Serializer& serializer, const InitializationConfiguration::Response& self); void extract(Serializer& serializer, InitializationConfiguration::Response& self); -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); -CmdResult saveInitializationConfiguration(C::mip_interface& device); -CmdResult loadInitializationConfiguration(C::mip_interface& device); -CmdResult defaultInitializationConfiguration(C::mip_interface& device); +TypedResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); +TypedResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); +TypedResult saveInitializationConfiguration(C::mip_interface& device); +TypedResult loadInitializationConfiguration(C::mip_interface& device); +TypedResult defaultInitializationConfiguration(C::mip_interface& device); ///@} /// @@ -3434,17 +3513,18 @@ struct AdaptiveFilterOptions uint8_t level = 0; ///< Auto-adaptive operating level:
0=Off,
1=Conservative,
2=Moderate (default),
3=Aggressive. uint16_t time_limit = 0; ///< Maximum duration of measurement rejection before entering recovery mode (ms) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3465,11 +3545,12 @@ struct AdaptiveFilterOptions struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t level = 0; ///< Auto-adaptive operating level:
0=Off,
1=Conservative,
2=Moderate (default),
3=Aggressive. uint16_t time_limit = 0; ///< Maximum duration of measurement rejection before entering recovery mode (ms) @@ -3486,11 +3567,11 @@ void extract(Serializer& serializer, AdaptiveFilterOptions& self); void insert(Serializer& serializer, const AdaptiveFilterOptions::Response& self); void extract(Serializer& serializer, AdaptiveFilterOptions::Response& self); -CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit); -CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut); -CmdResult saveAdaptiveFilterOptions(C::mip_interface& device); -CmdResult loadAdaptiveFilterOptions(C::mip_interface& device); -CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device); +TypedResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit); +TypedResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut); +TypedResult saveAdaptiveFilterOptions(C::mip_interface& device); +TypedResult loadAdaptiveFilterOptions(C::mip_interface& device); +TypedResult defaultAdaptiveFilterOptions(C::mip_interface& device); ///@} /// @@ -3509,17 +3590,18 @@ struct MultiAntennaOffset uint8_t receiver_id = 0; ///< Receiver: 1, 2, etc... Vector3f antenna_offset; ///< Antenna lever arm offset vector in the vehicle frame (m) - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3541,11 +3623,12 @@ struct MultiAntennaOffset struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t receiver_id = 0; Vector3f antenna_offset; @@ -3562,11 +3645,11 @@ void extract(Serializer& serializer, MultiAntennaOffset& self); void insert(Serializer& serializer, const MultiAntennaOffset::Response& self); void extract(Serializer& serializer, MultiAntennaOffset::Response& self); -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); -CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); -CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); -CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +TypedResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); +TypedResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); +TypedResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +TypedResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +TypedResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); ///@} /// @@ -3583,17 +3666,18 @@ struct RelPosConfiguration FilterReferenceFrame reference_frame_selector = static_cast(0); ///< ECEF or LLH Vector3d reference_coordinates; ///< reference coordinates, units determined by source selection - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8007; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8007; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3614,11 +3698,12 @@ struct RelPosConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t source = 0; ///< 0 - auto (RTK base station), 1 - manual FilterReferenceFrame reference_frame_selector = static_cast(0); ///< ECEF or LLH Vector3d reference_coordinates; ///< reference coordinates, units determined by source selection @@ -3636,11 +3721,11 @@ void extract(Serializer& serializer, RelPosConfiguration& self); void insert(Serializer& serializer, const RelPosConfiguration::Response& self); void extract(Serializer& serializer, RelPosConfiguration::Response& self); -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); -CmdResult saveRelPosConfiguration(C::mip_interface& device); -CmdResult loadRelPosConfiguration(C::mip_interface& device); -CmdResult defaultRelPosConfiguration(C::mip_interface& device); +TypedResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); +TypedResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); +TypedResult saveRelPosConfiguration(C::mip_interface& device); +TypedResult loadRelPosConfiguration(C::mip_interface& device); +TypedResult defaultRelPosConfiguration(C::mip_interface& device); ///@} /// @@ -3668,17 +3753,18 @@ struct RefPointLeverArm ReferencePointSelector ref_point_sel = static_cast(0); ///< Reserved, must be 1 Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3699,11 +3785,12 @@ struct RefPointLeverArm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; ReferencePointSelector ref_point_sel = static_cast(0); ///< Reserved, must be 1 Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. @@ -3720,11 +3807,11 @@ void extract(Serializer& serializer, RefPointLeverArm& self); void insert(Serializer& serializer, const RefPointLeverArm::Response& self); void extract(Serializer& serializer, RefPointLeverArm::Response& self); -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); -CmdResult saveRefPointLeverArm(C::mip_interface& device); -CmdResult loadRefPointLeverArm(C::mip_interface& device); -CmdResult defaultRefPointLeverArm(C::mip_interface& device); +TypedResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); +TypedResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); +TypedResult saveRefPointLeverArm(C::mip_interface& device); +TypedResult loadRefPointLeverArm(C::mip_interface& device); +TypedResult defaultRefPointLeverArm(C::mip_interface& device); ///@} /// @@ -3743,11 +3830,12 @@ struct SpeedMeasurement float speed = 0; ///< Estimated speed along vehicle's x-axis (may be positive or negative) [meters/second] float speed_uncertainty = 0; ///< Estimated uncertainty in the speed measurement (1-sigma value) [meters/second] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3763,7 +3851,7 @@ struct SpeedMeasurement void insert(Serializer& serializer, const SpeedMeasurement& self); void extract(Serializer& serializer, SpeedMeasurement& self); -CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty); +TypedResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty); ///@} /// @@ -3785,17 +3873,18 @@ struct SpeedLeverArm uint8_t source = 0; ///< Reserved, must be 1. Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8001; - static const uint32_t SAVE_PARAMS = 0x8001; - static const uint32_t LOAD_PARAMS = 0x8001; - static const uint32_t DEFAULT_PARAMS = 0x8001; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8001; + static constexpr const uint32_t SAVE_PARAMS = 0x8001; + static constexpr const uint32_t LOAD_PARAMS = 0x8001; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8001; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3817,11 +3906,12 @@ struct SpeedLeverArm struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0001; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0001; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t source = 0; ///< Reserved, must be 1. Vector3f lever_arm_offset; ///< [m] Lever arm offset vector in the vehicle's reference frame. @@ -3838,11 +3928,11 @@ void extract(Serializer& serializer, SpeedLeverArm& self); void insert(Serializer& serializer, const SpeedLeverArm::Response& self); void extract(Serializer& serializer, SpeedLeverArm::Response& self); -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); -CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); -CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); -CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); +TypedResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); +TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); +TypedResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); +TypedResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); +TypedResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); ///@} /// @@ -3863,17 +3953,18 @@ struct WheeledVehicleConstraintControl FunctionSelector function = static_cast(0); uint8_t enable = 0; ///< 0 - Disable, 1 - Enable - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3894,11 +3985,12 @@ struct WheeledVehicleConstraintControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable @@ -3914,11 +4006,11 @@ void extract(Serializer& serializer, WheeledVehicleConstraintControl& self); void insert(Serializer& serializer, const WheeledVehicleConstraintControl::Response& self); void extract(Serializer& serializer, WheeledVehicleConstraintControl::Response& self); -CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable); -CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut); -CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device); -CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device); -CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); +TypedResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable); +TypedResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut); +TypedResult saveWheeledVehicleConstraintControl(C::mip_interface& device); +TypedResult loadWheeledVehicleConstraintControl(C::mip_interface& device); +TypedResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); ///@} /// @@ -3937,17 +4029,18 @@ struct VerticalGyroConstraintControl FunctionSelector function = static_cast(0); uint8_t enable = 0; ///< 0 - Disable, 1 - Enable - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -3968,11 +4061,12 @@ struct VerticalGyroConstraintControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable @@ -3988,11 +4082,11 @@ void extract(Serializer& serializer, VerticalGyroConstraintControl& self); void insert(Serializer& serializer, const VerticalGyroConstraintControl::Response& self); void extract(Serializer& serializer, VerticalGyroConstraintControl::Response& self); -CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable); -CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut); -CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device); -CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device); -CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device); +TypedResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable); +TypedResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut); +TypedResult saveVerticalGyroConstraintControl(C::mip_interface& device); +TypedResult loadVerticalGyroConstraintControl(C::mip_interface& device); +TypedResult defaultVerticalGyroConstraintControl(C::mip_interface& device); ///@} /// @@ -4010,17 +4104,18 @@ struct GnssAntennaCalControl uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float max_offset = 0; ///< Maximum absolute value of lever arm offset error in the vehicle frame [meters]. See device user manual for the valid range of this parameter. - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -4041,11 +4136,12 @@ struct GnssAntennaCalControl struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; ///< 0 - Disable, 1 - Enable float max_offset = 0; ///< Maximum absolute value of lever arm offset error in the vehicle frame [meters]. See device user manual for the valid range of this parameter. @@ -4062,11 +4158,11 @@ void extract(Serializer& serializer, GnssAntennaCalControl& self); void insert(Serializer& serializer, const GnssAntennaCalControl::Response& self); void extract(Serializer& serializer, GnssAntennaCalControl::Response& self); -CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset); -CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut); -CmdResult saveGnssAntennaCalControl(C::mip_interface& device); -CmdResult loadGnssAntennaCalControl(C::mip_interface& device); -CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); +TypedResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset); +TypedResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut); +TypedResult saveGnssAntennaCalControl(C::mip_interface& device); +TypedResult loadGnssAntennaCalControl(C::mip_interface& device); +TypedResult defaultGnssAntennaCalControl(C::mip_interface& device); ///@} /// @@ -4083,11 +4179,12 @@ struct SetInitialHeading { float heading = 0; ///< Initial heading in radians [-pi, pi] - static const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -4103,7 +4200,7 @@ struct SetInitialHeading void insert(Serializer& serializer, const SetInitialHeading& self); void extract(Serializer& serializer, SetInitialHeading& self); -CmdResult setInitialHeading(C::mip_interface& device, float heading); +TypedResult setInitialHeading(C::mip_interface& device, float heading); ///@} /// diff --git a/src/mip/definitions/commands_gnss.cpp b/src/mip/definitions/commands_gnss.cpp index d099323d0..f980f2e9c 100644 --- a/src/mip/definitions/commands_gnss.cpp +++ b/src/mip/definitions/commands_gnss.cpp @@ -77,12 +77,12 @@ void extract(Serializer& serializer, ReceiverInfo::Info& self) } -CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut) +TypedResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LIST_RECEIVERS, NULL, 0, REPLY_LIST_RECEIVERS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LIST_RECEIVERS, NULL, 0, REPLY_LIST_RECEIVERS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -166,7 +166,7 @@ void extract(Serializer& serializer, SignalConfiguration::Response& self) } -CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved) +TypedResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -188,7 +188,7 @@ CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut) +TypedResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -197,7 +197,7 @@ CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SIGNAL_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SIGNAL_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -224,7 +224,7 @@ CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOu } return result; } -CmdResult saveSignalConfiguration(C::mip_interface& device) +TypedResult saveSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -234,7 +234,7 @@ CmdResult saveSignalConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSignalConfiguration(C::mip_interface& device) +TypedResult loadSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -244,7 +244,7 @@ CmdResult loadSignalConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSignalConfiguration(C::mip_interface& device) +TypedResult defaultSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -298,7 +298,7 @@ void extract(Serializer& serializer, RtkDongleConfiguration::Response& self) } -CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved) +TypedResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -314,7 +314,7 @@ CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut) +TypedResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -323,7 +323,7 @@ CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_RTK_DONGLE_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_RTK_DONGLE_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -341,7 +341,7 @@ CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOu } return result; } -CmdResult saveRtkDongleConfiguration(C::mip_interface& device) +TypedResult saveRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -351,7 +351,7 @@ CmdResult saveRtkDongleConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadRtkDongleConfiguration(C::mip_interface& device) +TypedResult loadRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -361,7 +361,7 @@ CmdResult loadRtkDongleConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultRtkDongleConfiguration(C::mip_interface& device) +TypedResult defaultRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index ee33ec593..7cce02d91 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -45,14 +45,14 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint16_t GNSS_GPS_ENABLE_L1CA = 0x0001; -static const uint16_t GNSS_GPS_ENABLE_L2C = 0x0002; -static const uint16_t GNSS_GLONASS_ENABLE_L1OF = 0x0001; -static const uint16_t GNSS_GLONASS_ENABLE_L2OF = 0x0002; -static const uint16_t GNSS_GALILEO_ENABLE_E1 = 0x0001; -static const uint16_t GNSS_GALILEO_ENABLE_E5B = 0x0002; -static const uint16_t GNSS_BEIDOU_ENABLE_B1 = 0x0001; -static const uint16_t GNSS_BEIDOU_ENABLE_B2 = 0x0002; +static constexpr const uint16_t GNSS_GPS_ENABLE_L1CA = 0x0001; +static constexpr const uint16_t GNSS_GPS_ENABLE_L2C = 0x0002; +static constexpr const uint16_t GNSS_GLONASS_ENABLE_L1OF = 0x0001; +static constexpr const uint16_t GNSS_GLONASS_ENABLE_L2OF = 0x0002; +static constexpr const uint16_t GNSS_GALILEO_ENABLE_E1 = 0x0001; +static constexpr const uint16_t GNSS_GALILEO_ENABLE_E5B = 0x0002; +static constexpr const uint16_t GNSS_BEIDOU_ENABLE_B1 = 0x0001; +static constexpr const uint16_t GNSS_BEIDOU_ENABLE_B2 = 0x0002; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -75,12 +75,13 @@ struct ReceiverInfo }; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -93,11 +94,12 @@ struct ReceiverInfo } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x0000000C; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; uint8_t num_receivers = 0; ///< Number of physical receivers in the device Info receiver_info[5]; @@ -117,7 +119,7 @@ void extract(Serializer& serializer, ReceiverInfo::Info& self); void insert(Serializer& serializer, const ReceiverInfo::Response& self); void extract(Serializer& serializer, ReceiverInfo::Response& self); -CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut); +TypedResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut); ///@} /// @@ -137,17 +139,18 @@ struct SignalConfiguration uint8_t beidou_enable = 0; ///< Bitfield 0: Enable B1, 1: Enable B2 uint8_t reserved[4] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x801F; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -168,11 +171,12 @@ struct SignalConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t gps_enable = 0; ///< Bitfield 0: Enable L1CA, 1: Enable L2C uint8_t glonass_enable = 0; ///< Bitfield 0: Enable L1OF, 1: Enable L2OF uint8_t galileo_enable = 0; ///< Bitfield 0: Enable E1, 1: Enable E5B @@ -192,11 +196,11 @@ void extract(Serializer& serializer, SignalConfiguration& self); void insert(Serializer& serializer, const SignalConfiguration::Response& self); void extract(Serializer& serializer, SignalConfiguration::Response& self); -CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved); -CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut); -CmdResult saveSignalConfiguration(C::mip_interface& device); -CmdResult loadSignalConfiguration(C::mip_interface& device); -CmdResult defaultSignalConfiguration(C::mip_interface& device); +TypedResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved); +TypedResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut); +TypedResult saveSignalConfiguration(C::mip_interface& device); +TypedResult loadSignalConfiguration(C::mip_interface& device); +TypedResult defaultSignalConfiguration(C::mip_interface& device); ///@} /// @@ -213,17 +217,18 @@ struct RtkDongleConfiguration uint8_t enable = 0; ///< 0 - Disabled, 1- Enabled uint8_t reserved[3] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8003; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8003; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -244,11 +249,12 @@ struct RtkDongleConfiguration struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t enable = 0; uint8_t reserved[3] = {0}; @@ -265,11 +271,11 @@ void extract(Serializer& serializer, RtkDongleConfiguration& self); void insert(Serializer& serializer, const RtkDongleConfiguration::Response& self); void extract(Serializer& serializer, RtkDongleConfiguration::Response& self); -CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved); -CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut); -CmdResult saveRtkDongleConfiguration(C::mip_interface& device); -CmdResult loadRtkDongleConfiguration(C::mip_interface& device); -CmdResult defaultRtkDongleConfiguration(C::mip_interface& device); +TypedResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved); +TypedResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut); +TypedResult saveRtkDongleConfiguration(C::mip_interface& device); +TypedResult loadRtkDongleConfiguration(C::mip_interface& device); +TypedResult defaultRtkDongleConfiguration(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_rtk.cpp b/src/mip/definitions/commands_rtk.cpp index b2997856e..2ec4b6dd2 100644 --- a/src/mip/definitions/commands_rtk.cpp +++ b/src/mip/definitions/commands_rtk.cpp @@ -51,12 +51,12 @@ void extract(Serializer& serializer, GetStatusFlags::Response& self) } -CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut) +TypedResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_STATUS_FLAGS, NULL, 0, REPLY_GET_STATUS_FLAGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_STATUS_FLAGS, NULL, 0, REPLY_GET_STATUS_FLAGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -94,12 +94,12 @@ void extract(Serializer& serializer, GetImei::Response& self) } -CmdResult getImei(C::mip_interface& device, char* imeiOut) +TypedResult getImei(C::mip_interface& device, char* imeiOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMEI, NULL, 0, REPLY_GET_IMEI, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMEI, NULL, 0, REPLY_GET_IMEI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -138,12 +138,12 @@ void extract(Serializer& serializer, GetImsi::Response& self) } -CmdResult getImsi(C::mip_interface& device, char* imsiOut) +TypedResult getImsi(C::mip_interface& device, char* imsiOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMSI, NULL, 0, REPLY_GET_IMSI, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMSI, NULL, 0, REPLY_GET_IMSI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -182,12 +182,12 @@ void extract(Serializer& serializer, GetIccid::Response& self) } -CmdResult getIccid(C::mip_interface& device, char* iccidOut) +TypedResult getIccid(C::mip_interface& device, char* iccidOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ICCID, NULL, 0, REPLY_GET_ICCID, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ICCID, NULL, 0, REPLY_GET_ICCID, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -234,7 +234,7 @@ void extract(Serializer& serializer, ConnectedDeviceType::Response& self) } -CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype) +TypedResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -246,7 +246,7 @@ CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut) +TypedResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -255,7 +255,7 @@ CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType: assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONNECTED_DEVICE_TYPE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONNECTED_DEVICE_TYPE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -269,7 +269,7 @@ CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType: } return result; } -CmdResult saveConnectedDeviceType(C::mip_interface& device) +TypedResult saveConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -279,7 +279,7 @@ CmdResult saveConnectedDeviceType(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadConnectedDeviceType(C::mip_interface& device) +TypedResult loadConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -289,7 +289,7 @@ CmdResult loadConnectedDeviceType(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultConnectedDeviceType(C::mip_interface& device) +TypedResult defaultConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -323,12 +323,12 @@ void extract(Serializer& serializer, GetActCode::Response& self) } -CmdResult getActCode(C::mip_interface& device, char* activationcodeOut) +TypedResult getActCode(C::mip_interface& device, char* activationcodeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ACT_CODE, NULL, 0, REPLY_GET_ACT_CODE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ACT_CODE, NULL, 0, REPLY_GET_ACT_CODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -367,12 +367,12 @@ void extract(Serializer& serializer, GetModemFirmwareVersion::Response& self) } -CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut) +TypedResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_MODEM_FIRMWARE_VERSION, NULL, 0, REPLY_GET_MODEM_FIRMWARE_VERSION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_MODEM_FIRMWARE_VERSION, NULL, 0, REPLY_GET_MODEM_FIRMWARE_VERSION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -417,12 +417,12 @@ void extract(Serializer& serializer, GetRssi::Response& self) } -CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut) +TypedResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_RSSI, NULL, 0, REPLY_GET_RSSI, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_RSSI, NULL, 0, REPLY_GET_RSSI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -480,7 +480,7 @@ void extract(Serializer& serializer, ServiceStatus::Response& self) } -CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) +TypedResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -492,7 +492,7 @@ CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t r assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SERVICE_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SERVICE_STATUS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SERVICE_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SERVICE_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -526,7 +526,7 @@ void extract(Serializer& serializer, ProdEraseStorage& self) } -CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media) +TypedResult prodEraseStorage(C::mip_interface& device, MediaSelector media) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -564,7 +564,7 @@ void extract(Serializer& serializer, LedControl& self) } -CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period) +TypedResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -596,7 +596,7 @@ void extract(Serializer& serializer, ModemHardReset& self) (void)self; } -CmdResult modemHardReset(C::mip_interface& device) +TypedResult modemHardReset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MODEM_HARD_RESET, NULL, 0); } diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index c5f0df4dd..8c680da82 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -109,7 +109,7 @@ struct GetStatusFlags StatusFlagsLegacy(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } StatusFlagsLegacy& operator=(uint32_t val) { value = val; return *this; } - StatusFlagsLegacy& operator=(int val) { value = val; return *this; } + StatusFlagsLegacy& operator=(int val) { value = uint32_t(val); return *this; } StatusFlagsLegacy& operator|=(uint32_t val) { return *this = value | val; } StatusFlagsLegacy& operator&=(uint32_t val) { return *this = value & val; } @@ -165,7 +165,7 @@ struct GetStatusFlags StatusFlags(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } StatusFlags& operator=(uint32_t val) { value = val; return *this; } - StatusFlags& operator=(int val) { value = val; return *this; } + StatusFlags& operator=(int val) { value = uint32_t(val); return *this; } StatusFlags& operator|=(uint32_t val) { return *this = value | val; } StatusFlags& operator&=(uint32_t val) { return *this = value & val; } @@ -199,12 +199,13 @@ struct GetStatusFlags }; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -217,11 +218,12 @@ struct GetStatusFlags } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; StatusFlags flags; ///< Model number dependent. See above structures. @@ -237,7 +239,7 @@ void extract(Serializer& serializer, GetStatusFlags& self); void insert(Serializer& serializer, const GetStatusFlags::Response& self); void extract(Serializer& serializer, GetStatusFlags::Response& self); -CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut); +TypedResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut); ///@} /// @@ -249,12 +251,13 @@ CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* struct GetImei { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -267,11 +270,12 @@ struct GetImei } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char IMEI[32] = {0}; @@ -287,7 +291,7 @@ void extract(Serializer& serializer, GetImei& self); void insert(Serializer& serializer, const GetImei::Response& self); void extract(Serializer& serializer, GetImei::Response& self); -CmdResult getImei(C::mip_interface& device, char* imeiOut); +TypedResult getImei(C::mip_interface& device, char* imeiOut); ///@} /// @@ -299,12 +303,13 @@ CmdResult getImei(C::mip_interface& device, char* imeiOut); struct GetImsi { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -317,11 +322,12 @@ struct GetImsi } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char IMSI[32] = {0}; @@ -337,7 +343,7 @@ void extract(Serializer& serializer, GetImsi& self); void insert(Serializer& serializer, const GetImsi::Response& self); void extract(Serializer& serializer, GetImsi::Response& self); -CmdResult getImsi(C::mip_interface& device, char* imsiOut); +TypedResult getImsi(C::mip_interface& device, char* imsiOut); ///@} /// @@ -349,12 +355,13 @@ CmdResult getImsi(C::mip_interface& device, char* imsiOut); struct GetIccid { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -367,11 +374,12 @@ struct GetIccid } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char ICCID[32] = {0}; @@ -387,7 +395,7 @@ void extract(Serializer& serializer, GetIccid& self); void insert(Serializer& serializer, const GetIccid::Response& self); void extract(Serializer& serializer, GetIccid::Response& self); -CmdResult getIccid(C::mip_interface& device, char* iccidOut); +TypedResult getIccid(C::mip_interface& device, char* iccidOut); ///@} /// @@ -407,17 +415,18 @@ struct ConnectedDeviceType FunctionSelector function = static_cast(0); Type devType = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x8000; - static const uint32_t LOAD_PARAMS = 0x8000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x8000; + static constexpr const uint32_t LOAD_PARAMS = 0x8000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -438,11 +447,12 @@ struct ConnectedDeviceType struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; Type devType = static_cast(0); @@ -458,11 +468,11 @@ void extract(Serializer& serializer, ConnectedDeviceType& self); void insert(Serializer& serializer, const ConnectedDeviceType::Response& self); void extract(Serializer& serializer, ConnectedDeviceType::Response& self); -CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype); -CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut); -CmdResult saveConnectedDeviceType(C::mip_interface& device); -CmdResult loadConnectedDeviceType(C::mip_interface& device); -CmdResult defaultConnectedDeviceType(C::mip_interface& device); +TypedResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype); +TypedResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut); +TypedResult saveConnectedDeviceType(C::mip_interface& device); +TypedResult loadConnectedDeviceType(C::mip_interface& device); +TypedResult defaultConnectedDeviceType(C::mip_interface& device); ///@} /// @@ -474,12 +484,13 @@ CmdResult defaultConnectedDeviceType(C::mip_interface& device); struct GetActCode { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -492,11 +503,12 @@ struct GetActCode } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char ActivationCode[32] = {0}; @@ -512,7 +524,7 @@ void extract(Serializer& serializer, GetActCode& self); void insert(Serializer& serializer, const GetActCode::Response& self); void extract(Serializer& serializer, GetActCode::Response& self); -CmdResult getActCode(C::mip_interface& device, char* activationcodeOut); +TypedResult getActCode(C::mip_interface& device, char* activationcodeOut); ///@} /// @@ -524,12 +536,13 @@ CmdResult getActCode(C::mip_interface& device, char* activationcodeOut); struct GetModemFirmwareVersion { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -542,11 +555,12 @@ struct GetModemFirmwareVersion } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; char ModemFirmwareVersion[32] = {0}; @@ -562,7 +576,7 @@ void extract(Serializer& serializer, GetModemFirmwareVersion& self); void insert(Serializer& serializer, const GetModemFirmwareVersion::Response& self); void extract(Serializer& serializer, GetModemFirmwareVersion::Response& self); -CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut); +TypedResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut); ///@} /// @@ -575,12 +589,13 @@ CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwarev struct GetRssi { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -593,11 +608,12 @@ struct GetRssi } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; bool valid = 0; int32_t rssi = 0; int32_t signalQuality = 0; @@ -615,7 +631,7 @@ void extract(Serializer& serializer, GetRssi& self); void insert(Serializer& serializer, const GetRssi::Response& self); void extract(Serializer& serializer, GetRssi::Response& self); -CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut); +TypedResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut); ///@} /// @@ -643,7 +659,7 @@ struct ServiceStatus ServiceFlags(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } ServiceFlags& operator=(uint8_t val) { value = val; return *this; } - ServiceFlags& operator=(int val) { value = val; return *this; } + ServiceFlags& operator=(int val) { value = uint8_t(val); return *this; } ServiceFlags& operator|=(uint8_t val) { return *this = value | val; } ServiceFlags& operator&=(uint8_t val) { return *this = value & val; } @@ -661,12 +677,13 @@ struct ServiceStatus uint32_t reserved1 = 0; uint32_t reserved2 = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -679,11 +696,12 @@ struct ServiceStatus } struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; ServiceFlags flags; uint32_t receivedBytes = 0; uint32_t lastBytes = 0; @@ -702,7 +720,7 @@ void extract(Serializer& serializer, ServiceStatus& self); void insert(Serializer& serializer, const ServiceStatus::Response& self); void extract(Serializer& serializer, ServiceStatus::Response& self); -CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); +TypedResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); ///@} /// @@ -717,11 +735,12 @@ struct ProdEraseStorage { MediaSelector media = static_cast(0); - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -737,7 +756,7 @@ struct ProdEraseStorage void insert(Serializer& serializer, const ProdEraseStorage& self); void extract(Serializer& serializer, ProdEraseStorage& self); -CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media); +TypedResult prodEraseStorage(C::mip_interface& device, MediaSelector media); ///@} /// @@ -754,11 +773,12 @@ struct LedControl LedAction act = static_cast(0); uint32_t period = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -774,7 +794,7 @@ struct LedControl void insert(Serializer& serializer, const LedControl& self); void extract(Serializer& serializer, LedControl& self); -CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period); +TypedResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period); ///@} /// @@ -788,11 +808,12 @@ CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, cons struct ModemHardReset { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = false; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -808,7 +829,7 @@ struct ModemHardReset void insert(Serializer& serializer, const ModemHardReset& self); void extract(Serializer& serializer, ModemHardReset& self); -CmdResult modemHardReset(C::mip_interface& device); +TypedResult modemHardReset(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_system.cpp b/src/mip/definitions/commands_system.cpp index 749941ce0..748a9c184 100644 --- a/src/mip/definitions/commands_system.cpp +++ b/src/mip/definitions/commands_system.cpp @@ -61,7 +61,7 @@ void extract(Serializer& serializer, CommMode::Response& self) } -CmdResult writeCommMode(C::mip_interface& device, uint8_t mode) +TypedResult writeCommMode(C::mip_interface& device, uint8_t mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -73,7 +73,7 @@ CmdResult writeCommMode(C::mip_interface& device, uint8_t mode) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut) +TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -82,7 +82,7 @@ CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COM_MODE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COM_MODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -96,7 +96,7 @@ CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut) } return result; } -CmdResult defaultCommMode(C::mip_interface& device) +TypedResult defaultCommMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index 537e0d042..b7f8595eb 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -46,10 +46,10 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_PASSTHRU = 0x00; -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_NORMAL = 0x01; -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_IMU = 0x02; -static const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_GPS = 0x03; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_PASSTHRU = 0x00; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_NORMAL = 0x01; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_IMU = 0x02; +static constexpr const uint8_t MIP_SYSTEM_COMMAND_COMM_MODE_GPS = 0x03; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -74,17 +74,18 @@ struct CommMode FunctionSelector function = static_cast(0); uint8_t mode = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const bool HAS_FUNCTION_SELECTOR = true; - static const uint32_t WRITE_PARAMS = 0x8001; - static const uint32_t READ_PARAMS = 0x8000; - static const uint32_t SAVE_PARAMS = 0x0000; - static const uint32_t LOAD_PARAMS = 0x0000; - static const uint32_t DEFAULT_PARAMS = 0x8000; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const bool HAS_FUNCTION_SELECTOR = true; + static constexpr const uint32_t WRITE_PARAMS = 0x8001; + static constexpr const uint32_t READ_PARAMS = 0x8000; + static constexpr const uint32_t SAVE_PARAMS = 0x0000; + static constexpr const uint32_t LOAD_PARAMS = 0x0000; + static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; auto as_tuple() const { @@ -105,11 +106,12 @@ struct CommMode struct Response { - static const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static const uint32_t ECHOED_PARAMS = 0x0000; - static const uint32_t COUNTER_PARAMS = 0x00000000; + static constexpr const uint32_t ECHOED_PARAMS = 0x0000; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t mode = 0; @@ -125,9 +127,9 @@ void extract(Serializer& serializer, CommMode& self); void insert(Serializer& serializer, const CommMode::Response& self); void extract(Serializer& serializer, CommMode::Response& self); -CmdResult writeCommMode(C::mip_interface& device, uint8_t mode); -CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut); -CmdResult defaultCommMode(C::mip_interface& device); +TypedResult writeCommMode(C::mip_interface& device, uint8_t mode); +TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut); +TypedResult defaultCommMode(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/data_filter.c b/src/mip/definitions/data_filter.c index f719207eb..0e395aba8 100644 --- a/src/mip/definitions/data_filter.c +++ b/src/mip/definitions/data_filter.c @@ -1687,6 +1687,70 @@ void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags( *self = tmp; } +void insert_mip_filter_aiding_frame_config_error_data(mip_serializer* serializer, const mip_filter_aiding_frame_config_error_data* self) +{ + insert_u8(serializer, self->frame_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->translation[i]); + + for(unsigned int i=0; i < 4; i++) + insert_float(serializer, self->attitude[i]); + +} +void extract_mip_filter_aiding_frame_config_error_data(mip_serializer* serializer, mip_filter_aiding_frame_config_error_data* self) +{ + extract_u8(serializer, &self->frame_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->translation[i]); + + for(unsigned int i=0; i < 4; i++) + extract_float(serializer, &self->attitude[i]); + +} +bool extract_mip_filter_aiding_frame_config_error_data_from_field(const mip_field* field, void* ptr) +{ + assert(ptr); + mip_filter_aiding_frame_config_error_data* self = ptr; + struct mip_serializer serializer; + mip_serializer_init_from_field(&serializer, field); + extract_mip_filter_aiding_frame_config_error_data(&serializer, self); + return mip_serializer_is_complete(&serializer); +} + +void insert_mip_filter_aiding_frame_config_error_uncertainty_data(mip_serializer* serializer, const mip_filter_aiding_frame_config_error_uncertainty_data* self) +{ + insert_u8(serializer, self->frame_id); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + insert_float(serializer, self->attitude_unc[i]); + +} +void extract_mip_filter_aiding_frame_config_error_uncertainty_data(mip_serializer* serializer, mip_filter_aiding_frame_config_error_uncertainty_data* self) +{ + extract_u8(serializer, &self->frame_id); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + extract_float(serializer, &self->attitude_unc[i]); + +} +bool extract_mip_filter_aiding_frame_config_error_uncertainty_data_from_field(const mip_field* field, void* ptr) +{ + assert(ptr); + mip_filter_aiding_frame_config_error_uncertainty_data* self = ptr; + struct mip_serializer serializer; + mip_serializer_init_from_field(&serializer, field); + extract_mip_filter_aiding_frame_config_error_uncertainty_data(&serializer, self); + return mip_serializer_is_complete(&serializer); +} + #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/data_filter.cpp b/src/mip/definitions/data_filter.cpp index fdb85f32b..50e8ae9ba 100644 --- a/src/mip/definitions/data_filter.cpp +++ b/src/mip/definitions/data_filter.cpp @@ -1084,6 +1084,52 @@ void extract(Serializer& serializer, GnssDualAntennaStatus& self) } +void insert(Serializer& serializer, const AidingFrameConfigError& self) +{ + insert(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.translation[i]); + + for(unsigned int i=0; i < 4; i++) + insert(serializer, self.attitude[i]); + +} +void extract(Serializer& serializer, AidingFrameConfigError& self) +{ + extract(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.translation[i]); + + for(unsigned int i=0; i < 4; i++) + extract(serializer, self.attitude[i]); + +} + +void insert(Serializer& serializer, const AidingFrameConfigErrorUncertainty& self) +{ + insert(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + insert(serializer, self.attitude_unc[i]); + +} +void extract(Serializer& serializer, AidingFrameConfigErrorUncertainty& self) +{ + extract(serializer, self.frame_id); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.translation_unc[i]); + + for(unsigned int i=0; i < 3; i++) + extract(serializer, self.attitude_unc[i]); + +} + } // namespace data_filter } // namespace mip diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index b9aea9892..8898c974b 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -93,6 +93,8 @@ enum MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR = 0x47, MIP_DATA_DESC_FILTER_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, MIP_DATA_DESC_FILTER_GNSS_DUAL_ANTENNA_STATUS = 0x49, + MIP_DATA_DESC_FILTER_FRAME_CONFIG_ERROR = 0x50, + MIP_DATA_DESC_FILTER_FRAME_CONFIG_ERROR_UNCERTAINTY = 0x51, }; @@ -158,12 +160,18 @@ void insert_mip_filter_status_flags(struct mip_serializer* serializer, const mip void extract_mip_filter_status_flags(struct mip_serializer* serializer, mip_filter_status_flags* self); typedef uint8_t mip_filter_aiding_measurement_type; -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_GNSS = 1; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_DUAL_ANTENNA = 2; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING = 3; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_PRESSURE = 4; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_MAGNETOMETER = 5; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_SPEED = 6; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_GNSS = 1; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_DUAL_ANTENNA = 2; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING = 3; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_PRESSURE = 4; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_MAGNETOMETER = 5; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_SPEED = 6; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_POS_ECEF = 33; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_POS_LLH = 34; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_ECEF = 40; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_NED = 41; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_VEHICLE_FRAME = 42; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING_TRUE = 49; ///< void insert_mip_filter_aiding_measurement_type(struct mip_serializer* serializer, const mip_filter_aiding_measurement_type self); void extract_mip_filter_aiding_measurement_type(struct mip_serializer* serializer, mip_filter_aiding_measurement_type* self); @@ -1333,7 +1341,7 @@ struct mip_filter_aiding_measurement_summary_data { float time_of_week; ///< [seconds] uint8_t source; - mip_filter_aiding_measurement_type type; ///< (see product manual for supported types) + mip_filter_aiding_measurement_type type; ///< (see product manual for supported types) Note: values 0x20 and above correspond to commanded aiding measurements in the 0x13 Aiding command set. mip_filter_measurement_indicator indicator; }; @@ -1425,6 +1433,52 @@ void insert_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(s void extract_mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags(struct mip_serializer* serializer, mip_filter_gnss_dual_antenna_status_data_dual_antenna_status_flags* self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_aiding_frame_config_error (0x82,0x50) Aiding Frame Config Error [C] +/// Filter reported aiding source frame configuration error +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct mip_filter_aiding_frame_config_error_data +{ + uint8_t frame_id; ///< Frame ID for the receiver to which the antenna is attached + mip_vector3f translation; ///< Translation config X, Y, and Z (m). + mip_quatf attitude; ///< Attitude quaternion + +}; +typedef struct mip_filter_aiding_frame_config_error_data mip_filter_aiding_frame_config_error_data; +void insert_mip_filter_aiding_frame_config_error_data(struct mip_serializer* serializer, const mip_filter_aiding_frame_config_error_data* self); +void extract_mip_filter_aiding_frame_config_error_data(struct mip_serializer* serializer, mip_filter_aiding_frame_config_error_data* self); +bool extract_mip_filter_aiding_frame_config_error_data_from_field(const struct mip_field* field, void* ptr); + + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_filter_aiding_frame_config_error_uncertainty (0x82,0x51) Aiding Frame Config Error Uncertainty [C] +/// Filter reported aiding source frame configuration error uncertainty +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct mip_filter_aiding_frame_config_error_uncertainty_data +{ + uint8_t frame_id; ///< Frame ID for the receiver to which the antenna is attached + mip_vector3f translation_unc; ///< Translation uncertaint X, Y, and Z (m). + mip_vector3f attitude_unc; ///< Attitude uncertainty, X, Y, and Z (radians). + +}; +typedef struct mip_filter_aiding_frame_config_error_uncertainty_data mip_filter_aiding_frame_config_error_uncertainty_data; +void insert_mip_filter_aiding_frame_config_error_uncertainty_data(struct mip_serializer* serializer, const mip_filter_aiding_frame_config_error_uncertainty_data* self); +void extract_mip_filter_aiding_frame_config_error_uncertainty_data(struct mip_serializer* serializer, mip_filter_aiding_frame_config_error_uncertainty_data* self); +bool extract_mip_filter_aiding_frame_config_error_uncertainty_data_from_field(const struct mip_field* field, void* ptr); + + ///@} /// diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index d55259ec2..9802d413f 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -92,6 +92,8 @@ enum DATA_ODOMETER_SCALE_FACTOR_ERROR = 0x47, DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY = 0x48, DATA_GNSS_DUAL_ANTENNA_STATUS = 0x49, + DATA_FRAME_CONFIG_ERROR = 0x50, + DATA_FRAME_CONFIG_ERROR_UNCERTAINTY = 0x51, }; @@ -160,7 +162,7 @@ struct FilterStatusFlags : Bitfield FilterStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } FilterStatusFlags& operator=(uint16_t val) { value = val; return *this; } - FilterStatusFlags& operator=(int val) { value = val; return *this; } + FilterStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } FilterStatusFlags& operator|=(uint16_t val) { return *this = value | val; } FilterStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -227,12 +229,18 @@ struct FilterStatusFlags : Bitfield enum class FilterAidingMeasurementType : uint8_t { - GNSS = 1, ///< - DUAL_ANTENNA = 2, ///< - HEADING = 3, ///< - PRESSURE = 4, ///< - MAGNETOMETER = 5, ///< - SPEED = 6, ///< + GNSS = 1, ///< + DUAL_ANTENNA = 2, ///< + HEADING = 3, ///< + PRESSURE = 4, ///< + MAGNETOMETER = 5, ///< + SPEED = 6, ///< + POS_ECEF = 33, ///< + POS_LLH = 34, ///< + VEL_ECEF = 40, ///< + VEL_NED = 41, ///< + VEL_VEHICLE_FRAME = 42, ///< + HEADING_TRUE = 49, ///< }; struct FilterMeasurementIndicator : Bitfield @@ -254,7 +262,7 @@ struct FilterMeasurementIndicator : Bitfield FilterMeasurementIndicator(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } FilterMeasurementIndicator& operator=(uint8_t val) { value = val; return *this; } - FilterMeasurementIndicator& operator=(int val) { value = val; return *this; } + FilterMeasurementIndicator& operator=(int val) { value = uint8_t(val); return *this; } FilterMeasurementIndicator& operator|=(uint8_t val) { return *this = value | val; } FilterMeasurementIndicator& operator&=(uint8_t val) { return *this = value & val; } @@ -304,7 +312,7 @@ struct GnssAidStatusFlags : Bitfield GnssAidStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } GnssAidStatusFlags& operator=(uint16_t val) { value = val; return *this; } - GnssAidStatusFlags& operator=(int val) { value = val; return *this; } + GnssAidStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } GnssAidStatusFlags& operator|=(uint16_t val) { return *this = value | val; } GnssAidStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -363,8 +371,9 @@ struct PositionLlh double ellipsoid_height = 0; ///< [meters] uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -396,8 +405,9 @@ struct VelocityNed float down = 0; ///< [meters/second] uint16_t valid_flags = 0; ///< 0 - Invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -435,8 +445,9 @@ struct AttitudeQuaternion Quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -476,8 +487,9 @@ struct AttitudeDcm Matrix3f dcm; ///< Matrix elements in row-major order. uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -510,8 +522,9 @@ struct EulerAngles float yaw = 0; ///< [radians] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -541,8 +554,9 @@ struct GyroBias Vector3f bias; ///< (x, y, z) [radians/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -572,8 +586,9 @@ struct AccelBias Vector3f bias; ///< (x, y, z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -605,8 +620,9 @@ struct PositionLlhUncertainty float down = 0; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -638,8 +654,9 @@ struct VelocityNedUncertainty float down = 0; ///< [meters/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -672,8 +689,9 @@ struct EulerAnglesUncertainty float yaw = 0; ///< [radians] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -703,8 +721,9 @@ struct GyroBiasUncertainty Vector3f bias_uncert; ///< (x,y,z) [radians/sec] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -734,8 +753,9 @@ struct AccelBiasUncertainty Vector3f bias_uncert; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -772,8 +792,9 @@ struct Timestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -804,8 +825,9 @@ struct Status FilterDynamicsMode dynamics_mode = static_cast(0); ///< Device-specific dynamics mode. Please consult the user manual for definition. FilterStatusFlags status_flags; ///< Device-specific status flags. Please consult the user manual for definition. - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -836,8 +858,9 @@ struct LinearAccel Vector3f accel; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -867,8 +890,9 @@ struct GravityVector Vector3f gravity; ///< (x, y, z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -898,8 +922,9 @@ struct CompAccel Vector3f accel; ///< (x,y,z) [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -929,8 +954,9 @@ struct CompAngularRate Vector3f gyro; ///< (x, y, z) [radians/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -960,8 +986,9 @@ struct QuaternionAttitudeUncertainty Quatf q; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -991,8 +1018,9 @@ struct Wgs84GravityMag float magnitude = 0; ///< [meters/second^2] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1036,8 +1064,9 @@ struct HeadingUpdateState HeadingSource source = static_cast(0); uint16_t valid_flags = 0; ///< 1 if a valid heading update was received in 2 seconds, 0 otherwise. - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1072,8 +1101,9 @@ struct MagneticModel float declination = 0; ///< [radians] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1103,8 +1133,9 @@ struct AccelScaleFactor Vector3f scale_factor; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1134,8 +1165,9 @@ struct AccelScaleFactorUncertainty Vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1165,8 +1197,9 @@ struct GyroScaleFactor Vector3f scale_factor; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1196,8 +1229,9 @@ struct GyroScaleFactorUncertainty Vector3f scale_factor_uncert; ///< (x,y,z) [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1227,8 +1261,9 @@ struct MagBias Vector3f bias; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1258,8 +1293,9 @@ struct MagBiasUncertainty Vector3f bias_uncert; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1295,8 +1331,9 @@ struct StandardAtmosphere float standard_density = 0; ///< [kilogram/meter^3] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1330,8 +1367,9 @@ struct PressureAltitude float pressure_altitude = 0; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1360,8 +1398,9 @@ struct DensityAltitude float density_altitude = 0; ///< m uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1393,8 +1432,9 @@ struct AntennaOffsetCorrection Vector3f offset; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1424,8 +1464,9 @@ struct AntennaOffsetCorrectionUncertainty Vector3f offset_uncert; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1458,8 +1499,9 @@ struct MultiAntennaOffsetCorrection Vector3f offset; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1490,8 +1532,9 @@ struct MultiAntennaOffsetCorrectionUncertainty Vector3f offset_uncert; ///< (x,y,z) [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1523,8 +1566,9 @@ struct MagnetometerOffset Vector3f hard_iron; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1556,8 +1600,9 @@ struct MagnetometerMatrix Matrix3f soft_iron; ///< Row-major [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1587,8 +1632,9 @@ struct MagnetometerOffsetUncertainty Vector3f hard_iron_uncertainty; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1618,8 +1664,9 @@ struct MagnetometerMatrixUncertainty Matrix3f soft_iron_uncertainty; ///< Row-major [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1648,8 +1695,9 @@ struct MagnetometerCovarianceMatrix Matrix3f covariance; uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1679,8 +1727,9 @@ struct MagnetometerResidualVector Vector3f residual; ///< (x,y,z) [Gauss] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1712,8 +1761,9 @@ struct ClockCorrection float bias_drift = 0; ///< [seconds/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1745,8 +1795,9 @@ struct ClockCorrectionUncertainty float bias_drift_uncertainty = 0; ///< [seconds/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1778,8 +1829,9 @@ struct GnssPosAidStatus GnssAidStatusFlags status; ///< Aiding measurement status bitfield uint8_t reserved[8] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1810,8 +1862,9 @@ struct GnssAttAidStatus GnssAidStatusFlags status; ///< Last valid aiding measurement status bitfield uint8_t reserved[8] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1848,8 +1901,9 @@ struct HeadAidStatus HeadingAidType type = static_cast(0); ///< 1 - Dual antenna, 2 - External heading message (user supplied) float reserved[2] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1879,8 +1933,9 @@ struct RelPosNed Vector3d relative_position; ///< [meters, NED] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1910,8 +1965,9 @@ struct EcefPos Vector3d position_ecef; ///< [meters, ECEF] uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1941,8 +1997,9 @@ struct EcefVel Vector3f velocity_ecef; ///< [meters/second, ECEF] uint16_t valid_flags = 0; ///< 0 - invalid, 1 valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1972,8 +2029,9 @@ struct EcefPosUncertainty Vector3f pos_uncertainty; ///< [meters] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2003,8 +2061,9 @@ struct EcefVelUncertainty Vector3f vel_uncertainty; ///< [meters/second] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2033,11 +2092,12 @@ struct AidingMeasurementSummary { float time_of_week = 0; ///< [seconds] uint8_t source = 0; - FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) + FilterAidingMeasurementType type = static_cast(0); ///< (see product manual for supported types) Note: values 0x20 and above correspond to commanded aiding measurements in the 0x13 Aiding command set. FilterMeasurementIndicator indicator; - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2067,8 +2127,9 @@ struct OdometerScaleFactorError float scale_factor_error = 0; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2098,8 +2159,9 @@ struct OdometerScaleFactorErrorUncertainty float scale_factor_error_uncertainty = 0; ///< [dimensionless] uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2149,7 +2211,7 @@ struct GnssDualAntennaStatus DualAntennaStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } DualAntennaStatusFlags& operator=(uint16_t val) { value = val; return *this; } - DualAntennaStatusFlags& operator=(int val) { value = val; return *this; } + DualAntennaStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } DualAntennaStatusFlags& operator|=(uint16_t val) { return *this = value | val; } DualAntennaStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2171,8 +2233,9 @@ struct GnssDualAntennaStatus DualAntennaStatusFlags status_flags; uint16_t valid_flags = 0; ///< 0 - invalid, 1 - valid - static const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2189,6 +2252,76 @@ void insert(Serializer& serializer, const GnssDualAntennaStatus& self); void extract(Serializer& serializer, GnssDualAntennaStatus& self); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_aiding_frame_config_error (0x82,0x50) Aiding Frame Config Error [CPP] +/// Filter reported aiding source frame configuration error +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct AidingFrameConfigError +{ + uint8_t frame_id = 0; ///< Frame ID for the receiver to which the antenna is attached + Vector3f translation; ///< Translation config X, Y, and Z (m). + Quatf attitude; ///< Attitude quaternion + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + + auto as_tuple() const + { + return std::make_tuple(frame_id,translation[0],translation[1],translation[2],attitude[0],attitude[1],attitude[2],attitude[3]); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(frame_id),std::ref(translation[0]),std::ref(translation[1]),std::ref(translation[2]),std::ref(attitude[0]),std::ref(attitude[1]),std::ref(attitude[2]),std::ref(attitude[3])); + } +}; +void insert(Serializer& serializer, const AidingFrameConfigError& self); +void extract(Serializer& serializer, AidingFrameConfigError& self); + + +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_filter_aiding_frame_config_error_uncertainty (0x82,0x51) Aiding Frame Config Error Uncertainty [CPP] +/// Filter reported aiding source frame configuration error uncertainty +/// +/// These estimates are used to compensate for small errors to the user-supplied aiding frame configurations (set with (0x13, 0x01) command ). +/// +///@{ + +struct AidingFrameConfigErrorUncertainty +{ + uint8_t frame_id = 0; ///< Frame ID for the receiver to which the antenna is attached + Vector3f translation_unc; ///< Translation uncertaint X, Y, and Z (m). + Vector3f attitude_unc; ///< Attitude uncertainty, X, Y, and Z (radians). + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR_UNCERTAINTY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + + + auto as_tuple() const + { + return std::make_tuple(frame_id,translation_unc[0],translation_unc[1],translation_unc[2],attitude_unc[0],attitude_unc[1],attitude_unc[2]); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(frame_id),std::ref(translation_unc[0]),std::ref(translation_unc[1]),std::ref(translation_unc[2]),std::ref(attitude_unc[0]),std::ref(attitude_unc[1]),std::ref(attitude_unc[2])); + } +}; +void insert(Serializer& serializer, const AidingFrameConfigErrorUncertainty& self); +void extract(Serializer& serializer, AidingFrameConfigErrorUncertainty& self); + + ///@} /// diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index 9417718d7..60b9a6035 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -368,12 +368,12 @@ static const mip_gnss_utc_time_data_valid_flags MIP_GNSS_UTC_TIME_DATA_VALID_FLA struct mip_gnss_utc_time_data { uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint32_t msec; ///< [Milliseconds] + uint8_t month; ///< Month (1-12) + uint8_t day; ///< Day (1-31) + uint8_t hour; ///< Hour (0-23) + uint8_t min; ///< Minute (0-59) + uint8_t sec; ///< Second (0-59) + uint32_t msec; ///< Millisecond(0-999) mip_gnss_utc_time_data_valid_flags valid_flags; }; @@ -1029,7 +1029,7 @@ struct mip_gnss_rtk_corrections_status_data double time_of_week; ///< GPS Time of week [seconds] uint16_t week_number; ///< GPS Week since 1980 [weeks] mip_gnss_rtk_corrections_status_data_epoch_status epoch_status; ///< Status of the corrections received during this epoch - uint32_t dongle_status; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see MIP_CMD_DESC_RTK_GET_STATUS_FLAGS for details) + uint32_t dongle_status; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see Get RTK Device Status Flags (0x0F,0x01) for details) float gps_correction_latency; ///< Latency of last GPS correction [seconds] float glonass_correction_latency; ///< Latency of last GLONASS correction [seconds] float galileo_correction_latency; ///< Latency of last Galileo correction [seconds] diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 9a52a8644..7b5fee4b8 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -68,11 +68,11 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint8_t MIP_GNSS1_DATA_DESC_SET = 0x91; -static const uint8_t MIP_GNSS2_DATA_DESC_SET = 0x92; -static const uint8_t MIP_GNSS3_DATA_DESC_SET = 0x93; -static const uint8_t MIP_GNSS4_DATA_DESC_SET = 0x94; -static const uint8_t MIP_GNSS5_DATA_DESC_SET = 0x95; +static constexpr const uint8_t MIP_GNSS1_DATA_DESC_SET = 0x91; +static constexpr const uint8_t MIP_GNSS2_DATA_DESC_SET = 0x92; +static constexpr const uint8_t MIP_GNSS3_DATA_DESC_SET = 0x93; +static constexpr const uint8_t MIP_GNSS4_DATA_DESC_SET = 0x94; +static constexpr const uint8_t MIP_GNSS5_DATA_DESC_SET = 0x95; enum class GnssConstellationId : uint8_t { UNKNOWN = 0, ///< @@ -161,8 +161,8 @@ enum class SbasSystem : uint8_t GAGAN = 4, ///< }; -static const uint32_t GNSS_DGPS_INFO_MAX_CHANNEL_NUMBER = 32; -static const uint32_t GNSS_SV_INFO_MAX_SV_NUMBER = 32; +static constexpr const uint32_t GNSS_DGPS_INFO_MAX_CHANNEL_NUMBER = 32; +static constexpr const uint32_t GNSS_SV_INFO_MAX_SV_NUMBER = 32; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -195,7 +195,7 @@ struct PosLlh ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -224,8 +224,9 @@ struct PosLlh float vertical_accuracy = 0; ///< [meters] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -268,7 +269,7 @@ struct PosEcef ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -287,8 +288,9 @@ struct PosEcef float x_accuracy = 0; ///< [meters] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -335,7 +337,7 @@ struct VelNed ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -366,8 +368,9 @@ struct VelNed float heading_accuracy = 0; ///< [degrees] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -410,7 +413,7 @@ struct VelEcef ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -429,8 +432,9 @@ struct VelEcef float v_accuracy = 0; ///< [meters/second] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -478,7 +482,7 @@ struct Dop ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -512,8 +516,9 @@ struct Dop float edop = 0; ///< Easting DOP ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -556,7 +561,7 @@ struct UtcTime ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -572,16 +577,17 @@ struct UtcTime }; uint16_t year = 0; - uint8_t month = 0; - uint8_t day = 0; - uint8_t hour = 0; - uint8_t min = 0; - uint8_t sec = 0; - uint32_t msec = 0; ///< [Milliseconds] + uint8_t month = 0; ///< Month (1-12) + uint8_t day = 0; ///< Day (1-31) + uint8_t hour = 0; ///< Hour (0-23) + uint8_t min = 0; ///< Minute (0-59) + uint8_t sec = 0; ///< Second (0-59) + uint32_t msec = 0; ///< Millisecond(0-999) ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -624,7 +630,7 @@ struct GpsTime ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -643,8 +649,9 @@ struct GpsTime uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -688,7 +695,7 @@ struct ClockInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -710,8 +717,9 @@ struct ClockInfo double accuracy_estimate = 0; ///< [seconds] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -764,7 +772,7 @@ struct FixInfo FixFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } FixFlags& operator=(uint16_t val) { value = val; return *this; } - FixFlags& operator=(int val) { value = val; return *this; } + FixFlags& operator=(int val) { value = uint16_t(val); return *this; } FixFlags& operator|=(uint16_t val) { return *this = value | val; } FixFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -794,7 +802,7 @@ struct FixInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -816,8 +824,9 @@ struct FixInfo FixFlags fix_flags; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -861,7 +870,7 @@ struct SvInfo SVFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } SVFlags& operator=(uint16_t val) { value = val; return *this; } - SVFlags& operator=(int val) { value = val; return *this; } + SVFlags& operator=(int val) { value = uint16_t(val); return *this; } SVFlags& operator|=(uint16_t val) { return *this = value | val; } SVFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -894,7 +903,7 @@ struct SvInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -925,8 +934,9 @@ struct SvInfo SVFlags sv_flags; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -993,7 +1003,7 @@ struct HwStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1015,8 +1025,9 @@ struct HwStatus AntennaPower antenna_power = static_cast(0); ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1073,7 +1084,7 @@ struct DgpsInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1098,8 +1109,9 @@ struct DgpsInfo float range_rate_correction = 0; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1146,7 +1158,7 @@ struct DgpsChannel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1171,8 +1183,9 @@ struct DgpsChannel float range_rate_correction = 0; ///< [m/s] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1219,7 +1232,7 @@ struct ClockInfo2 ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1244,8 +1257,9 @@ struct ClockInfo2 double drift_accuracy_estimate = 0; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1286,7 +1300,7 @@ struct GpsLeapSeconds ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1300,8 +1314,9 @@ struct GpsLeapSeconds uint8_t leap_seconds = 0; ///< [s] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1345,7 +1360,7 @@ struct SbasInfo SbasStatus(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } SbasStatus& operator=(uint8_t val) { value = val; return *this; } - SbasStatus& operator=(int val) { value = val; return *this; } + SbasStatus& operator=(int val) { value = uint8_t(val); return *this; } SbasStatus& operator|=(uint8_t val) { return *this = value | val; } SbasStatus& operator&=(uint8_t val) { return *this = value & val; } @@ -1382,7 +1397,7 @@ struct SbasInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1413,8 +1428,9 @@ struct SbasInfo SbasStatus sbas_status; ///< Status of the SBAS service ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1480,7 +1496,7 @@ struct SbasCorrection ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1508,8 +1524,9 @@ struct SbasCorrection float iono_correction = 0; ///< Ionospheric correction [meters]. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1577,7 +1594,7 @@ struct RfErrorDetection ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1600,8 +1617,9 @@ struct RfErrorDetection uint8_t reserved[4] = {0}; ///< Reserved for future use ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1652,7 +1670,7 @@ struct BaseStationInfo IndicatorFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } IndicatorFlags& operator=(uint16_t val) { value = val; return *this; } - IndicatorFlags& operator=(int val) { value = val; return *this; } + IndicatorFlags& operator=(int val) { value = uint16_t(val); return *this; } IndicatorFlags& operator|=(uint16_t val) { return *this = value | val; } IndicatorFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1699,7 +1717,7 @@ struct BaseStationInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1730,8 +1748,9 @@ struct BaseStationInfo IndicatorFlags indicators; ///< Bitfield ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1779,7 +1798,7 @@ struct RtkCorrectionsStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1828,7 +1847,7 @@ struct RtkCorrectionsStatus EpochStatus(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } EpochStatus& operator=(uint16_t val) { value = val; return *this; } - EpochStatus& operator=(int val) { value = val; return *this; } + EpochStatus& operator=(int val) { value = uint16_t(val); return *this; } EpochStatus& operator|=(uint16_t val) { return *this = value | val; } EpochStatus& operator&=(uint16_t val) { return *this = value & val; } @@ -1858,7 +1877,7 @@ struct RtkCorrectionsStatus double time_of_week = 0; ///< GPS Time of week [seconds] uint16_t week_number = 0; ///< GPS Week since 1980 [weeks] EpochStatus epoch_status; ///< Status of the corrections received during this epoch - uint32_t dongle_status = 0; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see MIP_CMD_DESC_RTK_GET_STATUS_FLAGS for details) + uint32_t dongle_status = 0; ///< RTK Dongle Status Flags (valid only when using RTK dongle, see Get RTK Device Status Flags (0x0F,0x01) for details) float gps_correction_latency = 0; ///< Latency of last GPS correction [seconds] float glonass_correction_latency = 0; ///< Latency of last GLONASS correction [seconds] float galileo_correction_latency = 0; ///< Latency of last Galileo correction [seconds] @@ -1866,8 +1885,9 @@ struct RtkCorrectionsStatus uint32_t reserved[4] = {0}; ///< Reserved for future use ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -1915,7 +1935,7 @@ struct SatelliteStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1951,8 +1971,9 @@ struct SatelliteStatus bool health = 0; ///< True if the satellite is healthy. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2019,7 +2040,7 @@ struct Raw ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2082,8 +2103,9 @@ struct Raw float lock_time = 0; ///< DOC Minimum carrier phase lock time [s]. Note: the maximum value is dependent on the receiver. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2126,7 +2148,7 @@ struct GpsEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2176,8 +2198,9 @@ struct GpsEphemeris double c_rs = 0; ///< Harmonic Correction Term. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2220,7 +2243,7 @@ struct GalileoEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2270,8 +2293,9 @@ struct GalileoEphemeris double c_rs = 0; ///< Harmonic Correction Term. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2313,7 +2337,7 @@ struct GloEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2352,8 +2376,9 @@ struct GloEphemeris uint8_t P4 = 0; ///< Flag indicating ephemeris parameters are present ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2398,7 +2423,7 @@ struct GpsIonoCorr ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2423,8 +2448,9 @@ struct GpsIonoCorr double beta[4] = {0}; ///< Ionospheric Correction Terms. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -2469,7 +2495,7 @@ struct GalileoIonoCorr ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2494,8 +2520,9 @@ struct GalileoIonoCorr uint8_t disturbance_flags = 0; ///< Region disturbance flags (bits 1-5). ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 5e3142f72..10fee544f 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -81,8 +81,9 @@ struct RawAccel { Vector3f raw_accel; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -112,8 +113,9 @@ struct RawGyro { Vector3f raw_gyro; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -143,8 +145,9 @@ struct RawMag { Vector3f raw_mag; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -174,8 +177,9 @@ struct RawPressure { float raw_pressure = 0; ///< Native sensor counts - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -205,8 +209,9 @@ struct ScaledAccel { Vector3f scaled_accel; ///< (x, y, z)[g] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -236,8 +241,9 @@ struct ScaledGyro { Vector3f scaled_gyro; ///< (x, y, z) [radians/second] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -267,8 +273,9 @@ struct ScaledMag { Vector3f scaled_mag; ///< (x, y, z) [Gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -297,8 +304,9 @@ struct ScaledPressure { float scaled_pressure = 0; ///< [mBar] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -328,8 +336,9 @@ struct DeltaTheta { Vector3f delta_theta; ///< (x, y, z) [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -359,8 +368,9 @@ struct DeltaVelocity { Vector3f delta_velocity; ///< (x, y, z) [g*sec] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -399,8 +409,9 @@ struct CompOrientationMatrix { Matrix3f m; ///< Matrix elements in row-major order. - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -437,8 +448,9 @@ struct CompQuaternion { Quatf q; ///< Quaternion elements EQSTART q = (q_w, q_x, q_y, q_z) EQEND - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -470,8 +482,9 @@ struct CompEulerAngles float pitch = 0; ///< [radians] float yaw = 0; ///< [radians] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -500,8 +513,9 @@ struct CompOrientationUpdateMatrix { Matrix3f m; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -530,8 +544,9 @@ struct OrientationRawTemp { uint16_t raw_temp[4] = {0}; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -560,8 +575,9 @@ struct InternalTimestamp { uint32_t counts = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -591,8 +607,9 @@ struct PpsTimestamp uint32_t seconds = 0; uint32_t useconds = 0; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -643,7 +660,7 @@ struct GpsTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -666,8 +683,9 @@ struct GpsTimestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -702,8 +720,9 @@ struct TemperatureAbs float max_temp = 0; ///< [degC] float mean_temp = 0; ///< [degC] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -738,8 +757,9 @@ struct UpVector { Vector3f up; ///< [Gs] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -771,8 +791,9 @@ struct NorthVector { Vector3f north; ///< [Gauss] - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -821,7 +842,7 @@ struct OverrangeStatus Status(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } Status& operator=(uint16_t val) { value = val; return *this; } - Status& operator=(int val) { value = val; return *this; } + Status& operator=(int val) { value = uint16_t(val); return *this; } Status& operator|=(uint16_t val) { return *this = value | val; } Status& operator&=(uint16_t val) { return *this = value & val; } @@ -852,8 +873,9 @@ struct OverrangeStatus Status status; - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -883,8 +905,9 @@ struct OdometerData float uncertainty = 0; ///< Uncertainty of velocity [m/s]. uint16_t valid_flags = 0; ///< If odometer is configured, bit 0 will be set to 1. - static const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 0f26f11fb..5b44446e8 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -49,7 +49,7 @@ enum // Shared Type Definitions //////////////////////////////////////////////////////////////////////////////// -static const uint8_t MIP_DATA_DESC_SHARED_START = 0xD0; +static constexpr const uint8_t MIP_DATA_DESC_SHARED_START = 0xD0; //////////////////////////////////////////////////////////////////////////////// // Mip Fields @@ -68,8 +68,9 @@ struct EventSource { uint8_t trigger_id = 0; ///< Trigger ID number. If 0, this message was emitted due to being scheduled in the 3DM Message Format Command (0x0C,0x0F). - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -101,8 +102,9 @@ struct Ticks { uint32_t ticks = 0; ///< Ticks since powerup. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -135,8 +137,9 @@ struct DeltaTicks { uint32_t ticks = 0; ///< Ticks since last output. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -182,7 +185,7 @@ struct GpsTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -201,8 +204,9 @@ struct GpsTimestamp uint16_t week_number = 0; ///< GPS Week Number since 1980 [weeks] ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -240,8 +244,9 @@ struct DeltaTime { double seconds = 0; ///< Seconds since last output. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -277,8 +282,9 @@ struct ReferenceTimestamp { uint64_t nanoseconds = 0; ///< Nanoseconds since initialization. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -316,8 +322,9 @@ struct ReferenceTimeDelta { uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -366,7 +373,7 @@ struct ExternalTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -380,8 +387,9 @@ struct ExternalTimestamp uint64_t nanoseconds = 0; ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -434,7 +442,7 @@ struct ExternalTimeDelta ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -448,8 +456,9 @@ struct ExternalTimeDelta uint64_t dt_nanos = 0; ///< Nanoseconds since the last occurrence of this field in a packet of the same descriptor set and event source. ValidFlags valid_flags; - static const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 65982acab..80b457773 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -76,8 +76,9 @@ struct BuiltInTest { uint8_t result[16] = {0}; ///< Device-specific bitfield (128 bits). See device user manual. Bits are least-significant-byte first. For example, bit 0 is located at bit 0 of result[0], bit 1 is located at bit 1 of result[0], bit 8 is located at bit 0 of result[1], and bit 127 is located at bit 7 of result[15]. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -107,8 +108,9 @@ struct TimeSyncStatus bool time_sync = 0; ///< True if sync with the PPS signal is currently valid. False if PPS feature is disabled or a PPS signal is not detected. uint8_t last_pps_rcvd = 0; ///< Elapsed time in seconds since last PPS was received, with a maximum value of 255. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -155,8 +157,9 @@ struct GpioState { uint8_t states = 0; ///< Bitfield containing the states for each GPIO pin.
Bit 0 (0x01): pin 1
Bit 1 (0x02): pin 2
Bit 2 (0x04): pin 3
Bit 3 (0x08): pin 4
Bits for pins that don't exist will read as 0. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const @@ -187,8 +190,9 @@ struct GpioAnalogValue uint8_t gpio_id = 0; ///< GPIO pin number starting with 1. float value = 0; ///< Value of the GPIO line in scaled volts. - static const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; - static const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; auto as_tuple() const From e803ef1473ead415baa980e136ab4ae71d4ef3ac Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 5 Feb 2024 19:56:41 +0000 Subject: [PATCH 201/252] Generate MIP definitions from branches/ResultDescriptor@55936. --- src/mip/definitions/commands_aiding.cpp | 54 +++++++++++----------- src/mip/definitions/commands_aiding.hpp | 60 ++++++++++++------------- 2 files changed, 57 insertions(+), 57 deletions(-) diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index a5eb60002..9981eb802 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -88,7 +88,7 @@ void extract(Serializer& serializer, SensorFrameMapping::Response& self) } -CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) +TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -102,7 +102,7 @@ CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) +TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -111,7 +111,7 @@ CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -128,7 +128,7 @@ CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, } return result; } -CmdResult saveSensorFrameMapping(C::mip_interface& device) +TypedResult saveSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -138,7 +138,7 @@ CmdResult saveSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorFrameMapping(C::mip_interface& device) +TypedResult loadSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -148,7 +148,7 @@ CmdResult loadSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorFrameMapping(C::mip_interface& device) +TypedResult defaultSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -256,7 +256,7 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) } } -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) +TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -284,7 +284,7 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) +TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -297,7 +297,7 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -326,7 +326,7 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc } return result; } -CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -338,7 +338,7 @@ CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -350,7 +350,7 @@ CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -394,7 +394,7 @@ void extract(Serializer& serializer, AidingEchoControl::Response& self) } -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) +TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -406,7 +406,7 @@ CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) +TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -415,7 +415,7 @@ CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mod assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -429,7 +429,7 @@ CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mod } return result; } -CmdResult saveAidingEchoControl(C::mip_interface& device) +TypedResult saveAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -439,7 +439,7 @@ CmdResult saveAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAidingEchoControl(C::mip_interface& device) +TypedResult loadAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -449,7 +449,7 @@ CmdResult loadAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAidingEchoControl(C::mip_interface& device) +TypedResult defaultAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -490,7 +490,7 @@ void extract(Serializer& serializer, EcefPos& self) } -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -550,7 +550,7 @@ void extract(Serializer& serializer, LlhPos& self) } -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -602,7 +602,7 @@ void extract(Serializer& serializer, Height& self) } -CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) +TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -648,7 +648,7 @@ void extract(Serializer& serializer, Pressure& self) } -CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -698,7 +698,7 @@ void extract(Serializer& serializer, EcefVel& self) } -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -752,7 +752,7 @@ void extract(Serializer& serializer, NedVel& self) } -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -806,7 +806,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -856,7 +856,7 @@ void extract(Serializer& serializer, TrueHeading& self) } -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -906,7 +906,7 @@ void extract(Serializer& serializer, MagneticField& self) } -CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 5a72202c9..ea8dc7345 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -148,11 +148,11 @@ void extract(Serializer& serializer, SensorFrameMapping& self); void insert(Serializer& serializer, const SensorFrameMapping::Response& self); void extract(Serializer& serializer, SensorFrameMapping::Response& self); -CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); -CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); -CmdResult saveSensorFrameMapping(C::mip_interface& device); -CmdResult loadSensorFrameMapping(C::mip_interface& device); -CmdResult defaultSensorFrameMapping(C::mip_interface& device); +TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); +TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); +TypedResult saveSensorFrameMapping(C::mip_interface& device); +TypedResult loadSensorFrameMapping(C::mip_interface& device); +TypedResult defaultSensorFrameMapping(C::mip_interface& device); ///@} /// @@ -261,11 +261,11 @@ void extract(Serializer& serializer, ReferenceFrame& self); void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); -CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); -CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); -CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); +TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); +TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); ///@} /// @@ -340,11 +340,11 @@ void extract(Serializer& serializer, AidingEchoControl& self); void insert(Serializer& serializer, const AidingEchoControl::Response& self); void extract(Serializer& serializer, AidingEchoControl::Response& self); -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); -CmdResult saveAidingEchoControl(C::mip_interface& device); -CmdResult loadAidingEchoControl(C::mip_interface& device); -CmdResult defaultAidingEchoControl(C::mip_interface& device); +TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); +TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); +TypedResult saveAidingEchoControl(C::mip_interface& device); +TypedResult loadAidingEchoControl(C::mip_interface& device); +TypedResult defaultAidingEchoControl(C::mip_interface& device); ///@} /// @@ -372,7 +372,7 @@ struct EcefPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -414,7 +414,7 @@ struct EcefPos void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); ///@} /// @@ -443,7 +443,7 @@ struct LlhPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -487,7 +487,7 @@ struct LlhPos void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); ///@} /// @@ -526,7 +526,7 @@ struct Height void insert(Serializer& serializer, const Height& self); void extract(Serializer& serializer, Height& self); -CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); +TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); ///@} /// @@ -565,7 +565,7 @@ struct Pressure void insert(Serializer& serializer, const Pressure& self); void extract(Serializer& serializer, Pressure& self); -CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); ///@} /// @@ -593,7 +593,7 @@ struct EcefVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -635,7 +635,7 @@ struct EcefVel void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); ///@} /// @@ -663,7 +663,7 @@ struct NedVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -705,7 +705,7 @@ struct NedVel void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); ///@} /// @@ -734,7 +734,7 @@ struct VehicleFixedFrameVelocity ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -776,7 +776,7 @@ struct VehicleFixedFrameVelocity void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); ///@} /// @@ -814,7 +814,7 @@ struct TrueHeading void insert(Serializer& serializer, const TrueHeading& self); void extract(Serializer& serializer, TrueHeading& self); -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); ///@} /// @@ -842,7 +842,7 @@ struct MagneticField ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -884,7 +884,7 @@ struct MagneticField void insert(Serializer& serializer, const MagneticField& self); void extract(Serializer& serializer, MagneticField& self); -CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); ///@} /// From 0c9a3a1a3ac58ef26c38716533ca9ecfaea61758 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 9 Feb 2024 20:41:05 +0000 Subject: [PATCH 202/252] Generate MIP definitions from branches/ResultDescriptor@55983. --- src/mip/definitions/commands_3dm.hpp | 170 +++++++++++++++++++++ src/mip/definitions/commands_aiding.hpp | 30 ++++ src/mip/definitions/commands_base.hpp | 34 +++++ src/mip/definitions/commands_filter.hpp | 190 ++++++++++++++++++++++++ src/mip/definitions/commands_gnss.hpp | 12 ++ src/mip/definitions/commands_rtk.hpp | 42 ++++++ src/mip/definitions/commands_system.hpp | 4 + src/mip/definitions/data_filter.hpp | 114 ++++++++++++++ src/mip/definitions/data_gnss.hpp | 54 +++++++ src/mip/definitions/data_sensor.hpp | 46 ++++++ src/mip/definitions/data_shared.hpp | 18 +++ src/mip/definitions/data_system.hpp | 8 + 12 files changed, 722 insertions(+) diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 9317e4080..28987157f 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -213,6 +213,8 @@ struct PollImuMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollImuMessage"; + static constexpr const char* DOC_NAME = "PollImuMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -257,6 +259,8 @@ struct PollGnssMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollGnssMessage"; + static constexpr const char* DOC_NAME = "PollGnssMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -301,6 +305,8 @@ struct PollFilterMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollFilterMessage"; + static constexpr const char* DOC_NAME = "PollFilterMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -340,6 +346,8 @@ struct ImuMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuMessageFormat"; + static constexpr const char* DOC_NAME = "ImuMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -372,6 +380,8 @@ struct ImuMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuMessageFormat::Response"; + static constexpr const char* DOC_NAME = "ImuMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -416,6 +426,8 @@ struct GpsMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsMessageFormat"; + static constexpr const char* DOC_NAME = "GpsMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -448,6 +460,8 @@ struct GpsMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsMessageFormat::Response"; + static constexpr const char* DOC_NAME = "GpsMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -492,6 +506,8 @@ struct FilterMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterMessageFormat"; + static constexpr const char* DOC_NAME = "FilterMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -524,6 +540,8 @@ struct FilterMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterMessageFormat::Response"; + static constexpr const char* DOC_NAME = "FilterMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -566,6 +584,8 @@ struct ImuGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuGetBaseRate"; + static constexpr const char* DOC_NAME = "Get IMU Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -585,6 +605,8 @@ struct ImuGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuGetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get IMU Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -622,6 +644,8 @@ struct GpsGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsGetBaseRate"; + static constexpr const char* DOC_NAME = "Get GNSS Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -641,6 +665,8 @@ struct GpsGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsGetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get GNSS Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -678,6 +704,8 @@ struct FilterGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterGetBaseRate"; + static constexpr const char* DOC_NAME = "Get Estimation Filter Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -697,6 +725,8 @@ struct FilterGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterGetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get Estimation Filter Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -742,6 +772,8 @@ struct PollData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollData"; + static constexpr const char* DOC_NAME = "PollData"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -777,6 +809,8 @@ struct GetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetBaseRate"; + static constexpr const char* DOC_NAME = "Get Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -796,6 +830,8 @@ struct GetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -837,6 +873,8 @@ struct MessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MessageFormat"; + static constexpr const char* DOC_NAME = "MessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -870,6 +908,8 @@ struct MessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MessageFormat::Response"; + static constexpr const char* DOC_NAME = "MessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -919,6 +959,8 @@ struct NmeaPollData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NmeaPollData"; + static constexpr const char* DOC_NAME = "NmeaPollData"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -956,6 +998,8 @@ struct NmeaMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NmeaMessageFormat"; + static constexpr const char* DOC_NAME = "NmeaMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -988,6 +1032,8 @@ struct NmeaMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NmeaMessageFormat::Response"; + static constexpr const char* DOC_NAME = "NmeaMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -1032,6 +1078,8 @@ struct DeviceSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeviceSettings"; + static constexpr const char* DOC_NAME = "DeviceSettings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x0000; @@ -1097,6 +1145,8 @@ struct UartBaudrate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UartBaudrate"; + static constexpr const char* DOC_NAME = "UartBaudrate"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1129,6 +1179,8 @@ struct UartBaudrate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UartBaudrate::Response"; + static constexpr const char* DOC_NAME = "UartBaudrate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1179,6 +1231,8 @@ struct FactoryStreaming static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FactoryStreaming"; + static constexpr const char* DOC_NAME = "FactoryStreaming"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1225,6 +1279,8 @@ struct DatastreamControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DatastreamControl"; + static constexpr const char* DOC_NAME = "DatastreamControl"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1258,6 +1314,8 @@ struct DatastreamControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DatastreamControl::Response"; + static constexpr const char* DOC_NAME = "DatastreamControl Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1362,6 +1420,8 @@ struct ConstellationSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConstellationSettings"; + static constexpr const char* DOC_NAME = "ConstellationSettings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -1394,6 +1454,8 @@ struct ConstellationSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConstellationSettings::Response"; + static constexpr const char* DOC_NAME = "ConstellationSettings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -1477,6 +1539,8 @@ struct GnssSbasSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSbasSettings"; + static constexpr const char* DOC_NAME = "SBAS Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -1509,6 +1573,8 @@ struct GnssSbasSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSbasSettings::Response"; + static constexpr const char* DOC_NAME = "SBAS Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -1569,6 +1635,8 @@ struct GnssAssistedFix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAssistedFix"; + static constexpr const char* DOC_NAME = "GNSS Assisted Fix Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1601,6 +1669,8 @@ struct GnssAssistedFix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAssistedFix::Response"; + static constexpr const char* DOC_NAME = "GNSS Assisted Fix Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1647,6 +1717,8 @@ struct GnssTimeAssistance static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssTimeAssistance"; + static constexpr const char* DOC_NAME = "GnssTimeAssistance"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -1679,6 +1751,8 @@ struct GnssTimeAssistance static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssTimeAssistance::Response"; + static constexpr const char* DOC_NAME = "GnssTimeAssistance Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1737,6 +1811,8 @@ struct ImuLowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuLowpassFilter"; + static constexpr const char* DOC_NAME = "Advanced Low-Pass Filter Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -1770,6 +1846,8 @@ struct ImuLowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuLowpassFilter::Response"; + static constexpr const char* DOC_NAME = "Advanced Low-Pass Filter Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1823,6 +1901,8 @@ struct PpsSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PpsSource"; + static constexpr const char* DOC_NAME = "PpsSource"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1855,6 +1935,8 @@ struct PpsSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PpsSource::Response"; + static constexpr const char* DOC_NAME = "PpsSource Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1974,6 +2056,8 @@ struct GpioConfig static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioConfig"; + static constexpr const char* DOC_NAME = "GPIO Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2007,6 +2091,8 @@ struct GpioConfig static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioConfig::Response"; + static constexpr const char* DOC_NAME = "GPIO Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2066,6 +2152,8 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioState"; + static constexpr const char* DOC_NAME = "GPIO State"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2098,6 +2186,8 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioState::Response"; + static constexpr const char* DOC_NAME = "GPIO State Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2145,6 +2235,8 @@ struct Odometer static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Odometer"; + static constexpr const char* DOC_NAME = "Odometer Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -2177,6 +2269,8 @@ struct Odometer static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Odometer::Response"; + static constexpr const char* DOC_NAME = "Odometer Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2246,6 +2340,8 @@ struct GetEventSupport static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventSupport"; + static constexpr const char* DOC_NAME = "Get Supported Events"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -2265,6 +2361,8 @@ struct GetEventSupport static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventSupport::Response"; + static constexpr const char* DOC_NAME = "Get Supported Events Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -2325,6 +2423,8 @@ struct EventControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventControl"; + static constexpr const char* DOC_NAME = "Event Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2358,6 +2458,8 @@ struct EventControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventControl::Response"; + static constexpr const char* DOC_NAME = "Event Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2435,6 +2537,8 @@ struct GetEventTriggerStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventTriggerStatus"; + static constexpr const char* DOC_NAME = "Get Trigger Status"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -2454,6 +2558,8 @@ struct GetEventTriggerStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventTriggerStatus::Response"; + static constexpr const char* DOC_NAME = "Get Trigger Status Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -2499,6 +2605,8 @@ struct GetEventActionStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventActionStatus"; + static constexpr const char* DOC_NAME = "Get Action Status"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -2518,6 +2626,8 @@ struct GetEventActionStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventActionStatus::Response"; + static constexpr const char* DOC_NAME = "Get Action Status Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -2633,6 +2743,8 @@ struct EventTrigger static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventTrigger"; + static constexpr const char* DOC_NAME = "Event Trigger Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -2666,6 +2778,8 @@ struct EventTrigger static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventTrigger::Response"; + static constexpr const char* DOC_NAME = "Event Trigger Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2758,6 +2872,8 @@ struct EventAction static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventAction"; + static constexpr const char* DOC_NAME = "Event Action Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2791,6 +2907,8 @@ struct EventAction static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventAction::Response"; + static constexpr const char* DOC_NAME = "Event Action Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2842,6 +2960,8 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBias"; + static constexpr const char* DOC_NAME = "Configure Accel Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2874,6 +2994,8 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBias::Response"; + static constexpr const char* DOC_NAME = "Configure Accel Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2916,6 +3038,8 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBias"; + static constexpr const char* DOC_NAME = "Configure Gyro Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2948,6 +3072,8 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBias::Response"; + static constexpr const char* DOC_NAME = "Configure Gyro Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2992,6 +3118,8 @@ struct CaptureGyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CaptureGyroBias"; + static constexpr const char* DOC_NAME = "Capture Gyro Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -3011,6 +3139,8 @@ struct CaptureGyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CaptureGyroBias::Response"; + static constexpr const char* DOC_NAME = "Capture Gyro Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3053,6 +3183,8 @@ struct MagHardIronOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagHardIronOffset"; + static constexpr const char* DOC_NAME = "Magnetometer Hard Iron Offset"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3085,6 +3217,8 @@ struct MagHardIronOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagHardIronOffset::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Hard Iron Offset Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3135,6 +3269,8 @@ struct MagSoftIronMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagSoftIronMatrix"; + static constexpr const char* DOC_NAME = "Magnetometer Soft Iron Matrix"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3167,6 +3303,8 @@ struct MagSoftIronMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagSoftIronMatrix::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Soft Iron Matrix Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3207,6 +3345,8 @@ struct ConingScullingEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConingScullingEnable"; + static constexpr const char* DOC_NAME = "Coning and Sculling Enable"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3239,6 +3379,8 @@ struct ConingScullingEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConingScullingEnable::Response"; + static constexpr const char* DOC_NAME = "Coning and Sculling Enable Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3305,6 +3447,8 @@ struct Sensor2VehicleTransformEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformEuler"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Euler"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3337,6 +3481,8 @@ struct Sensor2VehicleTransformEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformEuler::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Euler Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3411,6 +3557,8 @@ struct Sensor2VehicleTransformQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformQuaternion"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Quaternion"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3443,6 +3591,8 @@ struct Sensor2VehicleTransformQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformQuaternion::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Quaternion Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3513,6 +3663,8 @@ struct Sensor2VehicleTransformDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformDcm"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Direction Cosine Matrix"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3545,6 +3697,8 @@ struct Sensor2VehicleTransformDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformDcm::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Direction Cosine Matrix Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3592,6 +3746,8 @@ struct ComplementaryFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ComplementaryFilter"; + static constexpr const char* DOC_NAME = "Complementary filter settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -3624,6 +3780,8 @@ struct ComplementaryFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ComplementaryFilter::Response"; + static constexpr const char* DOC_NAME = "Complementary filter settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3675,6 +3833,8 @@ struct SensorRange static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorRange"; + static constexpr const char* DOC_NAME = "Sensor Range"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3708,6 +3868,8 @@ struct SensorRange static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorRange::Response"; + static constexpr const char* DOC_NAME = "Sensor Range Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3757,6 +3919,8 @@ struct CalibratedSensorRanges static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CalibratedSensorRanges"; + static constexpr const char* DOC_NAME = "Get Calibrated Sensor Ranges"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -3776,6 +3940,8 @@ struct CalibratedSensorRanges static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CalibratedSensorRanges::Response"; + static constexpr const char* DOC_NAME = "Get Calibrated Sensor Ranges Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -3834,6 +4000,8 @@ struct LowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LowpassFilter"; + static constexpr const char* DOC_NAME = "Low-pass anti-aliasing filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -3868,6 +4036,8 @@ struct LowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LowpassFilter::Response"; + static constexpr const char* DOC_NAME = "Low-pass anti-aliasing filter Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index ea8dc7345..28ffd2617 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -97,6 +97,8 @@ struct SensorFrameMapping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorFrameMapping"; + static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -129,6 +131,8 @@ struct SensorFrameMapping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorFrameMapping::Response"; + static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -207,6 +211,8 @@ struct ReferenceFrame static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceFrame"; + static constexpr const char* DOC_NAME = "Reference Frame Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -240,6 +246,8 @@ struct ReferenceFrame static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceFrame::Response"; + static constexpr const char* DOC_NAME = "Reference Frame Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -290,6 +298,8 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingEchoControl"; + static constexpr const char* DOC_NAME = "Aiding Command Echo Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -322,6 +332,8 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingEchoControl::Response"; + static constexpr const char* DOC_NAME = "Aiding Command Echo Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -396,6 +408,8 @@ struct EcefPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefPos"; + static constexpr const char* DOC_NAME = "ECEF Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -469,6 +483,8 @@ struct LlhPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LlhPos"; + static constexpr const char* DOC_NAME = "LLH Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -508,6 +524,8 @@ struct Height static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEIGHT_ABS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Height"; + static constexpr const char* DOC_NAME = "Height"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -547,6 +565,8 @@ struct Pressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_PRESSURE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Pressure"; + static constexpr const char* DOC_NAME = "Pressure"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -617,6 +637,8 @@ struct EcefVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefVel"; + static constexpr const char* DOC_NAME = "ECEF Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -687,6 +709,8 @@ struct NedVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NedVel"; + static constexpr const char* DOC_NAME = "NED Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -758,6 +782,8 @@ struct VehicleFixedFrameVelocity static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VehicleFixedFrameVelocity"; + static constexpr const char* DOC_NAME = "Vehicle Frame Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -796,6 +822,8 @@ struct TrueHeading static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TrueHeading"; + static constexpr const char* DOC_NAME = "True Heading"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -866,6 +894,8 @@ struct MagneticField static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_MAGNETIC_FIELD; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticField"; + static constexpr const char* DOC_NAME = "Magnetic Field"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 2b8574205..e9d1bf0c3 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -202,6 +202,8 @@ struct Ping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Ping"; + static constexpr const char* DOC_NAME = "Ping"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -240,6 +242,8 @@ struct SetIdle static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SetIdle"; + static constexpr const char* DOC_NAME = "Set to idle"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -274,6 +278,8 @@ struct GetDeviceInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceInfo"; + static constexpr const char* DOC_NAME = "Get device information"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -293,6 +299,8 @@ struct GetDeviceInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceInfo::Response"; + static constexpr const char* DOC_NAME = "Get device information Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -330,6 +338,8 @@ struct GetDeviceDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceDescriptors"; + static constexpr const char* DOC_NAME = "Get device descriptors"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -349,6 +359,8 @@ struct GetDeviceDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceDescriptors::Response"; + static constexpr const char* DOC_NAME = "Get device descriptors Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; @@ -389,6 +401,8 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BuiltInTest"; + static constexpr const char* DOC_NAME = "Built in test"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -408,6 +422,8 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BuiltInTest::Response"; + static constexpr const char* DOC_NAME = "Built in test Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -444,6 +460,8 @@ struct Resume static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Resume"; + static constexpr const char* DOC_NAME = "Resume"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -481,6 +499,8 @@ struct GetExtendedDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetExtendedDescriptors"; + static constexpr const char* DOC_NAME = "Get device descriptors (extended)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -500,6 +520,8 @@ struct GetExtendedDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetExtendedDescriptors::Response"; + static constexpr const char* DOC_NAME = "Get device descriptors (extended) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; @@ -537,6 +559,8 @@ struct ContinuousBit static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ContinuousBit"; + static constexpr const char* DOC_NAME = "Continuous built-in test"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -556,6 +580,8 @@ struct ContinuousBit static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ContinuousBit::Response"; + static constexpr const char* DOC_NAME = "Continuous built-in test Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -607,6 +633,8 @@ struct CommSpeed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommSpeed"; + static constexpr const char* DOC_NAME = "Comm Port Speed"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -640,6 +668,8 @@ struct CommSpeed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommSpeed::Response"; + static constexpr const char* DOC_NAME = "Comm Port Speed Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -691,6 +721,8 @@ struct GpsTimeUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTimeUpdate"; + static constexpr const char* DOC_NAME = "Time Broadcast Command"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -740,6 +772,8 @@ struct SoftReset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SoftReset"; + static constexpr const char* DOC_NAME = "Reset device"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index e36706b43..42a300d54 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -195,6 +195,8 @@ struct Reset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Reset"; + static constexpr const char* DOC_NAME = "Reset Navigation Filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -243,6 +245,8 @@ struct SetInitialAttitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SetInitialAttitude"; + static constexpr const char* DOC_NAME = "Set Initial Attitude"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -332,6 +336,8 @@ struct EstimationControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EstimationControl"; + static constexpr const char* DOC_NAME = "Estimation Control Flags"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -364,6 +370,8 @@ struct EstimationControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EstimationControl::Response"; + static constexpr const char* DOC_NAME = "Estimation Control Flags Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -414,6 +422,8 @@ struct ExternalGnssUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalGnssUpdate"; + static constexpr const char* DOC_NAME = "External GNSS Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -463,6 +473,8 @@ struct ExternalHeadingUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalHeadingUpdate"; + static constexpr const char* DOC_NAME = "External Heading Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -518,6 +530,8 @@ struct ExternalHeadingUpdateWithTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalHeadingUpdateWithTime"; + static constexpr const char* DOC_NAME = "External Heading Update With Time"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -589,6 +603,8 @@ struct TareOrientation static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TareOrientation"; + static constexpr const char* DOC_NAME = "Tare Sensor Orientation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -621,6 +637,8 @@ struct TareOrientation static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TareOrientation::Response"; + static constexpr const char* DOC_NAME = "Tare Sensor Orientation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -669,6 +687,8 @@ struct VehicleDynamicsMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VehicleDynamicsMode"; + static constexpr const char* DOC_NAME = "Vehicle Dynamics Mode"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -701,6 +721,8 @@ struct VehicleDynamicsMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VehicleDynamicsMode::Response"; + static constexpr const char* DOC_NAME = "Vehicle Dynamics Mode Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -765,6 +787,8 @@ struct SensorToVehicleRotationEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationEuler"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Euler"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -797,6 +821,8 @@ struct SensorToVehicleRotationEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationEuler::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Euler Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -867,6 +893,8 @@ struct SensorToVehicleRotationDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationDcm"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation DCM"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -899,6 +927,8 @@ struct SensorToVehicleRotationDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationDcm::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation DCM Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -966,6 +996,8 @@ struct SensorToVehicleRotationQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationQuaternion"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Quaternion"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -998,6 +1030,8 @@ struct SensorToVehicleRotationQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationQuaternion::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Quaternion Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1046,6 +1080,8 @@ struct SensorToVehicleOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleOffset"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Offset"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1078,6 +1114,8 @@ struct SensorToVehicleOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleOffset::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Offset Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1123,6 +1161,8 @@ struct AntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffset"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1155,6 +1195,8 @@ struct AntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffset::Response"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1207,6 +1249,8 @@ struct GnssSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSource"; + static constexpr const char* DOC_NAME = "GNSS Aiding Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1239,6 +1283,8 @@ struct GnssSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSource::Response"; + static constexpr const char* DOC_NAME = "GNSS Aiding Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1302,6 +1348,8 @@ struct HeadingSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadingSource"; + static constexpr const char* DOC_NAME = "Heading Aiding Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1334,6 +1382,8 @@ struct HeadingSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadingSource::Response"; + static constexpr const char* DOC_NAME = "Heading Aiding Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1382,6 +1432,8 @@ struct AutoInitControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoInitControl"; + static constexpr const char* DOC_NAME = "Auto-initialization Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1414,6 +1466,8 @@ struct AutoInitControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoInitControl::Response"; + static constexpr const char* DOC_NAME = "Auto-initialization Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1460,6 +1514,8 @@ struct AccelNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelNoise"; + static constexpr const char* DOC_NAME = "Accelerometer Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1492,6 +1548,8 @@ struct AccelNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelNoise::Response"; + static constexpr const char* DOC_NAME = "Accelerometer Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1538,6 +1596,8 @@ struct GyroNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroNoise"; + static constexpr const char* DOC_NAME = "Gyroscope Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1570,6 +1630,8 @@ struct GyroNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroNoise::Response"; + static constexpr const char* DOC_NAME = "Gyroscope Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1614,6 +1676,8 @@ struct AccelBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBiasModel"; + static constexpr const char* DOC_NAME = "Accelerometer Bias Model Parameters"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1646,6 +1710,8 @@ struct AccelBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBiasModel::Response"; + static constexpr const char* DOC_NAME = "Accelerometer Bias Model Parameters Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1691,6 +1757,8 @@ struct GyroBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBiasModel"; + static constexpr const char* DOC_NAME = "Gyroscope Bias Model Parameters"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1723,6 +1791,8 @@ struct GyroBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBiasModel::Response"; + static constexpr const char* DOC_NAME = "Gyroscope Bias Model Parameters Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1774,6 +1844,8 @@ struct AltitudeAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AltitudeAiding"; + static constexpr const char* DOC_NAME = "Altitude Aiding Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1806,6 +1878,8 @@ struct AltitudeAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AltitudeAiding::Response"; + static constexpr const char* DOC_NAME = "Altitude Aiding Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1853,6 +1927,8 @@ struct PitchRollAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PitchRollAiding"; + static constexpr const char* DOC_NAME = "Pitch/Roll Aiding Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1885,6 +1961,8 @@ struct PitchRollAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PitchRollAiding::Response"; + static constexpr const char* DOC_NAME = "Pitch/Roll Aiding Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1927,6 +2005,8 @@ struct AutoZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoZupt"; + static constexpr const char* DOC_NAME = "Zero Velocity Update Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1959,6 +2039,8 @@ struct AutoZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoZupt::Response"; + static constexpr const char* DOC_NAME = "Zero Velocity Update Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2003,6 +2085,8 @@ struct AutoAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoAngularZupt"; + static constexpr const char* DOC_NAME = "Zero Angular Rate Update Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2035,6 +2119,8 @@ struct AutoAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoAngularZupt::Response"; + static constexpr const char* DOC_NAME = "Zero Angular Rate Update Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2074,6 +2160,8 @@ struct CommandedZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommandedZupt"; + static constexpr const char* DOC_NAME = "Commanded Zero Velocity Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2108,6 +2196,8 @@ struct CommandedAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommandedAngularZupt"; + static constexpr const char* DOC_NAME = "Commanded Zero Angular Rate Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2145,6 +2235,8 @@ struct MagCaptureAutoCal static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagCaptureAutoCal"; + static constexpr const char* DOC_NAME = "Magnetometer Capture Auto Calibration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8000; @@ -2200,6 +2292,8 @@ struct GravityNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GravityNoise"; + static constexpr const char* DOC_NAME = "Gravity Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2232,6 +2326,8 @@ struct GravityNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GravityNoise::Response"; + static constexpr const char* DOC_NAME = "Gravity Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2277,6 +2373,8 @@ struct PressureAltitudeNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PressureAltitudeNoise"; + static constexpr const char* DOC_NAME = "Pressure Altitude Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2309,6 +2407,8 @@ struct PressureAltitudeNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PressureAltitudeNoise::Response"; + static constexpr const char* DOC_NAME = "Pressure Altitude Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2356,6 +2456,8 @@ struct HardIronOffsetNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HardIronOffsetNoise"; + static constexpr const char* DOC_NAME = "Hard Iron Offset Process Noise"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2388,6 +2490,8 @@ struct HardIronOffsetNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HardIronOffsetNoise::Response"; + static constexpr const char* DOC_NAME = "Hard Iron Offset Process Noise Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2434,6 +2538,8 @@ struct SoftIronMatrixNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SoftIronMatrixNoise"; + static constexpr const char* DOC_NAME = "Soft Iron Offset Process Noise"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2466,6 +2572,8 @@ struct SoftIronMatrixNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SoftIronMatrixNoise::Response"; + static constexpr const char* DOC_NAME = "Soft Iron Offset Process Noise Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2512,6 +2620,8 @@ struct MagNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagNoise"; + static constexpr const char* DOC_NAME = "Magnetometer Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2544,6 +2654,8 @@ struct MagNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagNoise::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2589,6 +2701,8 @@ struct InclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InclinationSource"; + static constexpr const char* DOC_NAME = "Inclination Source"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2621,6 +2735,8 @@ struct InclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InclinationSource::Response"; + static constexpr const char* DOC_NAME = "Inclination Source Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2667,6 +2783,8 @@ struct MagneticDeclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticDeclinationSource"; + static constexpr const char* DOC_NAME = "Magnetic Field Declination Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2699,6 +2817,8 @@ struct MagneticDeclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticDeclinationSource::Response"; + static constexpr const char* DOC_NAME = "Magnetic Field Declination Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2744,6 +2864,8 @@ struct MagFieldMagnitudeSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagFieldMagnitudeSource"; + static constexpr const char* DOC_NAME = "Magnetic Field Magnitude Source"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2776,6 +2898,8 @@ struct MagFieldMagnitudeSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagFieldMagnitudeSource::Response"; + static constexpr const char* DOC_NAME = "Magnetic Field Magnitude Source Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2823,6 +2947,8 @@ struct ReferencePosition static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferencePosition"; + static constexpr const char* DOC_NAME = "Set Reference Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2855,6 +2981,8 @@ struct ReferencePosition static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferencePosition::Response"; + static constexpr const char* DOC_NAME = "Set Reference Position Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2917,6 +3045,8 @@ struct AccelMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelMagnitudeErrorAdaptiveMeasurement"; + static constexpr const char* DOC_NAME = "Gravity Magnitude Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x807F; @@ -2949,6 +3079,8 @@ struct AccelMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelMagnitudeErrorAdaptiveMeasurement::Response"; + static constexpr const char* DOC_NAME = "Gravity Magnitude Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3009,6 +3141,8 @@ struct MagMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagMagnitudeErrorAdaptiveMeasurement"; + static constexpr const char* DOC_NAME = "Magnetometer Magnitude Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x807F; @@ -3041,6 +3175,8 @@ struct MagMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagMagnitudeErrorAdaptiveMeasurement::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Magnitude Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3101,6 +3237,8 @@ struct MagDipAngleErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagDipAngleErrorAdaptiveMeasurement"; + static constexpr const char* DOC_NAME = "Magnetometer Dig Angle Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -3133,6 +3271,8 @@ struct MagDipAngleErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagDipAngleErrorAdaptiveMeasurement::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Dig Angle Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3193,6 +3333,8 @@ struct AidingMeasurementEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingMeasurementEnable"; + static constexpr const char* DOC_NAME = "Aiding Measurement Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3226,6 +3368,8 @@ struct AidingMeasurementEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingMeasurementEnable::Response"; + static constexpr const char* DOC_NAME = "Aiding Measurement Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3267,6 +3411,8 @@ struct Run static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Run"; + static constexpr const char* DOC_NAME = "Run Navigation Filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3307,6 +3453,8 @@ struct KinematicConstraint static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "KinematicConstraint"; + static constexpr const char* DOC_NAME = "Kinematic Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3339,6 +3487,8 @@ struct KinematicConstraint static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "KinematicConstraint::Response"; + static constexpr const char* DOC_NAME = "Kinematic Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3435,6 +3585,8 @@ struct InitializationConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InitializationConfiguration"; + static constexpr const char* DOC_NAME = "Navigation Filter Initialization"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x81FF; @@ -3467,6 +3619,8 @@ struct InitializationConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InitializationConfiguration::Response"; + static constexpr const char* DOC_NAME = "Navigation Filter Initialization Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3516,6 +3670,8 @@ struct AdaptiveFilterOptions static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AdaptiveFilterOptions"; + static constexpr const char* DOC_NAME = "Adaptive Filter Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3548,6 +3704,8 @@ struct AdaptiveFilterOptions static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AdaptiveFilterOptions::Response"; + static constexpr const char* DOC_NAME = "Adaptive Filter Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3593,6 +3751,8 @@ struct MultiAntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffset"; + static constexpr const char* DOC_NAME = "GNSS Multi-Antenna Offset Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3626,6 +3786,8 @@ struct MultiAntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffset::Response"; + static constexpr const char* DOC_NAME = "GNSS Multi-Antenna Offset Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3669,6 +3831,8 @@ struct RelPosConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RelPosConfiguration"; + static constexpr const char* DOC_NAME = "Relative Position Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3701,6 +3865,8 @@ struct RelPosConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RelPosConfiguration::Response"; + static constexpr const char* DOC_NAME = "Relative Position Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3756,6 +3922,8 @@ struct RefPointLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RefPointLeverArm"; + static constexpr const char* DOC_NAME = "Reference point lever arm"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3788,6 +3956,8 @@ struct RefPointLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RefPointLeverArm::Response"; + static constexpr const char* DOC_NAME = "Reference point lever arm Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3833,6 +4003,8 @@ struct SpeedMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SpeedMeasurement"; + static constexpr const char* DOC_NAME = "Input speed measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3876,6 +4048,8 @@ struct SpeedLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SpeedLeverArm"; + static constexpr const char* DOC_NAME = "Measurement speed lever arm"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3909,6 +4083,8 @@ struct SpeedLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SpeedLeverArm::Response"; + static constexpr const char* DOC_NAME = "Measurement speed lever arm Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3956,6 +4132,8 @@ struct WheeledVehicleConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "WheeledVehicleConstraintControl"; + static constexpr const char* DOC_NAME = "Wheeled Vehicle Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3988,6 +4166,8 @@ struct WheeledVehicleConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "WheeledVehicleConstraintControl::Response"; + static constexpr const char* DOC_NAME = "Wheeled Vehicle Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4032,6 +4212,8 @@ struct VerticalGyroConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VerticalGyroConstraintControl"; + static constexpr const char* DOC_NAME = "Vertical Gyro Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -4064,6 +4246,8 @@ struct VerticalGyroConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VerticalGyroConstraintControl::Response"; + static constexpr const char* DOC_NAME = "Vertical Gyro Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4107,6 +4291,8 @@ struct GnssAntennaCalControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAntennaCalControl"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Calibration Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -4139,6 +4325,8 @@ struct GnssAntennaCalControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAntennaCalControl::Response"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Calibration Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4182,6 +4370,8 @@ struct SetInitialHeading static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SetInitialHeading"; + static constexpr const char* DOC_NAME = "Set Initial Heading Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 7cce02d91..7f3471d7d 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -78,6 +78,8 @@ struct ReceiverInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReceiverInfo"; + static constexpr const char* DOC_NAME = "ReceiverInfo"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -97,6 +99,8 @@ struct ReceiverInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReceiverInfo::Response"; + static constexpr const char* DOC_NAME = "ReceiverInfo Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -142,6 +146,8 @@ struct SignalConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SignalConfiguration"; + static constexpr const char* DOC_NAME = "SignalConfiguration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -174,6 +180,8 @@ struct SignalConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SignalConfiguration::Response"; + static constexpr const char* DOC_NAME = "SignalConfiguration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -220,6 +228,8 @@ struct RtkDongleConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RtkDongleConfiguration"; + static constexpr const char* DOC_NAME = "RtkDongleConfiguration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -252,6 +262,8 @@ struct RtkDongleConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RtkDongleConfiguration::Response"; + static constexpr const char* DOC_NAME = "RtkDongleConfiguration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 8c680da82..5ee697759 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -202,6 +202,8 @@ struct GetStatusFlags static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetStatusFlags"; + static constexpr const char* DOC_NAME = "Get RTK Device Status Flags"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -221,6 +223,8 @@ struct GetStatusFlags static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetStatusFlags::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device Status Flags Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -254,6 +258,8 @@ struct GetImei static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImei"; + static constexpr const char* DOC_NAME = "Get RTK Device IMEI (International Mobile Equipment Identifier)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -273,6 +279,8 @@ struct GetImei static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImei::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device IMEI (International Mobile Equipment Identifier) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -306,6 +314,8 @@ struct GetImsi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImsi"; + static constexpr const char* DOC_NAME = "Get RTK Device IMSI (International Mobile Subscriber Identifier)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -325,6 +335,8 @@ struct GetImsi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImsi::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device IMSI (International Mobile Subscriber Identifier) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -358,6 +370,8 @@ struct GetIccid static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetIccid"; + static constexpr const char* DOC_NAME = "Get RTK Device ICCID (Integrated Circuit Card Identification [SIM Number])"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -377,6 +391,8 @@ struct GetIccid static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetIccid::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device ICCID (Integrated Circuit Card Identification [SIM Number]) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -418,6 +434,8 @@ struct ConnectedDeviceType static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConnectedDeviceType"; + static constexpr const char* DOC_NAME = "Configure or read the type of the connected device"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -450,6 +468,8 @@ struct ConnectedDeviceType static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConnectedDeviceType::Response"; + static constexpr const char* DOC_NAME = "Configure or read the type of the connected device Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -487,6 +507,8 @@ struct GetActCode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetActCode"; + static constexpr const char* DOC_NAME = "Get RTK Device Activation Code"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -506,6 +528,8 @@ struct GetActCode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetActCode::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device Activation Code Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -539,6 +563,8 @@ struct GetModemFirmwareVersion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetModemFirmwareVersion"; + static constexpr const char* DOC_NAME = "Get RTK Device's Cell Modem Firmware version number"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -558,6 +584,8 @@ struct GetModemFirmwareVersion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetModemFirmwareVersion::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device's Cell Modem Firmware version number Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -592,6 +620,8 @@ struct GetRssi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetRssi"; + static constexpr const char* DOC_NAME = "GetRssi"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -611,6 +641,8 @@ struct GetRssi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetRssi::Response"; + static constexpr const char* DOC_NAME = "GetRssi Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -680,6 +712,8 @@ struct ServiceStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ServiceStatus"; + static constexpr const char* DOC_NAME = "ServiceStatus"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -699,6 +733,8 @@ struct ServiceStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ServiceStatus::Response"; + static constexpr const char* DOC_NAME = "ServiceStatus Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -738,6 +774,8 @@ struct ProdEraseStorage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ProdEraseStorage"; + static constexpr const char* DOC_NAME = "ProdEraseStorage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -776,6 +814,8 @@ struct LedControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LedControl"; + static constexpr const char* DOC_NAME = "LedControl"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -811,6 +851,8 @@ struct ModemHardReset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ModemHardReset"; + static constexpr const char* DOC_NAME = "ModemHardReset"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index b7f8595eb..90b1e88fa 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -77,6 +77,8 @@ struct CommMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommMode"; + static constexpr const char* DOC_NAME = "CommMode"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -109,6 +111,8 @@ struct CommMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommMode::Response"; + static constexpr const char* DOC_NAME = "CommMode Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 9802d413f..5b2b04fc5 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -374,6 +374,8 @@ struct PositionLlh static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PositionLlh"; + static constexpr const char* DOC_NAME = "LLH Position"; auto as_tuple() const @@ -408,6 +410,8 @@ struct VelocityNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelocityNed"; + static constexpr const char* DOC_NAME = "VelocityNed"; auto as_tuple() const @@ -448,6 +452,8 @@ struct AttitudeQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AttitudeQuaternion"; + static constexpr const char* DOC_NAME = "AttitudeQuaternion"; auto as_tuple() const @@ -490,6 +496,8 @@ struct AttitudeDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AttitudeDcm"; + static constexpr const char* DOC_NAME = "AttitudeDcm"; auto as_tuple() const @@ -525,6 +533,8 @@ struct EulerAngles static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EulerAngles"; + static constexpr const char* DOC_NAME = "EulerAngles"; auto as_tuple() const @@ -557,6 +567,8 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBias"; + static constexpr const char* DOC_NAME = "GyroBias"; auto as_tuple() const @@ -589,6 +601,8 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBias"; + static constexpr const char* DOC_NAME = "AccelBias"; auto as_tuple() const @@ -623,6 +637,8 @@ struct PositionLlhUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PositionLlhUncertainty"; + static constexpr const char* DOC_NAME = "LLH Position Uncertainty"; auto as_tuple() const @@ -657,6 +673,8 @@ struct VelocityNedUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelocityNedUncertainty"; + static constexpr const char* DOC_NAME = "NED Velocity Uncertainty"; auto as_tuple() const @@ -692,6 +710,8 @@ struct EulerAnglesUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EulerAnglesUncertainty"; + static constexpr const char* DOC_NAME = "EulerAnglesUncertainty"; auto as_tuple() const @@ -724,6 +744,8 @@ struct GyroBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBiasUncertainty"; + static constexpr const char* DOC_NAME = "GyroBiasUncertainty"; auto as_tuple() const @@ -756,6 +778,8 @@ struct AccelBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBiasUncertainty"; + static constexpr const char* DOC_NAME = "AccelBiasUncertainty"; auto as_tuple() const @@ -795,6 +819,8 @@ struct Timestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Timestamp"; + static constexpr const char* DOC_NAME = "Timestamp"; auto as_tuple() const @@ -828,6 +854,8 @@ struct Status static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Status"; + static constexpr const char* DOC_NAME = "Status"; auto as_tuple() const @@ -861,6 +889,8 @@ struct LinearAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LinearAccel"; + static constexpr const char* DOC_NAME = "LinearAccel"; auto as_tuple() const @@ -893,6 +923,8 @@ struct GravityVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GravityVector"; + static constexpr const char* DOC_NAME = "GravityVector"; auto as_tuple() const @@ -925,6 +957,8 @@ struct CompAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompAccel"; + static constexpr const char* DOC_NAME = "Compensated Acceleration"; auto as_tuple() const @@ -957,6 +991,8 @@ struct CompAngularRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompAngularRate"; + static constexpr const char* DOC_NAME = "CompAngularRate"; auto as_tuple() const @@ -989,6 +1025,8 @@ struct QuaternionAttitudeUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "QuaternionAttitudeUncertainty"; + static constexpr const char* DOC_NAME = "QuaternionAttitudeUncertainty"; auto as_tuple() const @@ -1021,6 +1059,8 @@ struct Wgs84GravityMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Wgs84GravityMag"; + static constexpr const char* DOC_NAME = "Wgs84GravityMag"; auto as_tuple() const @@ -1067,6 +1107,8 @@ struct HeadingUpdateState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadingUpdateState"; + static constexpr const char* DOC_NAME = "HeadingUpdateState"; auto as_tuple() const @@ -1104,6 +1146,8 @@ struct MagneticModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticModel"; + static constexpr const char* DOC_NAME = "MagneticModel"; auto as_tuple() const @@ -1136,6 +1180,8 @@ struct AccelScaleFactor static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelScaleFactor"; + static constexpr const char* DOC_NAME = "AccelScaleFactor"; auto as_tuple() const @@ -1168,6 +1214,8 @@ struct AccelScaleFactorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelScaleFactorUncertainty"; + static constexpr const char* DOC_NAME = "AccelScaleFactorUncertainty"; auto as_tuple() const @@ -1200,6 +1248,8 @@ struct GyroScaleFactor static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroScaleFactor"; + static constexpr const char* DOC_NAME = "GyroScaleFactor"; auto as_tuple() const @@ -1232,6 +1282,8 @@ struct GyroScaleFactorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroScaleFactorUncertainty"; + static constexpr const char* DOC_NAME = "GyroScaleFactorUncertainty"; auto as_tuple() const @@ -1264,6 +1316,8 @@ struct MagBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagBias"; + static constexpr const char* DOC_NAME = "MagBias"; auto as_tuple() const @@ -1296,6 +1350,8 @@ struct MagBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagBiasUncertainty"; + static constexpr const char* DOC_NAME = "MagBiasUncertainty"; auto as_tuple() const @@ -1334,6 +1390,8 @@ struct StandardAtmosphere static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "StandardAtmosphere"; + static constexpr const char* DOC_NAME = "StandardAtmosphere"; auto as_tuple() const @@ -1370,6 +1428,8 @@ struct PressureAltitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PressureAltitude"; + static constexpr const char* DOC_NAME = "PressureAltitude"; auto as_tuple() const @@ -1401,6 +1461,8 @@ struct DensityAltitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DensityAltitude"; + static constexpr const char* DOC_NAME = "DensityAltitude"; auto as_tuple() const @@ -1435,6 +1497,8 @@ struct AntennaOffsetCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffsetCorrection"; + static constexpr const char* DOC_NAME = "AntennaOffsetCorrection"; auto as_tuple() const @@ -1467,6 +1531,8 @@ struct AntennaOffsetCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffsetCorrectionUncertainty"; + static constexpr const char* DOC_NAME = "AntennaOffsetCorrectionUncertainty"; auto as_tuple() const @@ -1502,6 +1568,8 @@ struct MultiAntennaOffsetCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffsetCorrection"; + static constexpr const char* DOC_NAME = "MultiAntennaOffsetCorrection"; auto as_tuple() const @@ -1535,6 +1603,8 @@ struct MultiAntennaOffsetCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffsetCorrectionUncertainty"; + static constexpr const char* DOC_NAME = "MultiAntennaOffsetCorrectionUncertainty"; auto as_tuple() const @@ -1569,6 +1639,8 @@ struct MagnetometerOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerOffset"; + static constexpr const char* DOC_NAME = "MagnetometerOffset"; auto as_tuple() const @@ -1603,6 +1675,8 @@ struct MagnetometerMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerMatrix"; + static constexpr const char* DOC_NAME = "MagnetometerMatrix"; auto as_tuple() const @@ -1635,6 +1709,8 @@ struct MagnetometerOffsetUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerOffsetUncertainty"; + static constexpr const char* DOC_NAME = "MagnetometerOffsetUncertainty"; auto as_tuple() const @@ -1667,6 +1743,8 @@ struct MagnetometerMatrixUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerMatrixUncertainty"; + static constexpr const char* DOC_NAME = "MagnetometerMatrixUncertainty"; auto as_tuple() const @@ -1698,6 +1776,8 @@ struct MagnetometerCovarianceMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerCovarianceMatrix"; + static constexpr const char* DOC_NAME = "MagnetometerCovarianceMatrix"; auto as_tuple() const @@ -1730,6 +1810,8 @@ struct MagnetometerResidualVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerResidualVector"; + static constexpr const char* DOC_NAME = "MagnetometerResidualVector"; auto as_tuple() const @@ -1764,6 +1846,8 @@ struct ClockCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockCorrection"; + static constexpr const char* DOC_NAME = "ClockCorrection"; auto as_tuple() const @@ -1798,6 +1882,8 @@ struct ClockCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockCorrectionUncertainty"; + static constexpr const char* DOC_NAME = "ClockCorrectionUncertainty"; auto as_tuple() const @@ -1832,6 +1918,8 @@ struct GnssPosAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssPosAidStatus"; + static constexpr const char* DOC_NAME = "GNSS Position Aiding Status"; auto as_tuple() const @@ -1865,6 +1953,8 @@ struct GnssAttAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAttAidStatus"; + static constexpr const char* DOC_NAME = "GNSS Attitude Aiding Status"; auto as_tuple() const @@ -1904,6 +1994,8 @@ struct HeadAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadAidStatus"; + static constexpr const char* DOC_NAME = "HeadAidStatus"; auto as_tuple() const @@ -1936,6 +2028,8 @@ struct RelPosNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RelPosNed"; + static constexpr const char* DOC_NAME = "NED Relative Position"; auto as_tuple() const @@ -1968,6 +2062,8 @@ struct EcefPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefPos"; + static constexpr const char* DOC_NAME = "ECEF Position"; auto as_tuple() const @@ -2000,6 +2096,8 @@ struct EcefVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefVel"; + static constexpr const char* DOC_NAME = "ECEF Velocity"; auto as_tuple() const @@ -2032,6 +2130,8 @@ struct EcefPosUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefPosUncertainty"; + static constexpr const char* DOC_NAME = "ECEF Position Uncertainty"; auto as_tuple() const @@ -2064,6 +2164,8 @@ struct EcefVelUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefVelUncertainty"; + static constexpr const char* DOC_NAME = "ECEF Velocity Uncertainty"; auto as_tuple() const @@ -2098,6 +2200,8 @@ struct AidingMeasurementSummary static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingMeasurementSummary"; + static constexpr const char* DOC_NAME = "AidingMeasurementSummary"; auto as_tuple() const @@ -2130,6 +2234,8 @@ struct OdometerScaleFactorError static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OdometerScaleFactorError"; + static constexpr const char* DOC_NAME = "Odometer Scale Factor Error"; auto as_tuple() const @@ -2162,6 +2268,8 @@ struct OdometerScaleFactorErrorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OdometerScaleFactorErrorUncertainty"; + static constexpr const char* DOC_NAME = "Odometer Scale Factor Error Uncertainty"; auto as_tuple() const @@ -2236,6 +2344,8 @@ struct GnssDualAntennaStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssDualAntennaStatus"; + static constexpr const char* DOC_NAME = "GNSS Dual Antenna Status"; auto as_tuple() const @@ -2271,6 +2381,8 @@ struct AidingFrameConfigError static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingFrameConfigError"; + static constexpr const char* DOC_NAME = "Aiding Frame Configuration Error"; auto as_tuple() const @@ -2306,6 +2418,8 @@ struct AidingFrameConfigErrorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingFrameConfigErrorUncertainty"; + static constexpr const char* DOC_NAME = "Aiding Frame Configuration Error Uncertainty"; auto as_tuple() const diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 7b5fee4b8..5a59b0704 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -227,6 +227,8 @@ struct PosLlh static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PosLlh"; + static constexpr const char* DOC_NAME = "GNSS LLH Position"; auto as_tuple() const @@ -291,6 +293,8 @@ struct PosEcef static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PosEcef"; + static constexpr const char* DOC_NAME = "GNSS ECEF Position"; auto as_tuple() const @@ -371,6 +375,8 @@ struct VelNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelNed"; + static constexpr const char* DOC_NAME = "NED Velocity"; auto as_tuple() const @@ -435,6 +441,8 @@ struct VelEcef static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelEcef"; + static constexpr const char* DOC_NAME = "GNSS ECEF Velocity"; auto as_tuple() const @@ -519,6 +527,8 @@ struct Dop static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Dop"; + static constexpr const char* DOC_NAME = "Dop"; auto as_tuple() const @@ -588,6 +598,8 @@ struct UtcTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UtcTime"; + static constexpr const char* DOC_NAME = "UtcTime"; auto as_tuple() const @@ -652,6 +664,8 @@ struct GpsTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTime"; + static constexpr const char* DOC_NAME = "GpsTime"; auto as_tuple() const @@ -720,6 +734,8 @@ struct ClockInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockInfo"; + static constexpr const char* DOC_NAME = "ClockInfo"; auto as_tuple() const @@ -827,6 +843,8 @@ struct FixInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FixInfo"; + static constexpr const char* DOC_NAME = "FixInfo"; auto as_tuple() const @@ -937,6 +955,8 @@ struct SvInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SvInfo"; + static constexpr const char* DOC_NAME = "SvInfo"; auto as_tuple() const @@ -1028,6 +1048,8 @@ struct HwStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HwStatus"; + static constexpr const char* DOC_NAME = "GNSS Hardware Status"; auto as_tuple() const @@ -1112,6 +1134,8 @@ struct DgpsInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DgpsInfo"; + static constexpr const char* DOC_NAME = "DgpsInfo"; auto as_tuple() const @@ -1186,6 +1210,8 @@ struct DgpsChannel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DgpsChannel"; + static constexpr const char* DOC_NAME = "DgpsChannel"; auto as_tuple() const @@ -1260,6 +1286,8 @@ struct ClockInfo2 static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockInfo2"; + static constexpr const char* DOC_NAME = "ClockInfo2"; auto as_tuple() const @@ -1317,6 +1345,8 @@ struct GpsLeapSeconds static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsLeapSeconds"; + static constexpr const char* DOC_NAME = "GpsLeapSeconds"; auto as_tuple() const @@ -1431,6 +1461,8 @@ struct SbasInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SbasInfo"; + static constexpr const char* DOC_NAME = "SbasInfo"; auto as_tuple() const @@ -1527,6 +1559,8 @@ struct SbasCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SbasCorrection"; + static constexpr const char* DOC_NAME = "SbasCorrection"; auto as_tuple() const @@ -1620,6 +1654,8 @@ struct RfErrorDetection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RfErrorDetection"; + static constexpr const char* DOC_NAME = "RfErrorDetection"; auto as_tuple() const @@ -1751,6 +1787,8 @@ struct BaseStationInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BaseStationInfo"; + static constexpr const char* DOC_NAME = "BaseStationInfo"; auto as_tuple() const @@ -1888,6 +1926,8 @@ struct RtkCorrectionsStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RtkCorrectionsStatus"; + static constexpr const char* DOC_NAME = "RtkCorrectionsStatus"; auto as_tuple() const @@ -1974,6 +2014,8 @@ struct SatelliteStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SatelliteStatus"; + static constexpr const char* DOC_NAME = "SatelliteStatus"; auto as_tuple() const @@ -2106,6 +2148,8 @@ struct Raw static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Raw"; + static constexpr const char* DOC_NAME = "Raw"; auto as_tuple() const @@ -2201,6 +2245,8 @@ struct GpsEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsEphemeris"; + static constexpr const char* DOC_NAME = "GpsEphemeris"; auto as_tuple() const @@ -2296,6 +2342,8 @@ struct GalileoEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GalileoEphemeris"; + static constexpr const char* DOC_NAME = "GalileoEphemeris"; auto as_tuple() const @@ -2379,6 +2427,8 @@ struct GloEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GloEphemeris"; + static constexpr const char* DOC_NAME = "Glonass Ephemeris"; auto as_tuple() const @@ -2451,6 +2501,8 @@ struct GpsIonoCorr static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsIonoCorr"; + static constexpr const char* DOC_NAME = "GPS Ionospheric Correction"; auto as_tuple() const @@ -2523,6 +2575,8 @@ struct GalileoIonoCorr static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GalileoIonoCorr"; + static constexpr const char* DOC_NAME = "Galileo Ionospheric Correction"; auto as_tuple() const diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index 10fee544f..dde268718 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -84,6 +84,8 @@ struct RawAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawAccel"; + static constexpr const char* DOC_NAME = "RawAccel"; auto as_tuple() const @@ -116,6 +118,8 @@ struct RawGyro static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawGyro"; + static constexpr const char* DOC_NAME = "RawGyro"; auto as_tuple() const @@ -148,6 +152,8 @@ struct RawMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawMag"; + static constexpr const char* DOC_NAME = "RawMag"; auto as_tuple() const @@ -180,6 +186,8 @@ struct RawPressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawPressure"; + static constexpr const char* DOC_NAME = "RawPressure"; auto as_tuple() const @@ -212,6 +220,8 @@ struct ScaledAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledAccel"; + static constexpr const char* DOC_NAME = "ScaledAccel"; auto as_tuple() const @@ -244,6 +254,8 @@ struct ScaledGyro static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledGyro"; + static constexpr const char* DOC_NAME = "ScaledGyro"; auto as_tuple() const @@ -276,6 +288,8 @@ struct ScaledMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledMag"; + static constexpr const char* DOC_NAME = "ScaledMag"; auto as_tuple() const @@ -307,6 +321,8 @@ struct ScaledPressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledPressure"; + static constexpr const char* DOC_NAME = "ScaledPressure"; auto as_tuple() const @@ -339,6 +355,8 @@ struct DeltaTheta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaTheta"; + static constexpr const char* DOC_NAME = "DeltaTheta"; auto as_tuple() const @@ -371,6 +389,8 @@ struct DeltaVelocity static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaVelocity"; + static constexpr const char* DOC_NAME = "DeltaVelocity"; auto as_tuple() const @@ -412,6 +432,8 @@ struct CompOrientationMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompOrientationMatrix"; + static constexpr const char* DOC_NAME = "Complementary Filter Orientation Matrix"; auto as_tuple() const @@ -451,6 +473,8 @@ struct CompQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompQuaternion"; + static constexpr const char* DOC_NAME = "Complementary Filter Quaternion"; auto as_tuple() const @@ -485,6 +509,8 @@ struct CompEulerAngles static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompEulerAngles"; + static constexpr const char* DOC_NAME = "Complementary Filter Euler Angles"; auto as_tuple() const @@ -516,6 +542,8 @@ struct CompOrientationUpdateMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompOrientationUpdateMatrix"; + static constexpr const char* DOC_NAME = "Complementary Filter Orientation Update Matrix"; auto as_tuple() const @@ -547,6 +575,8 @@ struct OrientationRawTemp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OrientationRawTemp"; + static constexpr const char* DOC_NAME = "OrientationRawTemp"; auto as_tuple() const @@ -578,6 +608,8 @@ struct InternalTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InternalTimestamp"; + static constexpr const char* DOC_NAME = "InternalTimestamp"; auto as_tuple() const @@ -610,6 +642,8 @@ struct PpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PpsTimestamp"; + static constexpr const char* DOC_NAME = "PPS Timestamp"; auto as_tuple() const @@ -686,6 +720,8 @@ struct GpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTimestamp"; + static constexpr const char* DOC_NAME = "GpsTimestamp"; auto as_tuple() const @@ -723,6 +759,8 @@ struct TemperatureAbs static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TemperatureAbs"; + static constexpr const char* DOC_NAME = "Temperature Statistics"; auto as_tuple() const @@ -760,6 +798,8 @@ struct UpVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UpVector"; + static constexpr const char* DOC_NAME = "UpVector"; auto as_tuple() const @@ -794,6 +834,8 @@ struct NorthVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NorthVector"; + static constexpr const char* DOC_NAME = "NorthVector"; auto as_tuple() const @@ -876,6 +918,8 @@ struct OverrangeStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OverrangeStatus"; + static constexpr const char* DOC_NAME = "OverrangeStatus"; auto as_tuple() const @@ -908,6 +952,8 @@ struct OdometerData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OdometerData"; + static constexpr const char* DOC_NAME = "OdometerData"; auto as_tuple() const diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 5b44446e8..05df1eddf 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -71,6 +71,8 @@ struct EventSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventSource"; + static constexpr const char* DOC_NAME = "EventSource"; auto as_tuple() const @@ -105,6 +107,8 @@ struct Ticks static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Ticks"; + static constexpr const char* DOC_NAME = "Ticks"; auto as_tuple() const @@ -140,6 +144,8 @@ struct DeltaTicks static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaTicks"; + static constexpr const char* DOC_NAME = "DeltaTicks"; auto as_tuple() const @@ -207,6 +213,8 @@ struct GpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTimestamp"; + static constexpr const char* DOC_NAME = "GpsTimestamp"; auto as_tuple() const @@ -247,6 +255,8 @@ struct DeltaTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaTime"; + static constexpr const char* DOC_NAME = "DeltaTime"; auto as_tuple() const @@ -285,6 +295,8 @@ struct ReferenceTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceTimestamp"; + static constexpr const char* DOC_NAME = "ReferenceTimestamp"; auto as_tuple() const @@ -325,6 +337,8 @@ struct ReferenceTimeDelta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceTimeDelta"; + static constexpr const char* DOC_NAME = "ReferenceTimeDelta"; auto as_tuple() const @@ -390,6 +404,8 @@ struct ExternalTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalTimestamp"; + static constexpr const char* DOC_NAME = "ExternalTimestamp"; auto as_tuple() const @@ -459,6 +475,8 @@ struct ExternalTimeDelta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalTimeDelta"; + static constexpr const char* DOC_NAME = "ExternalTimeDelta"; auto as_tuple() const diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 80b457773..17170999f 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -79,6 +79,8 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BuiltInTest"; + static constexpr const char* DOC_NAME = "BuiltInTest"; auto as_tuple() const @@ -111,6 +113,8 @@ struct TimeSyncStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TimeSyncStatus"; + static constexpr const char* DOC_NAME = "TimeSyncStatus"; auto as_tuple() const @@ -160,6 +164,8 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioState"; + static constexpr const char* DOC_NAME = "GpioState"; auto as_tuple() const @@ -193,6 +199,8 @@ struct GpioAnalogValue static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioAnalogValue"; + static constexpr const char* DOC_NAME = "GpioAnalogValue"; auto as_tuple() const From b29b7b84e07fd51718443728238dd55eb7fe8c1a Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 14 Feb 2024 14:53:39 -0500 Subject: [PATCH 203/252] Remove constexpr from CompositeDescriptor::operator= --- src/mip/definitions/descriptors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index 29ce8efb8..4369bdf00 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -72,7 +72,7 @@ struct CompositeDescriptor constexpr CompositeDescriptor(uint8_t descSet, uint8_t fieldDesc) : descriptorSet(descSet), fieldDescriptor(fieldDesc) {} constexpr CompositeDescriptor(uint16_t combo) : descriptorSet(combo >> 8), fieldDescriptor(combo & 0xFF) {} - constexpr CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } + CompositeDescriptor& operator=(uint16_t combo) { return *this = CompositeDescriptor(combo); } constexpr uint16_t as_u16() const { return (uint16_t(descriptorSet) << 8) | fieldDescriptor; } From 2bba988764eaa52eab8e79133893ea8930ba337b Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 15 Feb 2024 21:06:54 +0000 Subject: [PATCH 204/252] Generate MIP definitions from branches/dev@56013. --- src/mip/definitions/commands_3dm.cpp | 402 +++++++-------- src/mip/definitions/commands_3dm.hpp | 502 ++++++------------ src/mip/definitions/commands_aiding.cpp | 54 +- src/mip/definitions/commands_aiding.hpp | 90 ++-- src/mip/definitions/commands_base.cpp | 42 +- src/mip/definitions/commands_base.hpp | 66 +-- src/mip/definitions/commands_filter.cpp | 528 +++++++++---------- src/mip/definitions/commands_filter.h | 3 +- src/mip/definitions/commands_filter.hpp | 643 +++++++++--------------- src/mip/definitions/commands_gnss.cpp | 28 +- src/mip/definitions/commands_gnss.hpp | 34 +- src/mip/definitions/commands_rtk.cpp | 50 +- src/mip/definitions/commands_rtk.hpp | 80 +-- src/mip/definitions/commands_system.cpp | 8 +- src/mip/definitions/commands_system.hpp | 10 +- src/mip/definitions/data_filter.hpp | 122 +---- src/mip/definitions/data_gnss.hpp | 118 ++--- src/mip/definitions/data_sensor.hpp | 50 +- src/mip/definitions/data_shared.hpp | 24 +- src/mip/definitions/data_system.hpp | 8 - 20 files changed, 1071 insertions(+), 1791 deletions(-) diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index 477119d41..563119dcc 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -72,7 +72,7 @@ void extract(Serializer& serializer, PollImuMessage& self) } -TypedResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -109,7 +109,7 @@ void extract(Serializer& serializer, PollGnssMessage& self) } -TypedResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -146,7 +146,7 @@ void extract(Serializer& serializer, PollFilterMessage& self) } -TypedResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -205,7 +205,7 @@ void extract(Serializer& serializer, ImuMessageFormat::Response& self) } -TypedResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -221,7 +221,7 @@ TypedResult writeImuMessageFormat(C::mip_interface& device, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -230,7 +230,7 @@ TypedResult readImuMessageFormat(C::mip_interface& device, uin assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_IMU_MESSAGE_FORMAT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_IMU_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -246,7 +246,7 @@ TypedResult readImuMessageFormat(C::mip_interface& device, uin } return result; } -TypedResult saveImuMessageFormat(C::mip_interface& device) +CmdResult saveImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -256,7 +256,7 @@ TypedResult saveImuMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadImuMessageFormat(C::mip_interface& device) +CmdResult loadImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -266,7 +266,7 @@ TypedResult loadImuMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultImuMessageFormat(C::mip_interface& device) +CmdResult defaultImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -318,7 +318,7 @@ void extract(Serializer& serializer, GpsMessageFormat::Response& self) } -TypedResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -334,7 +334,7 @@ TypedResult writeGpsMessageFormat(C::mip_interface& device, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -343,7 +343,7 @@ TypedResult readGpsMessageFormat(C::mip_interface& device, uin assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_MESSAGE_FORMAT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -359,7 +359,7 @@ TypedResult readGpsMessageFormat(C::mip_interface& device, uin } return result; } -TypedResult saveGpsMessageFormat(C::mip_interface& device) +CmdResult saveGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -369,7 +369,7 @@ TypedResult saveGpsMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGpsMessageFormat(C::mip_interface& device) +CmdResult loadGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -379,7 +379,7 @@ TypedResult loadGpsMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGpsMessageFormat(C::mip_interface& device) +CmdResult defaultGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -431,7 +431,7 @@ void extract(Serializer& serializer, FilterMessageFormat::Response& self) } -TypedResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -447,7 +447,7 @@ TypedResult writeFilterMessageFormat(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -456,7 +456,7 @@ TypedResult readFilterMessageFormat(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FILTER_MESSAGE_FORMAT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FILTER_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -472,7 +472,7 @@ TypedResult readFilterMessageFormat(C::mip_interface& devic } return result; } -TypedResult saveFilterMessageFormat(C::mip_interface& device) +CmdResult saveFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -482,7 +482,7 @@ TypedResult saveFilterMessageFormat(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadFilterMessageFormat(C::mip_interface& device) +CmdResult loadFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -492,7 +492,7 @@ TypedResult loadFilterMessageFormat(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultFilterMessageFormat(C::mip_interface& device) +CmdResult defaultFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -524,12 +524,12 @@ void extract(Serializer& serializer, ImuGetBaseRate::Response& self) } -TypedResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMU_BASE_RATE, NULL, 0, REPLY_IMU_BASE_RATE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMU_BASE_RATE, NULL, 0, REPLY_IMU_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -565,12 +565,12 @@ void extract(Serializer& serializer, GpsGetBaseRate::Response& self) } -TypedResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_GNSS_BASE_RATE, NULL, 0, REPLY_GNSS_BASE_RATE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_GNSS_BASE_RATE, NULL, 0, REPLY_GNSS_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -606,12 +606,12 @@ void extract(Serializer& serializer, FilterGetBaseRate::Response& self) } -TypedResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_FILTER_BASE_RATE, NULL, 0, REPLY_FILTER_BASE_RATE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_FILTER_BASE_RATE, NULL, 0, REPLY_FILTER_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -649,7 +649,7 @@ void extract(Serializer& serializer, PollData& self) } -TypedResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors) +CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -694,7 +694,7 @@ void extract(Serializer& serializer, GetBaseRate::Response& self) } -TypedResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut) +CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -704,7 +704,7 @@ TypedResult getBaseRate(C::mip_interface& device, uint8_t descSet, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_BASE_RATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_BASE_RATE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_BASE_RATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -770,7 +770,7 @@ void extract(Serializer& serializer, MessageFormat::Response& self) } -TypedResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors) +CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -788,7 +788,7 @@ TypedResult writeMessageFormat(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -799,7 +799,7 @@ TypedResult readMessageFormat(C::mip_interface& device, uint8_t d assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MESSAGE_FORMAT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -817,7 +817,7 @@ TypedResult readMessageFormat(C::mip_interface& device, uint8_t d } return result; } -TypedResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) +CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -829,7 +829,7 @@ TypedResult saveMessageFormat(C::mip_interface& device, uint8_t d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) +CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -841,7 +841,7 @@ TypedResult loadMessageFormat(C::mip_interface& device, uint8_t d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet) +CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -873,7 +873,7 @@ void extract(Serializer& serializer, NmeaPollData& self) } -TypedResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries) +CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -932,7 +932,7 @@ void extract(Serializer& serializer, NmeaMessageFormat::Response& self) } -TypedResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries) +CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -948,7 +948,7 @@ TypedResult writeNmeaMessageFormat(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut) +CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -957,7 +957,7 @@ TypedResult readNmeaMessageFormat(C::mip_interface& device, u assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NMEA_MESSAGE_FORMAT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NMEA_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -973,7 +973,7 @@ TypedResult readNmeaMessageFormat(C::mip_interface& device, u } return result; } -TypedResult saveNmeaMessageFormat(C::mip_interface& device) +CmdResult saveNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -983,7 +983,7 @@ TypedResult saveNmeaMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadNmeaMessageFormat(C::mip_interface& device) +CmdResult loadNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -993,7 +993,7 @@ TypedResult loadNmeaMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultNmeaMessageFormat(C::mip_interface& device) +CmdResult defaultNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1014,7 +1014,7 @@ void extract(Serializer& serializer, DeviceSettings& self) } -TypedResult saveDeviceSettings(C::mip_interface& device) +CmdResult saveDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1024,7 +1024,7 @@ TypedResult saveDeviceSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DEVICE_STARTUP_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadDeviceSettings(C::mip_interface& device) +CmdResult loadDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1034,7 +1034,7 @@ TypedResult loadDeviceSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DEVICE_STARTUP_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultDeviceSettings(C::mip_interface& device) +CmdResult defaultDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1076,7 +1076,7 @@ void extract(Serializer& serializer, UartBaudrate::Response& self) } -TypedResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) +CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1088,7 +1088,7 @@ TypedResult writeUartBaudrate(C::mip_interface& device, uint32_t b return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) +CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1097,7 +1097,7 @@ TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* b assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_UART_BAUDRATE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_UART_BAUDRATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1111,7 +1111,7 @@ TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* b } return result; } -TypedResult saveUartBaudrate(C::mip_interface& device) +CmdResult saveUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1121,7 +1121,7 @@ TypedResult saveUartBaudrate(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadUartBaudrate(C::mip_interface& device) +CmdResult loadUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1131,7 +1131,7 @@ TypedResult loadUartBaudrate(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultUartBaudrate(C::mip_interface& device) +CmdResult defaultUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1156,7 +1156,7 @@ void extract(Serializer& serializer, FactoryStreaming& self) } -TypedResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved) +CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1209,7 +1209,7 @@ void extract(Serializer& serializer, DatastreamControl::Response& self) } -TypedResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable) +CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1223,7 +1223,7 @@ TypedResult writeDatastreamControl(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut) +CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1234,7 +1234,7 @@ TypedResult readDatastreamControl(C::mip_interface& device, u assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DATASTREAM_ENABLE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DATASTREAM_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1250,7 +1250,7 @@ TypedResult readDatastreamControl(C::mip_interface& device, u } return result; } -TypedResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) +CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1262,7 +1262,7 @@ TypedResult saveDatastreamControl(C::mip_interface& device, u return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) +CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1274,7 +1274,7 @@ TypedResult loadDatastreamControl(C::mip_interface& device, u return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) +CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1367,7 +1367,7 @@ void extract(Serializer& serializer, ConstellationSettings::Settings& self) } -TypedResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) +CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1385,7 +1385,7 @@ TypedResult writeConstellationSettings(C::mip_interface& return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) +CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1394,7 +1394,7 @@ TypedResult readConstellationSettings(C::mip_interface& d assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1416,7 +1416,7 @@ TypedResult readConstellationSettings(C::mip_interface& d } return result; } -TypedResult saveConstellationSettings(C::mip_interface& device) +CmdResult saveConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1426,7 +1426,7 @@ TypedResult saveConstellationSettings(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadConstellationSettings(C::mip_interface& device) +CmdResult loadConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1436,7 +1436,7 @@ TypedResult loadConstellationSettings(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultConstellationSettings(C::mip_interface& device) +CmdResult defaultConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1504,7 +1504,7 @@ void extract(Serializer& serializer, GnssSbasSettings::Response& self) } -TypedResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns) +CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1524,7 +1524,7 @@ TypedResult writeGnssSbasSettings(C::mip_interface& device, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut) +CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1533,7 +1533,7 @@ TypedResult readGnssSbasSettings(C::mip_interface& device, uin assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SBAS_SETTINGS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SBAS_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1555,7 +1555,7 @@ TypedResult readGnssSbasSettings(C::mip_interface& device, uin } return result; } -TypedResult saveGnssSbasSettings(C::mip_interface& device) +CmdResult saveGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1565,7 +1565,7 @@ TypedResult saveGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGnssSbasSettings(C::mip_interface& device) +CmdResult loadGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1575,7 +1575,7 @@ TypedResult loadGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGnssSbasSettings(C::mip_interface& device) +CmdResult defaultGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1625,7 +1625,7 @@ void extract(Serializer& serializer, GnssAssistedFix::Response& self) } -TypedResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) +CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1639,7 +1639,7 @@ TypedResult writeGnssAssistedFix(C::mip_interface& device, Gnss return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) +CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1648,7 +1648,7 @@ TypedResult readGnssAssistedFix(C::mip_interface& device, GnssA assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1665,7 +1665,7 @@ TypedResult readGnssAssistedFix(C::mip_interface& device, GnssA } return result; } -TypedResult saveGnssAssistedFix(C::mip_interface& device) +CmdResult saveGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1675,7 +1675,7 @@ TypedResult saveGnssAssistedFix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGnssAssistedFix(C::mip_interface& device) +CmdResult loadGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1685,7 +1685,7 @@ TypedResult loadGnssAssistedFix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGnssAssistedFix(C::mip_interface& device) +CmdResult defaultGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1743,7 +1743,7 @@ void extract(Serializer& serializer, GnssTimeAssistance::Response& self) } -TypedResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy) +CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1759,7 +1759,7 @@ TypedResult writeGnssTimeAssistance(C::mip_interface& device return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut) +CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1768,7 +1768,7 @@ TypedResult readGnssTimeAssistance(C::mip_interface& device, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_TIME_ASSISTANCE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_TIME_ASSISTANCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1852,7 +1852,7 @@ void extract(Serializer& serializer, ImuLowpassFilter::Response& self) } -TypedResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) +CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1872,7 +1872,7 @@ TypedResult writeImuLowpassFilter(C::mip_interface& device, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) +CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1883,7 +1883,7 @@ TypedResult readImuLowpassFilter(C::mip_interface& device, uin assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1908,7 +1908,7 @@ TypedResult readImuLowpassFilter(C::mip_interface& device, uin } return result; } -TypedResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1920,7 +1920,7 @@ TypedResult saveImuLowpassFilter(C::mip_interface& device, uin return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1932,7 +1932,7 @@ TypedResult loadImuLowpassFilter(C::mip_interface& device, uin return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1976,7 +1976,7 @@ void extract(Serializer& serializer, PpsSource::Response& self) } -TypedResult writePpsSource(C::mip_interface& device, PpsSource::Source source) +CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1988,7 +1988,7 @@ TypedResult writePpsSource(C::mip_interface& device, PpsSource::Sourc return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) +CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1997,7 +1997,7 @@ TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PPS_SOURCE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PPS_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2011,7 +2011,7 @@ TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source } return result; } -TypedResult savePpsSource(C::mip_interface& device) +CmdResult savePpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2021,7 +2021,7 @@ TypedResult savePpsSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadPpsSource(C::mip_interface& device) +CmdResult loadPpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2031,7 +2031,7 @@ TypedResult loadPpsSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultPpsSource(C::mip_interface& device) +CmdResult defaultPpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2097,7 +2097,7 @@ void extract(Serializer& serializer, GpioConfig::Response& self) } -TypedResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode) +CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2115,7 +2115,7 @@ TypedResult writeGpioConfig(C::mip_interface& device, uint8_t pin, G return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut) +CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2126,7 +2126,7 @@ TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, Gp assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_CONFIG, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2148,7 +2148,7 @@ TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, Gp } return result; } -TypedResult saveGpioConfig(C::mip_interface& device, uint8_t pin) +CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2160,7 +2160,7 @@ TypedResult saveGpioConfig(C::mip_interface& device, uint8_t pin) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGpioConfig(C::mip_interface& device, uint8_t pin) +CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2172,7 +2172,7 @@ TypedResult loadGpioConfig(C::mip_interface& device, uint8_t pin) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGpioConfig(C::mip_interface& device, uint8_t pin) +CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2230,7 +2230,7 @@ void extract(Serializer& serializer, GpioState::Response& self) } -TypedResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) +CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2244,7 +2244,7 @@ TypedResult writeGpioState(C::mip_interface& device, uint8_t pin, boo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) +CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2255,7 +2255,7 @@ TypedResult readGpioState(C::mip_interface& device, uint8_t pin, bool assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_STATE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_STATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2319,7 +2319,7 @@ void extract(Serializer& serializer, Odometer::Response& self) } -TypedResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty) +CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2335,7 +2335,7 @@ TypedResult writeOdometer(C::mip_interface& device, Odometer::Mode mod return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut) +CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2344,7 +2344,7 @@ TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* mod assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOMETER_CONFIG, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOMETER_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2364,7 +2364,7 @@ TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* mod } return result; } -TypedResult saveOdometer(C::mip_interface& device) +CmdResult saveOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2374,7 +2374,7 @@ TypedResult saveOdometer(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadOdometer(C::mip_interface& device) +CmdResult loadOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2384,7 +2384,7 @@ TypedResult loadOdometer(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultOdometer(C::mip_interface& device) +CmdResult defaultOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2444,7 +2444,7 @@ void extract(Serializer& serializer, GetEventSupport::Info& self) } -TypedResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut) +CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2454,7 +2454,7 @@ TypedResult getEventSupport(C::mip_interface& device, GetEventS assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_SUPPORT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_SUPPORT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_SUPPORT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_SUPPORT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2515,7 +2515,7 @@ void extract(Serializer& serializer, EventControl::Response& self) } -TypedResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode) +CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2529,7 +2529,7 @@ TypedResult writeEventControl(C::mip_interface& device, uint8_t in return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut) +CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2540,7 +2540,7 @@ TypedResult readEventControl(C::mip_interface& device, uint8_t ins assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2556,7 +2556,7 @@ TypedResult readEventControl(C::mip_interface& device, uint8_t ins } return result; } -TypedResult saveEventControl(C::mip_interface& device, uint8_t instance) +CmdResult saveEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2568,7 +2568,7 @@ TypedResult saveEventControl(C::mip_interface& device, uint8_t ins return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadEventControl(C::mip_interface& device, uint8_t instance) +CmdResult loadEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2580,7 +2580,7 @@ TypedResult loadEventControl(C::mip_interface& device, uint8_t ins return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultEventControl(C::mip_interface& device, uint8_t instance) +CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2639,7 +2639,7 @@ void extract(Serializer& serializer, GetEventTriggerStatus::Entry& self) } -TypedResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut) +CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2653,7 +2653,7 @@ TypedResult getEventTriggerStatus(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_STATUS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2716,7 +2716,7 @@ void extract(Serializer& serializer, GetEventActionStatus::Entry& self) } -TypedResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut) +CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2730,7 +2730,7 @@ TypedResult getEventActionStatus(C::mip_interface& device, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_STATUS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2941,7 +2941,7 @@ void extract(Serializer& serializer, EventTrigger::CombinationParams& self) } -TypedResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters) +CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2970,7 +2970,7 @@ TypedResult writeEventTrigger(C::mip_interface& device, uint8_t in return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut) +CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2981,7 +2981,7 @@ TypedResult readEventTrigger(C::mip_interface& device, uint8_t ins assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_CONFIG, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3012,7 +3012,7 @@ TypedResult readEventTrigger(C::mip_interface& device, uint8_t ins } return result; } -TypedResult saveEventTrigger(C::mip_interface& device, uint8_t instance) +CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3024,7 +3024,7 @@ TypedResult saveEventTrigger(C::mip_interface& device, uint8_t ins return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadEventTrigger(C::mip_interface& device, uint8_t instance) +CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3036,7 +3036,7 @@ TypedResult loadEventTrigger(C::mip_interface& device, uint8_t ins return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultEventTrigger(C::mip_interface& device, uint8_t instance) +CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3175,7 +3175,7 @@ void extract(Serializer& serializer, EventAction::MessageParams& self) } -TypedResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters) +CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3201,7 +3201,7 @@ TypedResult writeEventAction(C::mip_interface& device, uint8_t inst return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut) +CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3212,7 +3212,7 @@ TypedResult readEventAction(C::mip_interface& device, uint8_t insta assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_CONFIG, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3241,7 +3241,7 @@ TypedResult readEventAction(C::mip_interface& device, uint8_t insta } return result; } -TypedResult saveEventAction(C::mip_interface& device, uint8_t instance) +CmdResult saveEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3253,7 +3253,7 @@ TypedResult saveEventAction(C::mip_interface& device, uint8_t insta return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadEventAction(C::mip_interface& device, uint8_t instance) +CmdResult loadEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3265,7 +3265,7 @@ TypedResult loadEventAction(C::mip_interface& device, uint8_t insta return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultEventAction(C::mip_interface& device, uint8_t instance) +CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3313,7 +3313,7 @@ void extract(Serializer& serializer, AccelBias::Response& self) } -TypedResult writeAccelBias(C::mip_interface& device, const float* bias) +CmdResult writeAccelBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3327,7 +3327,7 @@ TypedResult writeAccelBias(C::mip_interface& device, const float* bia return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAccelBias(C::mip_interface& device, float* biasOut) +CmdResult readAccelBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3336,7 +3336,7 @@ TypedResult readAccelBias(C::mip_interface& device, float* biasOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_VECTOR, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3351,7 +3351,7 @@ TypedResult readAccelBias(C::mip_interface& device, float* biasOut) } return result; } -TypedResult saveAccelBias(C::mip_interface& device) +CmdResult saveAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3361,7 +3361,7 @@ TypedResult saveAccelBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAccelBias(C::mip_interface& device) +CmdResult loadAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3371,7 +3371,7 @@ TypedResult loadAccelBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAccelBias(C::mip_interface& device) +CmdResult defaultAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3417,7 +3417,7 @@ void extract(Serializer& serializer, GyroBias::Response& self) } -TypedResult writeGyroBias(C::mip_interface& device, const float* bias) +CmdResult writeGyroBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3431,7 +3431,7 @@ TypedResult writeGyroBias(C::mip_interface& device, const float* bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGyroBias(C::mip_interface& device, float* biasOut) +CmdResult readGyroBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3440,7 +3440,7 @@ TypedResult readGyroBias(C::mip_interface& device, float* biasOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3455,7 +3455,7 @@ TypedResult readGyroBias(C::mip_interface& device, float* biasOut) } return result; } -TypedResult saveGyroBias(C::mip_interface& device) +CmdResult saveGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3465,7 +3465,7 @@ TypedResult saveGyroBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGyroBias(C::mip_interface& device) +CmdResult loadGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3475,7 +3475,7 @@ TypedResult loadGyroBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGyroBias(C::mip_interface& device) +CmdResult defaultGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3509,7 +3509,7 @@ void extract(Serializer& serializer, CaptureGyroBias::Response& self) } -TypedResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) +CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3519,7 +3519,7 @@ TypedResult captureGyroBias(C::mip_interface& device, uint16_t assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CAPTURE_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CAPTURE_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3570,7 +3570,7 @@ void extract(Serializer& serializer, MagHardIronOffset::Response& self) } -TypedResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) +CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3584,7 +3584,7 @@ TypedResult writeMagHardIronOffset(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) +CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3593,7 +3593,7 @@ TypedResult readMagHardIronOffset(C::mip_interface& device, f assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_VECTOR, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3608,7 +3608,7 @@ TypedResult readMagHardIronOffset(C::mip_interface& device, f } return result; } -TypedResult saveMagHardIronOffset(C::mip_interface& device) +CmdResult saveMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3618,7 +3618,7 @@ TypedResult saveMagHardIronOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMagHardIronOffset(C::mip_interface& device) +CmdResult loadMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3628,7 +3628,7 @@ TypedResult loadMagHardIronOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMagHardIronOffset(C::mip_interface& device) +CmdResult defaultMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3674,7 +3674,7 @@ void extract(Serializer& serializer, MagSoftIronMatrix::Response& self) } -TypedResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) +CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3688,7 +3688,7 @@ TypedResult writeMagSoftIronMatrix(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) +CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3697,7 +3697,7 @@ TypedResult readMagSoftIronMatrix(C::mip_interface& device, f assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_COMP_MATRIX, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_COMP_MATRIX, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3712,7 +3712,7 @@ TypedResult readMagSoftIronMatrix(C::mip_interface& device, f } return result; } -TypedResult saveMagSoftIronMatrix(C::mip_interface& device) +CmdResult saveMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3722,7 +3722,7 @@ TypedResult saveMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMagSoftIronMatrix(C::mip_interface& device) +CmdResult loadMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3732,7 +3732,7 @@ TypedResult loadMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMagSoftIronMatrix(C::mip_interface& device) +CmdResult defaultMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3774,7 +3774,7 @@ void extract(Serializer& serializer, ConingScullingEnable::Response& self) } -TypedResult writeConingScullingEnable(C::mip_interface& device, bool enable) +CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3786,7 +3786,7 @@ TypedResult writeConingScullingEnable(C::mip_interface& de return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) +CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3795,7 +3795,7 @@ TypedResult readConingScullingEnable(C::mip_interface& dev assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3809,7 +3809,7 @@ TypedResult readConingScullingEnable(C::mip_interface& dev } return result; } -TypedResult saveConingScullingEnable(C::mip_interface& device) +CmdResult saveConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3819,7 +3819,7 @@ TypedResult saveConingScullingEnable(C::mip_interface& dev return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadConingScullingEnable(C::mip_interface& device) +CmdResult loadConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3829,7 +3829,7 @@ TypedResult loadConingScullingEnable(C::mip_interface& dev return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultConingScullingEnable(C::mip_interface& device) +CmdResult defaultConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3887,7 +3887,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformEuler::Response& sel } -TypedResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw) +CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3903,7 +3903,7 @@ TypedResult writeSensor2VehicleTransformEuler(C::m return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) +CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3912,7 +3912,7 @@ TypedResult readSensor2VehicleTransformEuler(C::mi assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3932,7 +3932,7 @@ TypedResult readSensor2VehicleTransformEuler(C::mi } return result; } -TypedResult saveSensor2VehicleTransformEuler(C::mip_interface& device) +CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3942,7 +3942,7 @@ TypedResult saveSensor2VehicleTransformEuler(C::mi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensor2VehicleTransformEuler(C::mip_interface& device) +CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3952,7 +3952,7 @@ TypedResult loadSensor2VehicleTransformEuler(C::mi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensor2VehicleTransformEuler(C::mip_interface& device) +CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3998,7 +3998,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response } -TypedResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) +CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4012,7 +4012,7 @@ TypedResult writeSensor2VehicleTransformQuate return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) +CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4021,7 +4021,7 @@ TypedResult readSensor2VehicleTransformQuater assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4036,7 +4036,7 @@ TypedResult readSensor2VehicleTransformQuater } return result; } -TypedResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) +CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4046,7 +4046,7 @@ TypedResult saveSensor2VehicleTransformQuater return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) +CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4056,7 +4056,7 @@ TypedResult loadSensor2VehicleTransformQuater return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device) +CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4102,7 +4102,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self) } -TypedResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) +CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4116,7 +4116,7 @@ TypedResult writeSensor2VehicleTransformDcm(C::mip_i return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) +CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4125,7 +4125,7 @@ TypedResult readSensor2VehicleTransformDcm(C::mip_in assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4140,7 +4140,7 @@ TypedResult readSensor2VehicleTransformDcm(C::mip_in } return result; } -TypedResult saveSensor2VehicleTransformDcm(C::mip_interface& device) +CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4150,7 +4150,7 @@ TypedResult saveSensor2VehicleTransformDcm(C::mip_in return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensor2VehicleTransformDcm(C::mip_interface& device) +CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4160,7 +4160,7 @@ TypedResult loadSensor2VehicleTransformDcm(C::mip_in return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensor2VehicleTransformDcm(C::mip_interface& device) +CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4226,7 +4226,7 @@ void extract(Serializer& serializer, ComplementaryFilter::Response& self) } -TypedResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant) +CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4244,7 +4244,7 @@ TypedResult writeComplementaryFilter(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut) +CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4253,7 +4253,7 @@ TypedResult readComplementaryFilter(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LEGACY_COMP_FILTER, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LEGACY_COMP_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4276,7 +4276,7 @@ TypedResult readComplementaryFilter(C::mip_interface& devic } return result; } -TypedResult saveComplementaryFilter(C::mip_interface& device) +CmdResult saveComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4286,7 +4286,7 @@ TypedResult saveComplementaryFilter(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadComplementaryFilter(C::mip_interface& device) +CmdResult loadComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4296,7 +4296,7 @@ TypedResult loadComplementaryFilter(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultComplementaryFilter(C::mip_interface& device) +CmdResult defaultComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4346,7 +4346,7 @@ void extract(Serializer& serializer, SensorRange::Response& self) } -TypedResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting) +CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4360,7 +4360,7 @@ TypedResult writeSensorRange(C::mip_interface& device, SensorRangeT return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut) +CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4371,7 +4371,7 @@ TypedResult readSensorRange(C::mip_interface& device, SensorRangeTy assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_RANGE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_RANGE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4387,7 +4387,7 @@ TypedResult readSensorRange(C::mip_interface& device, SensorRangeTy } return result; } -TypedResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) +CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4399,7 +4399,7 @@ TypedResult saveSensorRange(C::mip_interface& device, SensorRangeTy return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) +CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4411,7 +4411,7 @@ TypedResult loadSensorRange(C::mip_interface& device, SensorRangeTy return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor) +CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4469,7 +4469,7 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Entry& self) } -TypedResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut) +CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4479,7 +4479,7 @@ TypedResult calibratedSensorRanges(C::mip_interface& dev assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CALIBRATED_RANGES, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CALIBRATED_RANGES, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CALIBRATED_RANGES, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CALIBRATED_RANGES, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4561,7 +4561,7 @@ void extract(Serializer& serializer, LowpassFilter::Response& self) } -TypedResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) +CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4581,7 +4581,7 @@ TypedResult writeLowpassFilter(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) +CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4594,7 +4594,7 @@ TypedResult readLowpassFilter(C::mip_interface& device, uint8_t d assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LOWPASS_FILTER, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LOWPASS_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4618,7 +4618,7 @@ TypedResult readLowpassFilter(C::mip_interface& device, uint8_t d } return result; } -TypedResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4632,7 +4632,7 @@ TypedResult saveLowpassFilter(C::mip_interface& device, uint8_t d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4646,7 +4646,7 @@ TypedResult loadLowpassFilter(C::mip_interface& device, uint8_t d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 28987157f..dbbe7447a 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -213,8 +213,6 @@ struct PollImuMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PollImuMessage"; - static constexpr const char* DOC_NAME = "PollImuMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -233,7 +231,7 @@ struct PollImuMessage void insert(Serializer& serializer, const PollImuMessage& self); void extract(Serializer& serializer, PollImuMessage& self); -TypedResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -259,8 +257,6 @@ struct PollGnssMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PollGnssMessage"; - static constexpr const char* DOC_NAME = "PollGnssMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -279,7 +275,7 @@ struct PollGnssMessage void insert(Serializer& serializer, const PollGnssMessage& self); void extract(Serializer& serializer, PollGnssMessage& self); -TypedResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -305,8 +301,6 @@ struct PollFilterMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PollFilterMessage"; - static constexpr const char* DOC_NAME = "PollFilterMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -325,7 +319,7 @@ struct PollFilterMessage void insert(Serializer& serializer, const PollFilterMessage& self); void extract(Serializer& serializer, PollFilterMessage& self); -TypedResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -346,8 +340,6 @@ struct ImuMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ImuMessageFormat"; - static constexpr const char* DOC_NAME = "ImuMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -380,8 +372,6 @@ struct ImuMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ImuMessageFormat::Response"; - static constexpr const char* DOC_NAME = "ImuMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -401,11 +391,11 @@ void extract(Serializer& serializer, ImuMessageFormat& self); void insert(Serializer& serializer, const ImuMessageFormat::Response& self); void extract(Serializer& serializer, ImuMessageFormat::Response& self); -TypedResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -TypedResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -TypedResult saveImuMessageFormat(C::mip_interface& device); -TypedResult loadImuMessageFormat(C::mip_interface& device); -TypedResult defaultImuMessageFormat(C::mip_interface& device); +CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +CmdResult saveImuMessageFormat(C::mip_interface& device); +CmdResult loadImuMessageFormat(C::mip_interface& device); +CmdResult defaultImuMessageFormat(C::mip_interface& device); ///@} /// @@ -426,8 +416,6 @@ struct GpsMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsMessageFormat"; - static constexpr const char* DOC_NAME = "GpsMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -460,8 +448,6 @@ struct GpsMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsMessageFormat::Response"; - static constexpr const char* DOC_NAME = "GpsMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -481,11 +467,11 @@ void extract(Serializer& serializer, GpsMessageFormat& self); void insert(Serializer& serializer, const GpsMessageFormat::Response& self); void extract(Serializer& serializer, GpsMessageFormat::Response& self); -TypedResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -TypedResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -TypedResult saveGpsMessageFormat(C::mip_interface& device); -TypedResult loadGpsMessageFormat(C::mip_interface& device); -TypedResult defaultGpsMessageFormat(C::mip_interface& device); +CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +CmdResult saveGpsMessageFormat(C::mip_interface& device); +CmdResult loadGpsMessageFormat(C::mip_interface& device); +CmdResult defaultGpsMessageFormat(C::mip_interface& device); ///@} /// @@ -506,8 +492,6 @@ struct FilterMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "FilterMessageFormat"; - static constexpr const char* DOC_NAME = "FilterMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -540,8 +524,6 @@ struct FilterMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "FilterMessageFormat::Response"; - static constexpr const char* DOC_NAME = "FilterMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -561,11 +543,11 @@ void extract(Serializer& serializer, FilterMessageFormat& self); void insert(Serializer& serializer, const FilterMessageFormat::Response& self); void extract(Serializer& serializer, FilterMessageFormat::Response& self); -TypedResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -TypedResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -TypedResult saveFilterMessageFormat(C::mip_interface& device); -TypedResult loadFilterMessageFormat(C::mip_interface& device); -TypedResult defaultFilterMessageFormat(C::mip_interface& device); +CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +CmdResult saveFilterMessageFormat(C::mip_interface& device); +CmdResult loadFilterMessageFormat(C::mip_interface& device); +CmdResult defaultFilterMessageFormat(C::mip_interface& device); ///@} /// @@ -584,8 +566,6 @@ struct ImuGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ImuGetBaseRate"; - static constexpr const char* DOC_NAME = "Get IMU Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -605,8 +585,6 @@ struct ImuGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ImuGetBaseRate::Response"; - static constexpr const char* DOC_NAME = "Get IMU Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -625,7 +603,7 @@ void extract(Serializer& serializer, ImuGetBaseRate& self); void insert(Serializer& serializer, const ImuGetBaseRate::Response& self); void extract(Serializer& serializer, ImuGetBaseRate::Response& self); -TypedResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -644,8 +622,6 @@ struct GpsGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsGetBaseRate"; - static constexpr const char* DOC_NAME = "Get GNSS Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -665,8 +641,6 @@ struct GpsGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsGetBaseRate::Response"; - static constexpr const char* DOC_NAME = "Get GNSS Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -685,7 +659,7 @@ void extract(Serializer& serializer, GpsGetBaseRate& self); void insert(Serializer& serializer, const GpsGetBaseRate::Response& self); void extract(Serializer& serializer, GpsGetBaseRate::Response& self); -TypedResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -704,8 +678,6 @@ struct FilterGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "FilterGetBaseRate"; - static constexpr const char* DOC_NAME = "Get Estimation Filter Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -725,8 +697,6 @@ struct FilterGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "FilterGetBaseRate::Response"; - static constexpr const char* DOC_NAME = "Get Estimation Filter Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -745,7 +715,7 @@ void extract(Serializer& serializer, FilterGetBaseRate& self); void insert(Serializer& serializer, const FilterGetBaseRate::Response& self); void extract(Serializer& serializer, FilterGetBaseRate::Response& self); -TypedResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -772,8 +742,6 @@ struct PollData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PollData"; - static constexpr const char* DOC_NAME = "PollData"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -792,7 +760,7 @@ struct PollData void insert(Serializer& serializer, const PollData& self); void extract(Serializer& serializer, PollData& self); -TypedResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors); +CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors); ///@} /// @@ -809,8 +777,6 @@ struct GetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetBaseRate"; - static constexpr const char* DOC_NAME = "Get Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -830,8 +796,6 @@ struct GetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetBaseRate::Response"; - static constexpr const char* DOC_NAME = "Get Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -851,7 +815,7 @@ void extract(Serializer& serializer, GetBaseRate& self); void insert(Serializer& serializer, const GetBaseRate::Response& self); void extract(Serializer& serializer, GetBaseRate::Response& self); -TypedResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut); +CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut); ///@} /// @@ -873,8 +837,6 @@ struct MessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MessageFormat"; - static constexpr const char* DOC_NAME = "MessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -908,8 +870,6 @@ struct MessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MessageFormat::Response"; - static constexpr const char* DOC_NAME = "MessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -930,11 +890,11 @@ void extract(Serializer& serializer, MessageFormat& self); void insert(Serializer& serializer, const MessageFormat::Response& self); void extract(Serializer& serializer, MessageFormat::Response& self); -TypedResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors); -TypedResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -TypedResult saveMessageFormat(C::mip_interface& device, uint8_t descSet); -TypedResult loadMessageFormat(C::mip_interface& device, uint8_t descSet); -TypedResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); +CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors); +CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet); +CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet); +CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); ///@} /// @@ -959,8 +919,6 @@ struct NmeaPollData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "NmeaPollData"; - static constexpr const char* DOC_NAME = "NmeaPollData"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -979,7 +937,7 @@ struct NmeaPollData void insert(Serializer& serializer, const NmeaPollData& self); void extract(Serializer& serializer, NmeaPollData& self); -TypedResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries); +CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries); ///@} /// @@ -998,8 +956,6 @@ struct NmeaMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "NmeaMessageFormat"; - static constexpr const char* DOC_NAME = "NmeaMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1032,8 +988,6 @@ struct NmeaMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "NmeaMessageFormat::Response"; - static constexpr const char* DOC_NAME = "NmeaMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -1053,11 +1007,11 @@ void extract(Serializer& serializer, NmeaMessageFormat& self); void insert(Serializer& serializer, const NmeaMessageFormat::Response& self); void extract(Serializer& serializer, NmeaMessageFormat::Response& self); -TypedResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries); -TypedResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut); -TypedResult saveNmeaMessageFormat(C::mip_interface& device); -TypedResult loadNmeaMessageFormat(C::mip_interface& device); -TypedResult defaultNmeaMessageFormat(C::mip_interface& device); +CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries); +CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut); +CmdResult saveNmeaMessageFormat(C::mip_interface& device); +CmdResult loadNmeaMessageFormat(C::mip_interface& device); +CmdResult defaultNmeaMessageFormat(C::mip_interface& device); ///@} /// @@ -1078,8 +1032,6 @@ struct DeviceSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DeviceSettings"; - static constexpr const char* DOC_NAME = "DeviceSettings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x0000; @@ -1111,9 +1063,9 @@ struct DeviceSettings void insert(Serializer& serializer, const DeviceSettings& self); void extract(Serializer& serializer, DeviceSettings& self); -TypedResult saveDeviceSettings(C::mip_interface& device); -TypedResult loadDeviceSettings(C::mip_interface& device); -TypedResult defaultDeviceSettings(C::mip_interface& device); +CmdResult saveDeviceSettings(C::mip_interface& device); +CmdResult loadDeviceSettings(C::mip_interface& device); +CmdResult defaultDeviceSettings(C::mip_interface& device); ///@} /// @@ -1145,8 +1097,6 @@ struct UartBaudrate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "UartBaudrate"; - static constexpr const char* DOC_NAME = "UartBaudrate"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1179,8 +1129,6 @@ struct UartBaudrate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "UartBaudrate::Response"; - static constexpr const char* DOC_NAME = "UartBaudrate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1199,11 +1147,11 @@ void extract(Serializer& serializer, UartBaudrate& self); void insert(Serializer& serializer, const UartBaudrate::Response& self); void extract(Serializer& serializer, UartBaudrate::Response& self); -TypedResult writeUartBaudrate(C::mip_interface& device, uint32_t baud); -TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut); -TypedResult saveUartBaudrate(C::mip_interface& device); -TypedResult loadUartBaudrate(C::mip_interface& device); -TypedResult defaultUartBaudrate(C::mip_interface& device); +CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud); +CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut); +CmdResult saveUartBaudrate(C::mip_interface& device); +CmdResult loadUartBaudrate(C::mip_interface& device); +CmdResult defaultUartBaudrate(C::mip_interface& device); ///@} /// @@ -1231,8 +1179,6 @@ struct FactoryStreaming static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "FactoryStreaming"; - static constexpr const char* DOC_NAME = "FactoryStreaming"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1251,7 +1197,7 @@ struct FactoryStreaming void insert(Serializer& serializer, const FactoryStreaming& self); void extract(Serializer& serializer, FactoryStreaming& self); -TypedResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved); +CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved); ///@} /// @@ -1279,8 +1225,6 @@ struct DatastreamControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DatastreamControl"; - static constexpr const char* DOC_NAME = "DatastreamControl"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1314,8 +1258,6 @@ struct DatastreamControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DatastreamControl::Response"; - static constexpr const char* DOC_NAME = "DatastreamControl Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1335,11 +1277,11 @@ void extract(Serializer& serializer, DatastreamControl& self); void insert(Serializer& serializer, const DatastreamControl::Response& self); void extract(Serializer& serializer, DatastreamControl::Response& self); -TypedResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable); -TypedResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut); -TypedResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet); -TypedResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet); -TypedResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); +CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable); +CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut); +CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet); +CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet); +CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); ///@} /// @@ -1392,7 +1334,7 @@ struct ConstellationSettings OptionFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } OptionFlags& operator=(uint16_t val) { value = val; return *this; } - OptionFlags& operator=(int val) { value = uint16_t(val); return *this; } + OptionFlags& operator=(int val) { value = val; return *this; } OptionFlags& operator|=(uint16_t val) { return *this = value | val; } OptionFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1420,8 +1362,6 @@ struct ConstellationSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ConstellationSettings"; - static constexpr const char* DOC_NAME = "ConstellationSettings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -1454,8 +1394,6 @@ struct ConstellationSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ConstellationSettings::Response"; - static constexpr const char* DOC_NAME = "ConstellationSettings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -1480,11 +1418,11 @@ void extract(Serializer& serializer, ConstellationSettings::Settings& self); void insert(Serializer& serializer, const ConstellationSettings::Response& self); void extract(Serializer& serializer, ConstellationSettings::Response& self); -TypedResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); -TypedResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); -TypedResult saveConstellationSettings(C::mip_interface& device); -TypedResult loadConstellationSettings(C::mip_interface& device); -TypedResult defaultConstellationSettings(C::mip_interface& device); +CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); +CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); +CmdResult saveConstellationSettings(C::mip_interface& device); +CmdResult loadConstellationSettings(C::mip_interface& device); +CmdResult defaultConstellationSettings(C::mip_interface& device); ///@} /// @@ -1515,7 +1453,7 @@ struct GnssSbasSettings SBASOptions(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } SBASOptions& operator=(uint16_t val) { value = val; return *this; } - SBASOptions& operator=(int val) { value = uint16_t(val); return *this; } + SBASOptions& operator=(int val) { value = val; return *this; } SBASOptions& operator|=(uint16_t val) { return *this = value | val; } SBASOptions& operator&=(uint16_t val) { return *this = value & val; } @@ -1539,8 +1477,6 @@ struct GnssSbasSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssSbasSettings"; - static constexpr const char* DOC_NAME = "SBAS Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -1573,8 +1509,6 @@ struct GnssSbasSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssSbasSettings::Response"; - static constexpr const char* DOC_NAME = "SBAS Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -1596,11 +1530,11 @@ void extract(Serializer& serializer, GnssSbasSettings& self); void insert(Serializer& serializer, const GnssSbasSettings::Response& self); void extract(Serializer& serializer, GnssSbasSettings::Response& self); -TypedResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns); -TypedResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut); -TypedResult saveGnssSbasSettings(C::mip_interface& device); -TypedResult loadGnssSbasSettings(C::mip_interface& device); -TypedResult defaultGnssSbasSettings(C::mip_interface& device); +CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns); +CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut); +CmdResult saveGnssSbasSettings(C::mip_interface& device); +CmdResult loadGnssSbasSettings(C::mip_interface& device); +CmdResult defaultGnssSbasSettings(C::mip_interface& device); ///@} /// @@ -1635,8 +1569,6 @@ struct GnssAssistedFix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssAssistedFix"; - static constexpr const char* DOC_NAME = "GNSS Assisted Fix Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1669,8 +1601,6 @@ struct GnssAssistedFix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssAssistedFix::Response"; - static constexpr const char* DOC_NAME = "GNSS Assisted Fix Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1690,11 +1620,11 @@ void extract(Serializer& serializer, GnssAssistedFix& self); void insert(Serializer& serializer, const GnssAssistedFix::Response& self); void extract(Serializer& serializer, GnssAssistedFix::Response& self); -TypedResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); -TypedResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); -TypedResult saveGnssAssistedFix(C::mip_interface& device); -TypedResult loadGnssAssistedFix(C::mip_interface& device); -TypedResult defaultGnssAssistedFix(C::mip_interface& device); +CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); +CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); +CmdResult saveGnssAssistedFix(C::mip_interface& device); +CmdResult loadGnssAssistedFix(C::mip_interface& device); +CmdResult defaultGnssAssistedFix(C::mip_interface& device); ///@} /// @@ -1717,8 +1647,6 @@ struct GnssTimeAssistance static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssTimeAssistance"; - static constexpr const char* DOC_NAME = "GnssTimeAssistance"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -1751,8 +1679,6 @@ struct GnssTimeAssistance static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssTimeAssistance::Response"; - static constexpr const char* DOC_NAME = "GnssTimeAssistance Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1773,8 +1699,8 @@ void extract(Serializer& serializer, GnssTimeAssistance& self); void insert(Serializer& serializer, const GnssTimeAssistance::Response& self); void extract(Serializer& serializer, GnssTimeAssistance::Response& self); -TypedResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy); -TypedResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut); +CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy); +CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut); ///@} /// @@ -1811,8 +1737,6 @@ struct ImuLowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ImuLowpassFilter"; - static constexpr const char* DOC_NAME = "Advanced Low-Pass Filter Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -1846,8 +1770,6 @@ struct ImuLowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ImuLowpassFilter::Response"; - static constexpr const char* DOC_NAME = "Advanced Low-Pass Filter Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1870,11 +1792,11 @@ void extract(Serializer& serializer, ImuLowpassFilter& self); void insert(Serializer& serializer, const ImuLowpassFilter::Response& self); void extract(Serializer& serializer, ImuLowpassFilter::Response& self); -TypedResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); -TypedResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); -TypedResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -TypedResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -TypedResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); +CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); +CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); ///@} /// @@ -1901,8 +1823,6 @@ struct PpsSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PpsSource"; - static constexpr const char* DOC_NAME = "PpsSource"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1935,8 +1855,6 @@ struct PpsSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PpsSource::Response"; - static constexpr const char* DOC_NAME = "PpsSource Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1955,11 +1873,11 @@ void extract(Serializer& serializer, PpsSource& self); void insert(Serializer& serializer, const PpsSource::Response& self); void extract(Serializer& serializer, PpsSource::Response& self); -TypedResult writePpsSource(C::mip_interface& device, PpsSource::Source source); -TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut); -TypedResult savePpsSource(C::mip_interface& device); -TypedResult loadPpsSource(C::mip_interface& device); -TypedResult defaultPpsSource(C::mip_interface& device); +CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source); +CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut); +CmdResult savePpsSource(C::mip_interface& device); +CmdResult loadPpsSource(C::mip_interface& device); +CmdResult defaultPpsSource(C::mip_interface& device); ///@} /// @@ -2032,7 +1950,7 @@ struct GpioConfig PinMode(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } PinMode& operator=(uint8_t val) { value = val; return *this; } - PinMode& operator=(int val) { value = uint8_t(val); return *this; } + PinMode& operator=(int val) { value = val; return *this; } PinMode& operator|=(uint8_t val) { return *this = value | val; } PinMode& operator&=(uint8_t val) { return *this = value & val; } @@ -2056,8 +1974,6 @@ struct GpioConfig static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpioConfig"; - static constexpr const char* DOC_NAME = "GPIO Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2091,8 +2007,6 @@ struct GpioConfig static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpioConfig::Response"; - static constexpr const char* DOC_NAME = "GPIO Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2114,11 +2028,11 @@ void extract(Serializer& serializer, GpioConfig& self); void insert(Serializer& serializer, const GpioConfig::Response& self); void extract(Serializer& serializer, GpioConfig::Response& self); -TypedResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode); -TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut); -TypedResult saveGpioConfig(C::mip_interface& device, uint8_t pin); -TypedResult loadGpioConfig(C::mip_interface& device, uint8_t pin); -TypedResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); +CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode); +CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut); +CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin); +CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin); +CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); ///@} /// @@ -2152,8 +2066,6 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpioState"; - static constexpr const char* DOC_NAME = "GPIO State"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2186,8 +2098,6 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpioState::Response"; - static constexpr const char* DOC_NAME = "GPIO State Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2207,8 +2117,8 @@ void extract(Serializer& serializer, GpioState& self); void insert(Serializer& serializer, const GpioState::Response& self); void extract(Serializer& serializer, GpioState::Response& self); -TypedResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state); -TypedResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); +CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state); +CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); ///@} /// @@ -2235,8 +2145,6 @@ struct Odometer static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Odometer"; - static constexpr const char* DOC_NAME = "Odometer Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -2269,8 +2177,6 @@ struct Odometer static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Odometer::Response"; - static constexpr const char* DOC_NAME = "Odometer Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2291,11 +2197,11 @@ void extract(Serializer& serializer, Odometer& self); void insert(Serializer& serializer, const Odometer::Response& self); void extract(Serializer& serializer, Odometer::Response& self); -TypedResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty); -TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut); -TypedResult saveOdometer(C::mip_interface& device); -TypedResult loadOdometer(C::mip_interface& device); -TypedResult defaultOdometer(C::mip_interface& device); +CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty); +CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut); +CmdResult saveOdometer(C::mip_interface& device); +CmdResult loadOdometer(C::mip_interface& device); +CmdResult defaultOdometer(C::mip_interface& device); ///@} /// @@ -2340,8 +2246,6 @@ struct GetEventSupport static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetEventSupport"; - static constexpr const char* DOC_NAME = "Get Supported Events"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -2361,8 +2265,6 @@ struct GetEventSupport static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetEventSupport::Response"; - static constexpr const char* DOC_NAME = "Get Supported Events Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -2387,7 +2289,7 @@ void extract(Serializer& serializer, GetEventSupport::Info& self); void insert(Serializer& serializer, const GetEventSupport::Response& self); void extract(Serializer& serializer, GetEventSupport::Response& self); -TypedResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut); +CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut); ///@} /// @@ -2423,8 +2325,6 @@ struct EventControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EventControl"; - static constexpr const char* DOC_NAME = "Event Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2458,8 +2358,6 @@ struct EventControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EventControl::Response"; - static constexpr const char* DOC_NAME = "Event Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2479,11 +2377,11 @@ void extract(Serializer& serializer, EventControl& self); void insert(Serializer& serializer, const EventControl::Response& self); void extract(Serializer& serializer, EventControl::Response& self); -TypedResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode); -TypedResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut); -TypedResult saveEventControl(C::mip_interface& device, uint8_t instance); -TypedResult loadEventControl(C::mip_interface& device, uint8_t instance); -TypedResult defaultEventControl(C::mip_interface& device, uint8_t instance); +CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode); +CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut); +CmdResult saveEventControl(C::mip_interface& device, uint8_t instance); +CmdResult loadEventControl(C::mip_interface& device, uint8_t instance); +CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2510,7 +2408,7 @@ struct GetEventTriggerStatus Status(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } Status& operator=(uint8_t val) { value = val; return *this; } - Status& operator=(int val) { value = uint8_t(val); return *this; } + Status& operator=(int val) { value = val; return *this; } Status& operator|=(uint8_t val) { return *this = value | val; } Status& operator&=(uint8_t val) { return *this = value & val; } @@ -2537,8 +2435,6 @@ struct GetEventTriggerStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetEventTriggerStatus"; - static constexpr const char* DOC_NAME = "Get Trigger Status"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -2558,8 +2454,6 @@ struct GetEventTriggerStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetEventTriggerStatus::Response"; - static constexpr const char* DOC_NAME = "Get Trigger Status Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -2582,7 +2476,7 @@ void extract(Serializer& serializer, GetEventTriggerStatus::Entry& self); void insert(Serializer& serializer, const GetEventTriggerStatus::Response& self); void extract(Serializer& serializer, GetEventTriggerStatus::Response& self); -TypedResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut); +CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut); ///@} /// @@ -2605,8 +2499,6 @@ struct GetEventActionStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetEventActionStatus"; - static constexpr const char* DOC_NAME = "Get Action Status"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -2626,8 +2518,6 @@ struct GetEventActionStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetEventActionStatus::Response"; - static constexpr const char* DOC_NAME = "Get Action Status Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -2650,7 +2540,7 @@ void extract(Serializer& serializer, GetEventActionStatus::Entry& self); void insert(Serializer& serializer, const GetEventActionStatus::Response& self); void extract(Serializer& serializer, GetEventActionStatus::Response& self); -TypedResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut); +CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut); ///@} /// @@ -2743,8 +2633,6 @@ struct EventTrigger static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EventTrigger"; - static constexpr const char* DOC_NAME = "Event Trigger Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -2778,8 +2666,6 @@ struct EventTrigger static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EventTrigger::Response"; - static constexpr const char* DOC_NAME = "Event Trigger Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2809,11 +2695,11 @@ void extract(Serializer& serializer, EventTrigger::CombinationParams& self); void insert(Serializer& serializer, const EventTrigger::Response& self); void extract(Serializer& serializer, EventTrigger::Response& self); -TypedResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters); -TypedResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut); -TypedResult saveEventTrigger(C::mip_interface& device, uint8_t instance); -TypedResult loadEventTrigger(C::mip_interface& device, uint8_t instance); -TypedResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); +CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters); +CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut); +CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance); +CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance); +CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2872,8 +2758,6 @@ struct EventAction static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EventAction"; - static constexpr const char* DOC_NAME = "Event Action Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2907,8 +2791,6 @@ struct EventAction static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EventAction::Response"; - static constexpr const char* DOC_NAME = "Event Action Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2936,11 +2818,11 @@ void extract(Serializer& serializer, EventAction::MessageParams& self); void insert(Serializer& serializer, const EventAction::Response& self); void extract(Serializer& serializer, EventAction::Response& self); -TypedResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters); -TypedResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut); -TypedResult saveEventAction(C::mip_interface& device, uint8_t instance); -TypedResult loadEventAction(C::mip_interface& device, uint8_t instance); -TypedResult defaultEventAction(C::mip_interface& device, uint8_t instance); +CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters); +CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut); +CmdResult saveEventAction(C::mip_interface& device, uint8_t instance); +CmdResult loadEventAction(C::mip_interface& device, uint8_t instance); +CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2960,8 +2842,6 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelBias"; - static constexpr const char* DOC_NAME = "Configure Accel Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2994,8 +2874,6 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelBias::Response"; - static constexpr const char* DOC_NAME = "Configure Accel Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3014,11 +2892,11 @@ void extract(Serializer& serializer, AccelBias& self); void insert(Serializer& serializer, const AccelBias::Response& self); void extract(Serializer& serializer, AccelBias::Response& self); -TypedResult writeAccelBias(C::mip_interface& device, const float* bias); -TypedResult readAccelBias(C::mip_interface& device, float* biasOut); -TypedResult saveAccelBias(C::mip_interface& device); -TypedResult loadAccelBias(C::mip_interface& device); -TypedResult defaultAccelBias(C::mip_interface& device); +CmdResult writeAccelBias(C::mip_interface& device, const float* bias); +CmdResult readAccelBias(C::mip_interface& device, float* biasOut); +CmdResult saveAccelBias(C::mip_interface& device); +CmdResult loadAccelBias(C::mip_interface& device); +CmdResult defaultAccelBias(C::mip_interface& device); ///@} /// @@ -3038,8 +2916,6 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroBias"; - static constexpr const char* DOC_NAME = "Configure Gyro Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3072,8 +2948,6 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroBias::Response"; - static constexpr const char* DOC_NAME = "Configure Gyro Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3092,11 +2966,11 @@ void extract(Serializer& serializer, GyroBias& self); void insert(Serializer& serializer, const GyroBias::Response& self); void extract(Serializer& serializer, GyroBias::Response& self); -TypedResult writeGyroBias(C::mip_interface& device, const float* bias); -TypedResult readGyroBias(C::mip_interface& device, float* biasOut); -TypedResult saveGyroBias(C::mip_interface& device); -TypedResult loadGyroBias(C::mip_interface& device); -TypedResult defaultGyroBias(C::mip_interface& device); +CmdResult writeGyroBias(C::mip_interface& device, const float* bias); +CmdResult readGyroBias(C::mip_interface& device, float* biasOut); +CmdResult saveGyroBias(C::mip_interface& device); +CmdResult loadGyroBias(C::mip_interface& device); +CmdResult defaultGyroBias(C::mip_interface& device); ///@} /// @@ -3118,8 +2992,6 @@ struct CaptureGyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CaptureGyroBias"; - static constexpr const char* DOC_NAME = "Capture Gyro Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -3139,8 +3011,6 @@ struct CaptureGyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CaptureGyroBias::Response"; - static constexpr const char* DOC_NAME = "Capture Gyro Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3159,7 +3029,7 @@ void extract(Serializer& serializer, CaptureGyroBias& self); void insert(Serializer& serializer, const CaptureGyroBias::Response& self); void extract(Serializer& serializer, CaptureGyroBias::Response& self); -TypedResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); +CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); ///@} /// @@ -3183,8 +3053,6 @@ struct MagHardIronOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagHardIronOffset"; - static constexpr const char* DOC_NAME = "Magnetometer Hard Iron Offset"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3217,8 +3085,6 @@ struct MagHardIronOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagHardIronOffset::Response"; - static constexpr const char* DOC_NAME = "Magnetometer Hard Iron Offset Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3237,11 +3103,11 @@ void extract(Serializer& serializer, MagHardIronOffset& self); void insert(Serializer& serializer, const MagHardIronOffset::Response& self); void extract(Serializer& serializer, MagHardIronOffset::Response& self); -TypedResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); -TypedResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); -TypedResult saveMagHardIronOffset(C::mip_interface& device); -TypedResult loadMagHardIronOffset(C::mip_interface& device); -TypedResult defaultMagHardIronOffset(C::mip_interface& device); +CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); +CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); +CmdResult saveMagHardIronOffset(C::mip_interface& device); +CmdResult loadMagHardIronOffset(C::mip_interface& device); +CmdResult defaultMagHardIronOffset(C::mip_interface& device); ///@} /// @@ -3269,8 +3135,6 @@ struct MagSoftIronMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagSoftIronMatrix"; - static constexpr const char* DOC_NAME = "Magnetometer Soft Iron Matrix"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3303,8 +3167,6 @@ struct MagSoftIronMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagSoftIronMatrix::Response"; - static constexpr const char* DOC_NAME = "Magnetometer Soft Iron Matrix Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3323,11 +3185,11 @@ void extract(Serializer& serializer, MagSoftIronMatrix& self); void insert(Serializer& serializer, const MagSoftIronMatrix::Response& self); void extract(Serializer& serializer, MagSoftIronMatrix::Response& self); -TypedResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); -TypedResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); -TypedResult saveMagSoftIronMatrix(C::mip_interface& device); -TypedResult loadMagSoftIronMatrix(C::mip_interface& device); -TypedResult defaultMagSoftIronMatrix(C::mip_interface& device); +CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); +CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); +CmdResult saveMagSoftIronMatrix(C::mip_interface& device); +CmdResult loadMagSoftIronMatrix(C::mip_interface& device); +CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); ///@} /// @@ -3345,8 +3207,6 @@ struct ConingScullingEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ConingScullingEnable"; - static constexpr const char* DOC_NAME = "Coning and Sculling Enable"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3379,8 +3239,6 @@ struct ConingScullingEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ConingScullingEnable::Response"; - static constexpr const char* DOC_NAME = "Coning and Sculling Enable Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3399,11 +3257,11 @@ void extract(Serializer& serializer, ConingScullingEnable& self); void insert(Serializer& serializer, const ConingScullingEnable::Response& self); void extract(Serializer& serializer, ConingScullingEnable::Response& self); -TypedResult writeConingScullingEnable(C::mip_interface& device, bool enable); -TypedResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); -TypedResult saveConingScullingEnable(C::mip_interface& device); -TypedResult loadConingScullingEnable(C::mip_interface& device); -TypedResult defaultConingScullingEnable(C::mip_interface& device); +CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable); +CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); +CmdResult saveConingScullingEnable(C::mip_interface& device); +CmdResult loadConingScullingEnable(C::mip_interface& device); +CmdResult defaultConingScullingEnable(C::mip_interface& device); ///@} /// @@ -3447,8 +3305,6 @@ struct Sensor2VehicleTransformEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Sensor2VehicleTransformEuler"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Euler"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3481,8 +3337,6 @@ struct Sensor2VehicleTransformEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Sensor2VehicleTransformEuler::Response"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Euler Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3503,11 +3357,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformEuler& self); void insert(Serializer& serializer, const Sensor2VehicleTransformEuler::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformEuler::Response& self); -TypedResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw); -TypedResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); -TypedResult saveSensor2VehicleTransformEuler(C::mip_interface& device); -TypedResult loadSensor2VehicleTransformEuler(C::mip_interface& device); -TypedResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); +CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw); +CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); +CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device); +CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device); +CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); ///@} /// @@ -3557,8 +3411,6 @@ struct Sensor2VehicleTransformQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Sensor2VehicleTransformQuaternion"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Quaternion"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3591,8 +3443,6 @@ struct Sensor2VehicleTransformQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Sensor2VehicleTransformQuaternion::Response"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Quaternion Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3611,11 +3461,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion& self); void insert(Serializer& serializer, const Sensor2VehicleTransformQuaternion::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response& self); -TypedResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); -TypedResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); -TypedResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); -TypedResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); -TypedResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); +CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); +CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); +CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); +CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); +CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); ///@} /// @@ -3663,8 +3513,6 @@ struct Sensor2VehicleTransformDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Sensor2VehicleTransformDcm"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Direction Cosine Matrix"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3697,8 +3545,6 @@ struct Sensor2VehicleTransformDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Sensor2VehicleTransformDcm::Response"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Direction Cosine Matrix Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3717,11 +3563,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm& self); void insert(Serializer& serializer, const Sensor2VehicleTransformDcm::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self); -TypedResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); -TypedResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); -TypedResult saveSensor2VehicleTransformDcm(C::mip_interface& device); -TypedResult loadSensor2VehicleTransformDcm(C::mip_interface& device); -TypedResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); +CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); +CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); +CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device); +CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device); +CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); ///@} /// @@ -3746,8 +3592,6 @@ struct ComplementaryFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ComplementaryFilter"; - static constexpr const char* DOC_NAME = "Complementary filter settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -3780,8 +3624,6 @@ struct ComplementaryFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ComplementaryFilter::Response"; - static constexpr const char* DOC_NAME = "Complementary filter settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3803,11 +3645,11 @@ void extract(Serializer& serializer, ComplementaryFilter& self); void insert(Serializer& serializer, const ComplementaryFilter::Response& self); void extract(Serializer& serializer, ComplementaryFilter::Response& self); -TypedResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant); -TypedResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut); -TypedResult saveComplementaryFilter(C::mip_interface& device); -TypedResult loadComplementaryFilter(C::mip_interface& device); -TypedResult defaultComplementaryFilter(C::mip_interface& device); +CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant); +CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut); +CmdResult saveComplementaryFilter(C::mip_interface& device); +CmdResult loadComplementaryFilter(C::mip_interface& device); +CmdResult defaultComplementaryFilter(C::mip_interface& device); ///@} /// @@ -3833,8 +3675,6 @@ struct SensorRange static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorRange"; - static constexpr const char* DOC_NAME = "Sensor Range"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3868,8 +3708,6 @@ struct SensorRange static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorRange::Response"; - static constexpr const char* DOC_NAME = "Sensor Range Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3889,11 +3727,11 @@ void extract(Serializer& serializer, SensorRange& self); void insert(Serializer& serializer, const SensorRange::Response& self); void extract(Serializer& serializer, SensorRange::Response& self); -TypedResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting); -TypedResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut); -TypedResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor); -TypedResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor); -TypedResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); +CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting); +CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut); +CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor); +CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor); +CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); ///@} /// @@ -3919,8 +3757,6 @@ struct CalibratedSensorRanges static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CalibratedSensorRanges"; - static constexpr const char* DOC_NAME = "Get Calibrated Sensor Ranges"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -3940,8 +3776,6 @@ struct CalibratedSensorRanges static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CalibratedSensorRanges::Response"; - static constexpr const char* DOC_NAME = "Get Calibrated Sensor Ranges Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -3965,7 +3799,7 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Entry& self); void insert(Serializer& serializer, const CalibratedSensorRanges::Response& self); void extract(Serializer& serializer, CalibratedSensorRanges::Response& self); -TypedResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); +CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); ///@} /// @@ -4000,8 +3834,6 @@ struct LowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "LowpassFilter"; - static constexpr const char* DOC_NAME = "Low-pass anti-aliasing filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -4036,8 +3868,6 @@ struct LowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "LowpassFilter::Response"; - static constexpr const char* DOC_NAME = "Low-pass anti-aliasing filter Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4060,11 +3890,11 @@ void extract(Serializer& serializer, LowpassFilter& self); void insert(Serializer& serializer, const LowpassFilter::Response& self); void extract(Serializer& serializer, LowpassFilter::Response& self); -TypedResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); -TypedResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); -TypedResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -TypedResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -TypedResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); +CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); +CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); ///@} /// diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 9981eb802..a5eb60002 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -88,7 +88,7 @@ void extract(Serializer& serializer, SensorFrameMapping::Response& self) } -TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) +CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -102,7 +102,7 @@ TypedResult writeSensorFrameMapping(C::mip_interface& device return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) +CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -111,7 +111,7 @@ TypedResult readSensorFrameMapping(C::mip_interface& device, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -128,7 +128,7 @@ TypedResult readSensorFrameMapping(C::mip_interface& device, } return result; } -TypedResult saveSensorFrameMapping(C::mip_interface& device) +CmdResult saveSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -138,7 +138,7 @@ TypedResult saveSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensorFrameMapping(C::mip_interface& device) +CmdResult loadSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -148,7 +148,7 @@ TypedResult loadSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensorFrameMapping(C::mip_interface& device) +CmdResult defaultSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -256,7 +256,7 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) } } -TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -284,7 +284,7 @@ TypedResult writeReferenceFrame(C::mip_interface& device, uint8_ return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -297,7 +297,7 @@ TypedResult readReferenceFrame(C::mip_interface& device, uint8_t assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -326,7 +326,7 @@ TypedResult readReferenceFrame(C::mip_interface& device, uint8_t } return result; } -TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) +CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -338,7 +338,7 @@ TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) +CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -350,7 +350,7 @@ TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) +CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -394,7 +394,7 @@ void extract(Serializer& serializer, AidingEchoControl::Response& self) } -TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) +CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -406,7 +406,7 @@ TypedResult writeAidingEchoControl(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) +CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -415,7 +415,7 @@ TypedResult readAidingEchoControl(C::mip_interface& device, A assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -429,7 +429,7 @@ TypedResult readAidingEchoControl(C::mip_interface& device, A } return result; } -TypedResult saveAidingEchoControl(C::mip_interface& device) +CmdResult saveAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -439,7 +439,7 @@ TypedResult saveAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAidingEchoControl(C::mip_interface& device) +CmdResult loadAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -449,7 +449,7 @@ TypedResult loadAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAidingEchoControl(C::mip_interface& device) +CmdResult defaultAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -490,7 +490,7 @@ void extract(Serializer& serializer, EcefPos& self) } -TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -550,7 +550,7 @@ void extract(Serializer& serializer, LlhPos& self) } -TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -602,7 +602,7 @@ void extract(Serializer& serializer, Height& self) } -TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) +CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -648,7 +648,7 @@ void extract(Serializer& serializer, Pressure& self) } -TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) +CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -698,7 +698,7 @@ void extract(Serializer& serializer, EcefVel& self) } -TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -752,7 +752,7 @@ void extract(Serializer& serializer, NedVel& self) } -TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -806,7 +806,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -856,7 +856,7 @@ void extract(Serializer& serializer, TrueHeading& self) } -TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) +CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -906,7 +906,7 @@ void extract(Serializer& serializer, MagneticField& self) } -TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) +CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 28ffd2617..5a72202c9 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -97,8 +97,6 @@ struct SensorFrameMapping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorFrameMapping"; - static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -131,8 +129,6 @@ struct SensorFrameMapping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorFrameMapping::Response"; - static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -152,11 +148,11 @@ void extract(Serializer& serializer, SensorFrameMapping& self); void insert(Serializer& serializer, const SensorFrameMapping::Response& self); void extract(Serializer& serializer, SensorFrameMapping::Response& self); -TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); -TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); -TypedResult saveSensorFrameMapping(C::mip_interface& device); -TypedResult loadSensorFrameMapping(C::mip_interface& device); -TypedResult defaultSensorFrameMapping(C::mip_interface& device); +CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); +CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); +CmdResult saveSensorFrameMapping(C::mip_interface& device); +CmdResult loadSensorFrameMapping(C::mip_interface& device); +CmdResult defaultSensorFrameMapping(C::mip_interface& device); ///@} /// @@ -211,8 +207,6 @@ struct ReferenceFrame static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferenceFrame"; - static constexpr const char* DOC_NAME = "Reference Frame Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -246,8 +240,6 @@ struct ReferenceFrame static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferenceFrame::Response"; - static constexpr const char* DOC_NAME = "Reference Frame Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -269,11 +261,11 @@ void extract(Serializer& serializer, ReferenceFrame& self); void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); -TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); -TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); -TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); -TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); -TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); +CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); +CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); +CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); +CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); +CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); ///@} /// @@ -298,8 +290,6 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingEchoControl"; - static constexpr const char* DOC_NAME = "Aiding Command Echo Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -332,8 +322,6 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingEchoControl::Response"; - static constexpr const char* DOC_NAME = "Aiding Command Echo Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -352,11 +340,11 @@ void extract(Serializer& serializer, AidingEchoControl& self); void insert(Serializer& serializer, const AidingEchoControl::Response& self); void extract(Serializer& serializer, AidingEchoControl::Response& self); -TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); -TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); -TypedResult saveAidingEchoControl(C::mip_interface& device); -TypedResult loadAidingEchoControl(C::mip_interface& device); -TypedResult defaultAidingEchoControl(C::mip_interface& device); +CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); +CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); +CmdResult saveAidingEchoControl(C::mip_interface& device); +CmdResult loadAidingEchoControl(C::mip_interface& device); +CmdResult defaultAidingEchoControl(C::mip_interface& device); ///@} /// @@ -384,7 +372,7 @@ struct EcefPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -408,8 +396,6 @@ struct EcefPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefPos"; - static constexpr const char* DOC_NAME = "ECEF Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -428,7 +414,7 @@ struct EcefPos void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); +CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); ///@} /// @@ -457,7 +443,7 @@ struct LlhPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -483,8 +469,6 @@ struct LlhPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "LlhPos"; - static constexpr const char* DOC_NAME = "LLH Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -503,7 +487,7 @@ struct LlhPos void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); ///@} /// @@ -524,8 +508,6 @@ struct Height static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEIGHT_ABS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Height"; - static constexpr const char* DOC_NAME = "Height"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -544,7 +526,7 @@ struct Height void insert(Serializer& serializer, const Height& self); void extract(Serializer& serializer, Height& self); -TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); +CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); ///@} /// @@ -565,8 +547,6 @@ struct Pressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_PRESSURE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Pressure"; - static constexpr const char* DOC_NAME = "Pressure"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -585,7 +565,7 @@ struct Pressure void insert(Serializer& serializer, const Pressure& self); void extract(Serializer& serializer, Pressure& self); -TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); +CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); ///@} /// @@ -613,7 +593,7 @@ struct EcefVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -637,8 +617,6 @@ struct EcefVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefVel"; - static constexpr const char* DOC_NAME = "ECEF Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -657,7 +635,7 @@ struct EcefVel void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); +CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); ///@} /// @@ -685,7 +663,7 @@ struct NedVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -709,8 +687,6 @@ struct NedVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "NedVel"; - static constexpr const char* DOC_NAME = "NED Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -729,7 +705,7 @@ struct NedVel void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); +CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); ///@} /// @@ -758,7 +734,7 @@ struct VehicleFixedFrameVelocity ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -782,8 +758,6 @@ struct VehicleFixedFrameVelocity static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VehicleFixedFrameVelocity"; - static constexpr const char* DOC_NAME = "Vehicle Frame Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -802,7 +776,7 @@ struct VehicleFixedFrameVelocity void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); ///@} /// @@ -822,8 +796,6 @@ struct TrueHeading static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "TrueHeading"; - static constexpr const char* DOC_NAME = "True Heading"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -842,7 +814,7 @@ struct TrueHeading void insert(Serializer& serializer, const TrueHeading& self); void extract(Serializer& serializer, TrueHeading& self); -TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); +CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); ///@} /// @@ -870,7 +842,7 @@ struct MagneticField ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -894,8 +866,6 @@ struct MagneticField static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_MAGNETIC_FIELD; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagneticField"; - static constexpr const char* DOC_NAME = "Magnetic Field"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -914,7 +884,7 @@ struct MagneticField void insert(Serializer& serializer, const MagneticField& self); void extract(Serializer& serializer, MagneticField& self); -TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); +CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); ///@} /// diff --git a/src/mip/definitions/commands_base.cpp b/src/mip/definitions/commands_base.cpp index bdd9de55d..035d105f7 100644 --- a/src/mip/definitions/commands_base.cpp +++ b/src/mip/definitions/commands_base.cpp @@ -81,7 +81,7 @@ void extract(Serializer& serializer, Ping& self) (void)self; } -TypedResult ping(C::mip_interface& device) +CmdResult ping(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PING, NULL, 0); } @@ -96,7 +96,7 @@ void extract(Serializer& serializer, SetIdle& self) (void)self; } -TypedResult setIdle(C::mip_interface& device) +CmdResult setIdle(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SET_TO_IDLE, NULL, 0); } @@ -122,12 +122,12 @@ void extract(Serializer& serializer, GetDeviceInfo::Response& self) } -TypedResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) +CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_INFO, NULL, 0, REPLY_DEVICE_INFO, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_INFO, NULL, 0, REPLY_DEVICE_INFO, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -165,12 +165,12 @@ void extract(Serializer& serializer, GetDeviceDescriptors::Response& self) } -TypedResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) +CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_DESCRIPTORS, NULL, 0, REPLY_DEVICE_DESCRIPTORS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_DESCRIPTORS, NULL, 0, REPLY_DEVICE_DESCRIPTORS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -206,12 +206,12 @@ void extract(Serializer& serializer, BuiltInTest::Response& self) } -TypedResult builtInTest(C::mip_interface& device, uint32_t* resultOut) +CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_BUILT_IN_TEST, NULL, 0, REPLY_BUILT_IN_TEST, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_BUILT_IN_TEST, NULL, 0, REPLY_BUILT_IN_TEST, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -236,7 +236,7 @@ void extract(Serializer& serializer, Resume& self) (void)self; } -TypedResult resume(C::mip_interface& device) +CmdResult resume(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RESUME, NULL, 0); } @@ -264,12 +264,12 @@ void extract(Serializer& serializer, GetExtendedDescriptors::Response& self) } -TypedResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) +CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_EXTENDED_DESCRIPTORS, NULL, 0, REPLY_GET_EXTENDED_DESCRIPTORS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_EXTENDED_DESCRIPTORS, NULL, 0, REPLY_GET_EXTENDED_DESCRIPTORS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -307,12 +307,12 @@ void extract(Serializer& serializer, ContinuousBit::Response& self) } -TypedResult continuousBit(C::mip_interface& device, uint8_t* resultOut) +CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTINUOUS_BIT, NULL, 0, REPLY_CONTINUOUS_BIT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTINUOUS_BIT, NULL, 0, REPLY_CONTINUOUS_BIT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -367,7 +367,7 @@ void extract(Serializer& serializer, CommSpeed::Response& self) } -TypedResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) +CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -381,7 +381,7 @@ TypedResult writeCommSpeed(C::mip_interface& device, uint8_t port, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut) +CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -392,7 +392,7 @@ TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uin assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COMM_SPEED, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COMM_SPEED, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -408,7 +408,7 @@ TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uin } return result; } -TypedResult saveCommSpeed(C::mip_interface& device, uint8_t port) +CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -420,7 +420,7 @@ TypedResult saveCommSpeed(C::mip_interface& device, uint8_t port) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadCommSpeed(C::mip_interface& device, uint8_t port) +CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -432,7 +432,7 @@ TypedResult loadCommSpeed(C::mip_interface& device, uint8_t port) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultCommSpeed(C::mip_interface& device, uint8_t port) +CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -469,7 +469,7 @@ void extract(Serializer& serializer, GpsTimeUpdate& self) } } -TypedResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value) +CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -494,7 +494,7 @@ void extract(Serializer& serializer, SoftReset& self) (void)self; } -TypedResult softReset(C::mip_interface& device) +CmdResult softReset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_RESET, NULL, 0); } diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index e9d1bf0c3..5665a4753 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -118,7 +118,7 @@ struct CommandedTestBitsGq7 : Bitfield CommandedTestBitsGq7(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } CommandedTestBitsGq7& operator=(uint32_t val) { value = val; return *this; } - CommandedTestBitsGq7& operator=(int val) { value = uint32_t(val); return *this; } + CommandedTestBitsGq7& operator=(int val) { value = val; return *this; } CommandedTestBitsGq7& operator|=(uint32_t val) { return *this = value | val; } CommandedTestBitsGq7& operator&=(uint32_t val) { return *this = value & val; } @@ -202,8 +202,6 @@ struct Ping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Ping"; - static constexpr const char* DOC_NAME = "Ping"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -222,7 +220,7 @@ struct Ping void insert(Serializer& serializer, const Ping& self); void extract(Serializer& serializer, Ping& self); -TypedResult ping(C::mip_interface& device); +CmdResult ping(C::mip_interface& device); ///@} /// @@ -242,8 +240,6 @@ struct SetIdle static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SetIdle"; - static constexpr const char* DOC_NAME = "Set to idle"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -262,7 +258,7 @@ struct SetIdle void insert(Serializer& serializer, const SetIdle& self); void extract(Serializer& serializer, SetIdle& self); -TypedResult setIdle(C::mip_interface& device); +CmdResult setIdle(C::mip_interface& device); ///@} /// @@ -278,8 +274,6 @@ struct GetDeviceInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetDeviceInfo"; - static constexpr const char* DOC_NAME = "Get device information"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -299,8 +293,6 @@ struct GetDeviceInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetDeviceInfo::Response"; - static constexpr const char* DOC_NAME = "Get device information Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -319,7 +311,7 @@ void extract(Serializer& serializer, GetDeviceInfo& self); void insert(Serializer& serializer, const GetDeviceInfo::Response& self); void extract(Serializer& serializer, GetDeviceInfo::Response& self); -TypedResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut); +CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut); ///@} /// @@ -338,8 +330,6 @@ struct GetDeviceDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetDeviceDescriptors"; - static constexpr const char* DOC_NAME = "Get device descriptors"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -359,8 +349,6 @@ struct GetDeviceDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetDeviceDescriptors::Response"; - static constexpr const char* DOC_NAME = "Get device descriptors Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; @@ -380,7 +368,7 @@ void extract(Serializer& serializer, GetDeviceDescriptors& self); void insert(Serializer& serializer, const GetDeviceDescriptors::Response& self); void extract(Serializer& serializer, GetDeviceDescriptors::Response& self); -TypedResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); +CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); ///@} /// @@ -401,8 +389,6 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "BuiltInTest"; - static constexpr const char* DOC_NAME = "Built in test"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -422,8 +408,6 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "BuiltInTest::Response"; - static constexpr const char* DOC_NAME = "Built in test Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -442,7 +426,7 @@ void extract(Serializer& serializer, BuiltInTest& self); void insert(Serializer& serializer, const BuiltInTest::Response& self); void extract(Serializer& serializer, BuiltInTest::Response& self); -TypedResult builtInTest(C::mip_interface& device, uint32_t* resultOut); +CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut); ///@} /// @@ -460,8 +444,6 @@ struct Resume static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Resume"; - static constexpr const char* DOC_NAME = "Resume"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -480,7 +462,7 @@ struct Resume void insert(Serializer& serializer, const Resume& self); void extract(Serializer& serializer, Resume& self); -TypedResult resume(C::mip_interface& device); +CmdResult resume(C::mip_interface& device); ///@} /// @@ -499,8 +481,6 @@ struct GetExtendedDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetExtendedDescriptors"; - static constexpr const char* DOC_NAME = "Get device descriptors (extended)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -520,8 +500,6 @@ struct GetExtendedDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetExtendedDescriptors::Response"; - static constexpr const char* DOC_NAME = "Get device descriptors (extended) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; @@ -541,7 +519,7 @@ void extract(Serializer& serializer, GetExtendedDescriptors& self); void insert(Serializer& serializer, const GetExtendedDescriptors::Response& self); void extract(Serializer& serializer, GetExtendedDescriptors::Response& self); -TypedResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); +CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); ///@} /// @@ -559,8 +537,6 @@ struct ContinuousBit static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ContinuousBit"; - static constexpr const char* DOC_NAME = "Continuous built-in test"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -580,8 +556,6 @@ struct ContinuousBit static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ContinuousBit::Response"; - static constexpr const char* DOC_NAME = "Continuous built-in test Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -600,7 +574,7 @@ void extract(Serializer& serializer, ContinuousBit& self); void insert(Serializer& serializer, const ContinuousBit::Response& self); void extract(Serializer& serializer, ContinuousBit::Response& self); -TypedResult continuousBit(C::mip_interface& device, uint8_t* resultOut); +CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut); ///@} /// @@ -633,8 +607,6 @@ struct CommSpeed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CommSpeed"; - static constexpr const char* DOC_NAME = "Comm Port Speed"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -668,8 +640,6 @@ struct CommSpeed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CommSpeed::Response"; - static constexpr const char* DOC_NAME = "Comm Port Speed Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -689,11 +659,11 @@ void extract(Serializer& serializer, CommSpeed& self); void insert(Serializer& serializer, const CommSpeed::Response& self); void extract(Serializer& serializer, CommSpeed::Response& self); -TypedResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud); -TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut); -TypedResult saveCommSpeed(C::mip_interface& device, uint8_t port); -TypedResult loadCommSpeed(C::mip_interface& device, uint8_t port); -TypedResult defaultCommSpeed(C::mip_interface& device, uint8_t port); +CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud); +CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut); +CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port); +CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port); +CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port); ///@} /// @@ -721,8 +691,6 @@ struct GpsTimeUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsTimeUpdate"; - static constexpr const char* DOC_NAME = "Time Broadcast Command"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -754,7 +722,7 @@ struct GpsTimeUpdate void insert(Serializer& serializer, const GpsTimeUpdate& self); void extract(Serializer& serializer, GpsTimeUpdate& self); -TypedResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value); +CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value); ///@} /// @@ -772,8 +740,6 @@ struct SoftReset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SoftReset"; - static constexpr const char* DOC_NAME = "Reset device"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -792,7 +758,7 @@ struct SoftReset void insert(Serializer& serializer, const SoftReset& self); void extract(Serializer& serializer, SoftReset& self); -TypedResult softReset(C::mip_interface& device); +CmdResult softReset(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index 7792142bf..c88cb0eb8 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -40,7 +40,7 @@ void extract(Serializer& serializer, Reset& self) (void)self; } -TypedResult reset(C::mip_interface& device) +CmdResult reset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RESET_FILTER, NULL, 0); } @@ -63,7 +63,7 @@ void extract(Serializer& serializer, SetInitialAttitude& self) } -TypedResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading) +CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -110,7 +110,7 @@ void extract(Serializer& serializer, EstimationControl::Response& self) } -TypedResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable) +CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -122,7 +122,7 @@ TypedResult writeEstimationControl(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut) +CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -131,7 +131,7 @@ TypedResult readEstimationControl(C::mip_interface& device, E assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ESTIMATION_CONTROL_FLAGS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ESTIMATION_CONTROL_FLAGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -145,7 +145,7 @@ TypedResult readEstimationControl(C::mip_interface& device, E } return result; } -TypedResult saveEstimationControl(C::mip_interface& device) +CmdResult saveEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -155,7 +155,7 @@ TypedResult saveEstimationControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadEstimationControl(C::mip_interface& device) +CmdResult loadEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -165,7 +165,7 @@ TypedResult loadEstimationControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultEstimationControl(C::mip_interface& device) +CmdResult defaultEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -220,7 +220,7 @@ void extract(Serializer& serializer, ExternalGnssUpdate& self) } -TypedResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) +CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -270,7 +270,7 @@ void extract(Serializer& serializer, ExternalHeadingUpdate& self) } -TypedResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type) +CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -312,7 +312,7 @@ void extract(Serializer& serializer, ExternalHeadingUpdateWithTime& self) } -TypedResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type) +CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -363,7 +363,7 @@ void extract(Serializer& serializer, TareOrientation::Response& self) } -TypedResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes) +CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -375,7 +375,7 @@ TypedResult writeTareOrientation(C::mip_interface& device, Tare return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut) +CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -384,7 +384,7 @@ TypedResult readTareOrientation(C::mip_interface& device, TareO assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_TARE_ORIENTATION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_TARE_ORIENTATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -398,7 +398,7 @@ TypedResult readTareOrientation(C::mip_interface& device, TareO } return result; } -TypedResult saveTareOrientation(C::mip_interface& device) +CmdResult saveTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -408,7 +408,7 @@ TypedResult saveTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadTareOrientation(C::mip_interface& device) +CmdResult loadTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -418,7 +418,7 @@ TypedResult loadTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultTareOrientation(C::mip_interface& device) +CmdResult defaultTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -460,7 +460,7 @@ void extract(Serializer& serializer, VehicleDynamicsMode::Response& self) } -TypedResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) +CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -472,7 +472,7 @@ TypedResult writeVehicleDynamicsMode(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) +CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -481,7 +481,7 @@ TypedResult readVehicleDynamicsMode(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -495,7 +495,7 @@ TypedResult readVehicleDynamicsMode(C::mip_interface& devic } return result; } -TypedResult saveVehicleDynamicsMode(C::mip_interface& device) +CmdResult saveVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -505,7 +505,7 @@ TypedResult saveVehicleDynamicsMode(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadVehicleDynamicsMode(C::mip_interface& device) +CmdResult loadVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -515,7 +515,7 @@ TypedResult loadVehicleDynamicsMode(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultVehicleDynamicsMode(C::mip_interface& device) +CmdResult defaultVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -573,7 +573,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationEuler::Response& sel } -TypedResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw) +CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -589,7 +589,7 @@ TypedResult writeSensorToVehicleRotationEuler(C::m return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) +CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -598,7 +598,7 @@ TypedResult readSensorToVehicleRotationEuler(C::mi assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_EULER, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_EULER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -618,7 +618,7 @@ TypedResult readSensorToVehicleRotationEuler(C::mi } return result; } -TypedResult saveSensorToVehicleRotationEuler(C::mip_interface& device) +CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -628,7 +628,7 @@ TypedResult saveSensorToVehicleRotationEuler(C::mi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensorToVehicleRotationEuler(C::mip_interface& device) +CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -638,7 +638,7 @@ TypedResult loadSensorToVehicleRotationEuler(C::mi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensorToVehicleRotationEuler(C::mip_interface& device) +CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -684,7 +684,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self) } -TypedResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) +CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -698,7 +698,7 @@ TypedResult writeSensorToVehicleRotationDcm(C::mip_i return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) +CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -707,7 +707,7 @@ TypedResult readSensorToVehicleRotationDcm(C::mip_in assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_DCM, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_DCM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -722,7 +722,7 @@ TypedResult readSensorToVehicleRotationDcm(C::mip_in } return result; } -TypedResult saveSensorToVehicleRotationDcm(C::mip_interface& device) +CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -732,7 +732,7 @@ TypedResult saveSensorToVehicleRotationDcm(C::mip_in return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensorToVehicleRotationDcm(C::mip_interface& device) +CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -742,7 +742,7 @@ TypedResult loadSensorToVehicleRotationDcm(C::mip_in return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensorToVehicleRotationDcm(C::mip_interface& device) +CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -788,7 +788,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response } -TypedResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) +CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -802,7 +802,7 @@ TypedResult writeSensorToVehicleRotationQuate return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) +CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -811,7 +811,7 @@ TypedResult readSensorToVehicleRotationQuater assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -826,7 +826,7 @@ TypedResult readSensorToVehicleRotationQuater } return result; } -TypedResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) +CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -836,7 +836,7 @@ TypedResult saveSensorToVehicleRotationQuater return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) +CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -846,7 +846,7 @@ TypedResult loadSensorToVehicleRotationQuater return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device) +CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -892,7 +892,7 @@ void extract(Serializer& serializer, SensorToVehicleOffset::Response& self) } -TypedResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) +CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -906,7 +906,7 @@ TypedResult writeSensorToVehicleOffset(C::mip_interface& return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) +CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -915,7 +915,7 @@ TypedResult readSensorToVehicleOffset(C::mip_interface& d assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_OFFSET, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -930,7 +930,7 @@ TypedResult readSensorToVehicleOffset(C::mip_interface& d } return result; } -TypedResult saveSensorToVehicleOffset(C::mip_interface& device) +CmdResult saveSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -940,7 +940,7 @@ TypedResult saveSensorToVehicleOffset(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSensorToVehicleOffset(C::mip_interface& device) +CmdResult loadSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -950,7 +950,7 @@ TypedResult loadSensorToVehicleOffset(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSensorToVehicleOffset(C::mip_interface& device) +CmdResult defaultSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -996,7 +996,7 @@ void extract(Serializer& serializer, AntennaOffset::Response& self) } -TypedResult writeAntennaOffset(C::mip_interface& device, const float* offset) +CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1010,7 +1010,7 @@ TypedResult writeAntennaOffset(C::mip_interface& device, const fl return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAntennaOffset(C::mip_interface& device, float* offsetOut) +CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1019,7 +1019,7 @@ TypedResult readAntennaOffset(C::mip_interface& device, float* of assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_OFFSET, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1034,7 +1034,7 @@ TypedResult readAntennaOffset(C::mip_interface& device, float* of } return result; } -TypedResult saveAntennaOffset(C::mip_interface& device) +CmdResult saveAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1044,7 +1044,7 @@ TypedResult saveAntennaOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAntennaOffset(C::mip_interface& device) +CmdResult loadAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1054,7 +1054,7 @@ TypedResult loadAntennaOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAntennaOffset(C::mip_interface& device) +CmdResult defaultAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1096,7 +1096,7 @@ void extract(Serializer& serializer, GnssSource::Response& self) } -TypedResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) +CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1108,7 +1108,7 @@ TypedResult writeGnssSource(C::mip_interface& device, GnssSource::So return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut) +CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1117,7 +1117,7 @@ TypedResult readGnssSource(C::mip_interface& device, GnssSource::Sou assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SOURCE_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SOURCE_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1131,7 +1131,7 @@ TypedResult readGnssSource(C::mip_interface& device, GnssSource::Sou } return result; } -TypedResult saveGnssSource(C::mip_interface& device) +CmdResult saveGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1141,7 +1141,7 @@ TypedResult saveGnssSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGnssSource(C::mip_interface& device) +CmdResult loadGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1151,7 +1151,7 @@ TypedResult loadGnssSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGnssSource(C::mip_interface& device) +CmdResult defaultGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1193,7 +1193,7 @@ void extract(Serializer& serializer, HeadingSource::Response& self) } -TypedResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source) +CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1205,7 +1205,7 @@ TypedResult writeHeadingSource(C::mip_interface& device, HeadingS return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut) +CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1214,7 +1214,7 @@ TypedResult readHeadingSource(C::mip_interface& device, HeadingSo assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_UPDATE_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_UPDATE_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1228,7 +1228,7 @@ TypedResult readHeadingSource(C::mip_interface& device, HeadingSo } return result; } -TypedResult saveHeadingSource(C::mip_interface& device) +CmdResult saveHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1238,7 +1238,7 @@ TypedResult saveHeadingSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadHeadingSource(C::mip_interface& device) +CmdResult loadHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1248,7 +1248,7 @@ TypedResult loadHeadingSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultHeadingSource(C::mip_interface& device) +CmdResult defaultHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1290,7 +1290,7 @@ void extract(Serializer& serializer, AutoInitControl::Response& self) } -TypedResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) +CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1302,7 +1302,7 @@ TypedResult writeAutoInitControl(C::mip_interface& device, uint return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) +CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1311,7 +1311,7 @@ TypedResult readAutoInitControl(C::mip_interface& device, uint8 assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AUTOINIT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AUTOINIT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1325,7 +1325,7 @@ TypedResult readAutoInitControl(C::mip_interface& device, uint8 } return result; } -TypedResult saveAutoInitControl(C::mip_interface& device) +CmdResult saveAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1335,7 +1335,7 @@ TypedResult saveAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAutoInitControl(C::mip_interface& device) +CmdResult loadAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1345,7 +1345,7 @@ TypedResult loadAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAutoInitControl(C::mip_interface& device) +CmdResult defaultAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1391,7 +1391,7 @@ void extract(Serializer& serializer, AccelNoise::Response& self) } -TypedResult writeAccelNoise(C::mip_interface& device, const float* noise) +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1405,7 +1405,7 @@ TypedResult writeAccelNoise(C::mip_interface& device, const float* n return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut) +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1414,7 +1414,7 @@ TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1429,7 +1429,7 @@ TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut } return result; } -TypedResult saveAccelNoise(C::mip_interface& device) +CmdResult saveAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1439,7 +1439,7 @@ TypedResult saveAccelNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAccelNoise(C::mip_interface& device) +CmdResult loadAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1449,7 +1449,7 @@ TypedResult loadAccelNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAccelNoise(C::mip_interface& device) +CmdResult defaultAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1495,7 +1495,7 @@ void extract(Serializer& serializer, GyroNoise::Response& self) } -TypedResult writeGyroNoise(C::mip_interface& device, const float* noise) +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1509,7 +1509,7 @@ TypedResult writeGyroNoise(C::mip_interface& device, const float* noi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut) +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1518,7 +1518,7 @@ TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1533,7 +1533,7 @@ TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut) } return result; } -TypedResult saveGyroNoise(C::mip_interface& device) +CmdResult saveGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1543,7 +1543,7 @@ TypedResult saveGyroNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGyroNoise(C::mip_interface& device) +CmdResult loadGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1553,7 +1553,7 @@ TypedResult loadGyroNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGyroNoise(C::mip_interface& device) +CmdResult defaultGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1611,7 +1611,7 @@ void extract(Serializer& serializer, AccelBiasModel::Response& self) } -TypedResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1629,7 +1629,7 @@ TypedResult writeAccelBiasModel(C::mip_interface& device, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1638,7 +1638,7 @@ TypedResult readAccelBiasModel(C::mip_interface& device, float* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1657,7 +1657,7 @@ TypedResult readAccelBiasModel(C::mip_interface& device, float* } return result; } -TypedResult saveAccelBiasModel(C::mip_interface& device) +CmdResult saveAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1667,7 +1667,7 @@ TypedResult saveAccelBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAccelBiasModel(C::mip_interface& device) +CmdResult loadAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1677,7 +1677,7 @@ TypedResult loadAccelBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAccelBiasModel(C::mip_interface& device) +CmdResult defaultAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1735,7 +1735,7 @@ void extract(Serializer& serializer, GyroBiasModel::Response& self) } -TypedResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1753,7 +1753,7 @@ TypedResult writeGyroBiasModel(C::mip_interface& device, const fl return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1762,7 +1762,7 @@ TypedResult readGyroBiasModel(C::mip_interface& device, float* be assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1781,7 +1781,7 @@ TypedResult readGyroBiasModel(C::mip_interface& device, float* be } return result; } -TypedResult saveGyroBiasModel(C::mip_interface& device) +CmdResult saveGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1791,7 +1791,7 @@ TypedResult saveGyroBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGyroBiasModel(C::mip_interface& device) +CmdResult loadGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1801,7 +1801,7 @@ TypedResult loadGyroBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGyroBiasModel(C::mip_interface& device) +CmdResult defaultGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1843,7 +1843,7 @@ void extract(Serializer& serializer, AltitudeAiding::Response& self) } -TypedResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) +CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1855,7 +1855,7 @@ TypedResult writeAltitudeAiding(C::mip_interface& device, Altitu return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) +CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1864,7 +1864,7 @@ TypedResult readAltitudeAiding(C::mip_interface& device, Altitud assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1878,7 +1878,7 @@ TypedResult readAltitudeAiding(C::mip_interface& device, Altitud } return result; } -TypedResult saveAltitudeAiding(C::mip_interface& device) +CmdResult saveAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1888,7 +1888,7 @@ TypedResult saveAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAltitudeAiding(C::mip_interface& device) +CmdResult loadAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1898,7 +1898,7 @@ TypedResult loadAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAltitudeAiding(C::mip_interface& device) +CmdResult defaultAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1940,7 +1940,7 @@ void extract(Serializer& serializer, PitchRollAiding::Response& self) } -TypedResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) +CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1952,7 +1952,7 @@ TypedResult writePitchRollAiding(C::mip_interface& device, Pitc return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) +CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1961,7 +1961,7 @@ TypedResult readPitchRollAiding(C::mip_interface& device, Pitch assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1975,7 +1975,7 @@ TypedResult readPitchRollAiding(C::mip_interface& device, Pitch } return result; } -TypedResult savePitchRollAiding(C::mip_interface& device) +CmdResult savePitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1985,7 +1985,7 @@ TypedResult savePitchRollAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadPitchRollAiding(C::mip_interface& device) +CmdResult loadPitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1995,7 +1995,7 @@ TypedResult loadPitchRollAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultPitchRollAiding(C::mip_interface& device) +CmdResult defaultPitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2045,7 +2045,7 @@ void extract(Serializer& serializer, AutoZupt::Response& self) } -TypedResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) +CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2059,7 +2059,7 @@ TypedResult writeAutoZupt(C::mip_interface& device, uint8_t enable, fl return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2068,7 +2068,7 @@ TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2085,7 +2085,7 @@ TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, } return result; } -TypedResult saveAutoZupt(C::mip_interface& device) +CmdResult saveAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2095,7 +2095,7 @@ TypedResult saveAutoZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAutoZupt(C::mip_interface& device) +CmdResult loadAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2105,7 +2105,7 @@ TypedResult loadAutoZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAutoZupt(C::mip_interface& device) +CmdResult defaultAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2155,7 +2155,7 @@ void extract(Serializer& serializer, AutoAngularZupt::Response& self) } -TypedResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) +CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2169,7 +2169,7 @@ TypedResult writeAutoAngularZupt(C::mip_interface& device, uint return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2178,7 +2178,7 @@ TypedResult readAutoAngularZupt(C::mip_interface& device, uint8 assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2195,7 +2195,7 @@ TypedResult readAutoAngularZupt(C::mip_interface& device, uint8 } return result; } -TypedResult saveAutoAngularZupt(C::mip_interface& device) +CmdResult saveAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2205,7 +2205,7 @@ TypedResult saveAutoAngularZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAutoAngularZupt(C::mip_interface& device) +CmdResult loadAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2215,7 +2215,7 @@ TypedResult loadAutoAngularZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAutoAngularZupt(C::mip_interface& device) +CmdResult defaultAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2236,7 +2236,7 @@ void extract(Serializer& serializer, CommandedZupt& self) (void)self; } -TypedResult commandedZupt(C::mip_interface& device) +CmdResult commandedZupt(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); } @@ -2251,7 +2251,7 @@ void extract(Serializer& serializer, CommandedAngularZupt& self) (void)self; } -TypedResult commandedAngularZupt(C::mip_interface& device) +CmdResult commandedAngularZupt(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); } @@ -2266,7 +2266,7 @@ void extract(Serializer& serializer, MagCaptureAutoCal& self) } -TypedResult writeMagCaptureAutoCal(C::mip_interface& device) +CmdResult writeMagCaptureAutoCal(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2276,7 +2276,7 @@ TypedResult writeMagCaptureAutoCal(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult saveMagCaptureAutoCal(C::mip_interface& device) +CmdResult saveMagCaptureAutoCal(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2322,7 +2322,7 @@ void extract(Serializer& serializer, GravityNoise::Response& self) } -TypedResult writeGravityNoise(C::mip_interface& device, const float* noise) +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2336,7 +2336,7 @@ TypedResult writeGravityNoise(C::mip_interface& device, const floa return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGravityNoise(C::mip_interface& device, float* noiseOut) +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2345,7 +2345,7 @@ TypedResult readGravityNoise(C::mip_interface& device, float* nois assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2360,7 +2360,7 @@ TypedResult readGravityNoise(C::mip_interface& device, float* nois } return result; } -TypedResult saveGravityNoise(C::mip_interface& device) +CmdResult saveGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2370,7 +2370,7 @@ TypedResult saveGravityNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGravityNoise(C::mip_interface& device) +CmdResult loadGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2380,7 +2380,7 @@ TypedResult loadGravityNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGravityNoise(C::mip_interface& device) +CmdResult defaultGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2422,7 +2422,7 @@ void extract(Serializer& serializer, PressureAltitudeNoise::Response& self) } -TypedResult writePressureAltitudeNoise(C::mip_interface& device, float noise) +CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2434,7 +2434,7 @@ TypedResult writePressureAltitudeNoise(C::mip_interface& return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) +CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2443,7 +2443,7 @@ TypedResult readPressureAltitudeNoise(C::mip_interface& d assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2457,7 +2457,7 @@ TypedResult readPressureAltitudeNoise(C::mip_interface& d } return result; } -TypedResult savePressureAltitudeNoise(C::mip_interface& device) +CmdResult savePressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2467,7 +2467,7 @@ TypedResult savePressureAltitudeNoise(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadPressureAltitudeNoise(C::mip_interface& device) +CmdResult loadPressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2477,7 +2477,7 @@ TypedResult loadPressureAltitudeNoise(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultPressureAltitudeNoise(C::mip_interface& device) +CmdResult defaultPressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2523,7 +2523,7 @@ void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) } -TypedResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2537,7 +2537,7 @@ TypedResult writeHardIronOffsetNoise(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2546,7 +2546,7 @@ TypedResult readHardIronOffsetNoise(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2561,7 +2561,7 @@ TypedResult readHardIronOffsetNoise(C::mip_interface& devic } return result; } -TypedResult saveHardIronOffsetNoise(C::mip_interface& device) +CmdResult saveHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2571,7 +2571,7 @@ TypedResult saveHardIronOffsetNoise(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadHardIronOffsetNoise(C::mip_interface& device) +CmdResult loadHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2581,7 +2581,7 @@ TypedResult loadHardIronOffsetNoise(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultHardIronOffsetNoise(C::mip_interface& device) +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2627,7 +2627,7 @@ void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self) } -TypedResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2641,7 +2641,7 @@ TypedResult writeSoftIronMatrixNoise(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2650,7 +2650,7 @@ TypedResult readSoftIronMatrixNoise(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2665,7 +2665,7 @@ TypedResult readSoftIronMatrixNoise(C::mip_interface& devic } return result; } -TypedResult saveSoftIronMatrixNoise(C::mip_interface& device) +CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2675,7 +2675,7 @@ TypedResult saveSoftIronMatrixNoise(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSoftIronMatrixNoise(C::mip_interface& device) +CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2685,7 +2685,7 @@ TypedResult loadSoftIronMatrixNoise(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSoftIronMatrixNoise(C::mip_interface& device) +CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2731,7 +2731,7 @@ void extract(Serializer& serializer, MagNoise::Response& self) } -TypedResult writeMagNoise(C::mip_interface& device, const float* noise) +CmdResult writeMagNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2745,7 +2745,7 @@ TypedResult writeMagNoise(C::mip_interface& device, const float* noise return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMagNoise(C::mip_interface& device, float* noiseOut) +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2754,7 +2754,7 @@ TypedResult readMagNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2769,7 +2769,7 @@ TypedResult readMagNoise(C::mip_interface& device, float* noiseOut) } return result; } -TypedResult saveMagNoise(C::mip_interface& device) +CmdResult saveMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2779,7 +2779,7 @@ TypedResult saveMagNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMagNoise(C::mip_interface& device) +CmdResult loadMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2789,7 +2789,7 @@ TypedResult loadMagNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMagNoise(C::mip_interface& device) +CmdResult defaultMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2839,7 +2839,7 @@ void extract(Serializer& serializer, InclinationSource::Response& self) } -TypedResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) +CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2853,7 +2853,7 @@ TypedResult writeInclinationSource(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) +CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2862,7 +2862,7 @@ TypedResult readInclinationSource(C::mip_interface& device, F assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2879,7 +2879,7 @@ TypedResult readInclinationSource(C::mip_interface& device, F } return result; } -TypedResult saveInclinationSource(C::mip_interface& device) +CmdResult saveInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2889,7 +2889,7 @@ TypedResult saveInclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadInclinationSource(C::mip_interface& device) +CmdResult loadInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2899,7 +2899,7 @@ TypedResult loadInclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultInclinationSource(C::mip_interface& device) +CmdResult defaultInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2949,7 +2949,7 @@ void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) } -TypedResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) +CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2963,7 +2963,7 @@ TypedResult writeMagneticDeclinationSource(C::mip_int return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) +CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2972,7 +2972,7 @@ TypedResult readMagneticDeclinationSource(C::mip_inte assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2989,7 +2989,7 @@ TypedResult readMagneticDeclinationSource(C::mip_inte } return result; } -TypedResult saveMagneticDeclinationSource(C::mip_interface& device) +CmdResult saveMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2999,7 +2999,7 @@ TypedResult saveMagneticDeclinationSource(C::mip_inte return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMagneticDeclinationSource(C::mip_interface& device) +CmdResult loadMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3009,7 +3009,7 @@ TypedResult loadMagneticDeclinationSource(C::mip_inte return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMagneticDeclinationSource(C::mip_interface& device) +CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3059,7 +3059,7 @@ void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self) } -TypedResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) +CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3073,7 +3073,7 @@ TypedResult writeMagFieldMagnitudeSource(C::mip_interfa return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) +CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3082,7 +3082,7 @@ TypedResult readMagFieldMagnitudeSource(C::mip_interfac assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3099,7 +3099,7 @@ TypedResult readMagFieldMagnitudeSource(C::mip_interfac } return result; } -TypedResult saveMagFieldMagnitudeSource(C::mip_interface& device) +CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3109,7 +3109,7 @@ TypedResult saveMagFieldMagnitudeSource(C::mip_interfac return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMagFieldMagnitudeSource(C::mip_interface& device) +CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3119,7 +3119,7 @@ TypedResult loadMagFieldMagnitudeSource(C::mip_interfac return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMagFieldMagnitudeSource(C::mip_interface& device) +CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3185,7 +3185,7 @@ void extract(Serializer& serializer, ReferencePosition::Response& self) } -TypedResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) +CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3203,7 +3203,7 @@ TypedResult writeReferencePosition(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) +CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3212,7 +3212,7 @@ TypedResult readReferencePosition(C::mip_interface& device, b assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3235,7 +3235,7 @@ TypedResult readReferencePosition(C::mip_interface& device, b } return result; } -TypedResult saveReferencePosition(C::mip_interface& device) +CmdResult saveReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3245,7 +3245,7 @@ TypedResult saveReferencePosition(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadReferencePosition(C::mip_interface& device) +CmdResult loadReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3255,7 +3255,7 @@ TypedResult loadReferencePosition(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultReferencePosition(C::mip_interface& device) +CmdResult defaultReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3345,7 +3345,7 @@ void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Res } -TypedResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) +CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3369,7 +3369,7 @@ TypedResult writeAccelMagnitudeErrorAdap return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3378,7 +3378,7 @@ TypedResult readAccelMagnitudeErrorAdapt assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3410,7 +3410,7 @@ TypedResult readAccelMagnitudeErrorAdapt } return result; } -TypedResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3420,7 +3420,7 @@ TypedResult saveAccelMagnitudeErrorAdapt return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3430,7 +3430,7 @@ TypedResult loadAccelMagnitudeErrorAdapt return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3520,7 +3520,7 @@ void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Respo } -TypedResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) +CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3544,7 +3544,7 @@ TypedResult writeMagMagnitudeErrorAdaptive return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3553,7 +3553,7 @@ TypedResult readMagMagnitudeErrorAdaptiveM assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3585,7 +3585,7 @@ TypedResult readMagMagnitudeErrorAdaptiveM } return result; } -TypedResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3595,7 +3595,7 @@ TypedResult saveMagMagnitudeErrorAdaptiveM return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3605,7 +3605,7 @@ TypedResult loadMagMagnitudeErrorAdaptiveM return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3679,7 +3679,7 @@ void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Respon } -TypedResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) +CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3699,7 +3699,7 @@ TypedResult writeMagDipAngleErrorAdaptiveMe return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3708,7 +3708,7 @@ TypedResult readMagDipAngleErrorAdaptiveMea assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3734,7 +3734,7 @@ TypedResult readMagDipAngleErrorAdaptiveMea } return result; } -TypedResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3744,7 +3744,7 @@ TypedResult saveMagDipAngleErrorAdaptiveMea return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3754,7 +3754,7 @@ TypedResult loadMagDipAngleErrorAdaptiveMea return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3804,7 +3804,7 @@ void extract(Serializer& serializer, AidingMeasurementEnable::Response& self) } -TypedResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable) +CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3818,7 +3818,7 @@ TypedResult writeAidingMeasurementEnable(C::mip_interfa return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut) +CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3829,7 +3829,7 @@ TypedResult readAidingMeasurementEnable(C::mip_interfac assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AIDING_MEASUREMENT_ENABLE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AIDING_MEASUREMENT_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3845,7 +3845,7 @@ TypedResult readAidingMeasurementEnable(C::mip_interfac } return result; } -TypedResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3857,7 +3857,7 @@ TypedResult saveAidingMeasurementEnable(C::mip_interfac return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3869,7 +3869,7 @@ TypedResult loadAidingMeasurementEnable(C::mip_interfac return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3892,7 +3892,7 @@ void extract(Serializer& serializer, Run& self) (void)self; } -TypedResult run(C::mip_interface& device) +CmdResult run(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RUN, NULL, 0); } @@ -3944,7 +3944,7 @@ void extract(Serializer& serializer, KinematicConstraint::Response& self) } -TypedResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection) +CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3960,7 +3960,7 @@ TypedResult writeKinematicConstraint(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut) +CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3969,7 +3969,7 @@ TypedResult readKinematicConstraint(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_KINEMATIC_CONSTRAINT, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_KINEMATIC_CONSTRAINT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3989,7 +3989,7 @@ TypedResult readKinematicConstraint(C::mip_interface& devic } return result; } -TypedResult saveKinematicConstraint(C::mip_interface& device) +CmdResult saveKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3999,7 +3999,7 @@ TypedResult saveKinematicConstraint(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadKinematicConstraint(C::mip_interface& device) +CmdResult loadKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4009,7 +4009,7 @@ TypedResult loadKinematicConstraint(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultKinematicConstraint(C::mip_interface& device) +CmdResult defaultKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4123,7 +4123,7 @@ void extract(Serializer& serializer, InitializationConfiguration::Response& self } -TypedResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) +CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4155,7 +4155,7 @@ TypedResult writeInitializationConfiguration(C::mip return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) +CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4164,7 +4164,7 @@ TypedResult readInitializationConfiguration(C::mip_ assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INITIALIZATION_CONFIGURATION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INITIALIZATION_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4204,7 +4204,7 @@ TypedResult readInitializationConfiguration(C::mip_ } return result; } -TypedResult saveInitializationConfiguration(C::mip_interface& device) +CmdResult saveInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4214,7 +4214,7 @@ TypedResult saveInitializationConfiguration(C::mip_ return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadInitializationConfiguration(C::mip_interface& device) +CmdResult loadInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4224,7 +4224,7 @@ TypedResult loadInitializationConfiguration(C::mip_ return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultInitializationConfiguration(C::mip_interface& device) +CmdResult defaultInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4274,7 +4274,7 @@ void extract(Serializer& serializer, AdaptiveFilterOptions::Response& self) } -TypedResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit) +CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4288,7 +4288,7 @@ TypedResult writeAdaptiveFilterOptions(C::mip_interface& return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut) +CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4297,7 +4297,7 @@ TypedResult readAdaptiveFilterOptions(C::mip_interface& d assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADAPTIVE_FILTER_OPTIONS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADAPTIVE_FILTER_OPTIONS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4314,7 +4314,7 @@ TypedResult readAdaptiveFilterOptions(C::mip_interface& d } return result; } -TypedResult saveAdaptiveFilterOptions(C::mip_interface& device) +CmdResult saveAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4324,7 +4324,7 @@ TypedResult saveAdaptiveFilterOptions(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAdaptiveFilterOptions(C::mip_interface& device) +CmdResult loadAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4334,7 +4334,7 @@ TypedResult loadAdaptiveFilterOptions(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAdaptiveFilterOptions(C::mip_interface& device) +CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4388,7 +4388,7 @@ void extract(Serializer& serializer, MultiAntennaOffset::Response& self) } -TypedResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) +CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4404,7 +4404,7 @@ TypedResult writeMultiAntennaOffset(C::mip_interface& device return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) +CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4415,7 +4415,7 @@ TypedResult readMultiAntennaOffset(C::mip_interface& device, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MULTI_ANTENNA_OFFSET, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MULTI_ANTENNA_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4432,7 +4432,7 @@ TypedResult readMultiAntennaOffset(C::mip_interface& device, } return result; } -TypedResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4444,7 +4444,7 @@ TypedResult saveMultiAntennaOffset(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4456,7 +4456,7 @@ TypedResult loadMultiAntennaOffset(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4520,7 +4520,7 @@ void extract(Serializer& serializer, RelPosConfiguration::Response& self) } -TypedResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) +CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4538,7 +4538,7 @@ TypedResult writeRelPosConfiguration(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) +CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4547,7 +4547,7 @@ TypedResult readRelPosConfiguration(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REL_POS_CONFIGURATION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REL_POS_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4568,7 +4568,7 @@ TypedResult readRelPosConfiguration(C::mip_interface& devic } return result; } -TypedResult saveRelPosConfiguration(C::mip_interface& device) +CmdResult saveRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4578,7 +4578,7 @@ TypedResult saveRelPosConfiguration(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadRelPosConfiguration(C::mip_interface& device) +CmdResult loadRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4588,7 +4588,7 @@ TypedResult loadRelPosConfiguration(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultRelPosConfiguration(C::mip_interface& device) +CmdResult defaultRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4642,7 +4642,7 @@ void extract(Serializer& serializer, RefPointLeverArm::Response& self) } -TypedResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) +CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4658,7 +4658,7 @@ TypedResult writeRefPointLeverArm(C::mip_interface& device, Re return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) +CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4667,7 +4667,7 @@ TypedResult readRefPointLeverArm(C::mip_interface& device, Ref assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REF_POINT_LEVER_ARM, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REF_POINT_LEVER_ARM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4685,7 +4685,7 @@ TypedResult readRefPointLeverArm(C::mip_interface& device, Ref } return result; } -TypedResult saveRefPointLeverArm(C::mip_interface& device) +CmdResult saveRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4695,7 +4695,7 @@ TypedResult saveRefPointLeverArm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadRefPointLeverArm(C::mip_interface& device) +CmdResult loadRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4705,7 +4705,7 @@ TypedResult loadRefPointLeverArm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultRefPointLeverArm(C::mip_interface& device) +CmdResult defaultRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4738,7 +4738,7 @@ void extract(Serializer& serializer, SpeedMeasurement& self) } -TypedResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty) +CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4799,7 +4799,7 @@ void extract(Serializer& serializer, SpeedLeverArm::Response& self) } -TypedResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) +CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4815,7 +4815,7 @@ TypedResult writeSpeedLeverArm(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) +CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4826,7 +4826,7 @@ TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t s assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPEED_LEVER_ARM, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPEED_LEVER_ARM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4843,7 +4843,7 @@ TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t s } return result; } -TypedResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) +CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4855,7 +4855,7 @@ TypedResult saveSpeedLeverArm(C::mip_interface& device, uint8_t s return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) +CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4867,7 +4867,7 @@ TypedResult loadSpeedLeverArm(C::mip_interface& device, uint8_t s return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source) +CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4911,7 +4911,7 @@ void extract(Serializer& serializer, WheeledVehicleConstraintControl::Response& } -TypedResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable) +CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4923,7 +4923,7 @@ TypedResult writeWheeledVehicleConstraintContro return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut) +CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4932,7 +4932,7 @@ TypedResult readWheeledVehicleConstraintControl assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_CONSTRAINT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_CONSTRAINT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4946,7 +4946,7 @@ TypedResult readWheeledVehicleConstraintControl } return result; } -TypedResult saveWheeledVehicleConstraintControl(C::mip_interface& device) +CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4956,7 +4956,7 @@ TypedResult saveWheeledVehicleConstraintControl return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadWheeledVehicleConstraintControl(C::mip_interface& device) +CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4966,7 +4966,7 @@ TypedResult loadWheeledVehicleConstraintControl return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultWheeledVehicleConstraintControl(C::mip_interface& device) +CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5008,7 +5008,7 @@ void extract(Serializer& serializer, VerticalGyroConstraintControl::Response& se } -TypedResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable) +CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5020,7 +5020,7 @@ TypedResult writeVerticalGyroConstraintControl(C: return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut) +CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5029,7 +5029,7 @@ TypedResult readVerticalGyroConstraintControl(C:: assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_CONSTRAINT_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_CONSTRAINT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -5043,7 +5043,7 @@ TypedResult readVerticalGyroConstraintControl(C:: } return result; } -TypedResult saveVerticalGyroConstraintControl(C::mip_interface& device) +CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5053,7 +5053,7 @@ TypedResult saveVerticalGyroConstraintControl(C:: return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadVerticalGyroConstraintControl(C::mip_interface& device) +CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5063,7 +5063,7 @@ TypedResult loadVerticalGyroConstraintControl(C:: return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultVerticalGyroConstraintControl(C::mip_interface& device) +CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5113,7 +5113,7 @@ void extract(Serializer& serializer, GnssAntennaCalControl::Response& self) } -TypedResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset) +CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5127,7 +5127,7 @@ TypedResult writeGnssAntennaCalControl(C::mip_interface& return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut) +CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5136,7 +5136,7 @@ TypedResult readGnssAntennaCalControl(C::mip_interface& d assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_CALIBRATION_CONTROL, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_CALIBRATION_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -5153,7 +5153,7 @@ TypedResult readGnssAntennaCalControl(C::mip_interface& d } return result; } -TypedResult saveGnssAntennaCalControl(C::mip_interface& device) +CmdResult saveGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5163,7 +5163,7 @@ TypedResult saveGnssAntennaCalControl(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadGnssAntennaCalControl(C::mip_interface& device) +CmdResult loadGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5173,7 +5173,7 @@ TypedResult loadGnssAntennaCalControl(C::mip_interface& d return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultGnssAntennaCalControl(C::mip_interface& device) +CmdResult defaultGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5194,7 +5194,7 @@ void extract(Serializer& serializer, SetInitialHeading& self) } -TypedResult setInitialHeading(C::mip_interface& device, float heading) +CmdResult setInitialHeading(C::mip_interface& device, float heading) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index e8ca1ccb3..8fddf4cd7 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -1764,7 +1764,8 @@ static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILT static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER = 4; ///< Magnetometer (built-in sensor) static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING = 5; ///< External heading input static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_ALTIMETER = 6; ///< External pressure altimeter input -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER = 7; ///< External magnetomer input input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER = 7; ///< External magnetomer input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_VEHICLE_FRAME_VEL = 8; ///< External vehicle frame velocity input static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALL = 65535; ///< Save/load/reset all options struct mip_filter_aiding_measurement_enable_command diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 42a300d54..bac15248d 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -195,8 +195,6 @@ struct Reset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Reset"; - static constexpr const char* DOC_NAME = "Reset Navigation Filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -215,7 +213,7 @@ struct Reset void insert(Serializer& serializer, const Reset& self); void extract(Serializer& serializer, Reset& self); -TypedResult reset(C::mip_interface& device); +CmdResult reset(C::mip_interface& device); ///@} /// @@ -245,8 +243,6 @@ struct SetInitialAttitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SetInitialAttitude"; - static constexpr const char* DOC_NAME = "Set Initial Attitude"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -265,7 +261,7 @@ struct SetInitialAttitude void insert(Serializer& serializer, const SetInitialAttitude& self); void extract(Serializer& serializer, SetInitialAttitude& self); -TypedResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading); +CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading); ///@} /// @@ -307,7 +303,7 @@ struct EstimationControl EnableFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } EnableFlags& operator=(uint16_t val) { value = val; return *this; } - EnableFlags& operator=(int val) { value = uint16_t(val); return *this; } + EnableFlags& operator=(int val) { value = val; return *this; } EnableFlags& operator|=(uint16_t val) { return *this = value | val; } EnableFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -336,8 +332,6 @@ struct EstimationControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EstimationControl"; - static constexpr const char* DOC_NAME = "Estimation Control Flags"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -370,8 +364,6 @@ struct EstimationControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EstimationControl::Response"; - static constexpr const char* DOC_NAME = "Estimation Control Flags Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -390,11 +382,11 @@ void extract(Serializer& serializer, EstimationControl& self); void insert(Serializer& serializer, const EstimationControl::Response& self); void extract(Serializer& serializer, EstimationControl::Response& self); -TypedResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable); -TypedResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut); -TypedResult saveEstimationControl(C::mip_interface& device); -TypedResult loadEstimationControl(C::mip_interface& device); -TypedResult defaultEstimationControl(C::mip_interface& device); +CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable); +CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut); +CmdResult saveEstimationControl(C::mip_interface& device); +CmdResult loadEstimationControl(C::mip_interface& device); +CmdResult defaultEstimationControl(C::mip_interface& device); ///@} /// @@ -422,8 +414,6 @@ struct ExternalGnssUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ExternalGnssUpdate"; - static constexpr const char* DOC_NAME = "External GNSS Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -442,7 +432,7 @@ struct ExternalGnssUpdate void insert(Serializer& serializer, const ExternalGnssUpdate& self); void extract(Serializer& serializer, ExternalGnssUpdate& self); -TypedResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); +CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); ///@} /// @@ -473,8 +463,6 @@ struct ExternalHeadingUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ExternalHeadingUpdate"; - static constexpr const char* DOC_NAME = "External Heading Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -493,7 +481,7 @@ struct ExternalHeadingUpdate void insert(Serializer& serializer, const ExternalHeadingUpdate& self); void extract(Serializer& serializer, ExternalHeadingUpdate& self); -TypedResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type); +CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type); ///@} /// @@ -530,8 +518,6 @@ struct ExternalHeadingUpdateWithTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ExternalHeadingUpdateWithTime"; - static constexpr const char* DOC_NAME = "External Heading Update With Time"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -550,7 +536,7 @@ struct ExternalHeadingUpdateWithTime void insert(Serializer& serializer, const ExternalHeadingUpdateWithTime& self); void extract(Serializer& serializer, ExternalHeadingUpdateWithTime& self); -TypedResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type); +CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type); ///@} /// @@ -582,7 +568,7 @@ struct TareOrientation MipTareAxes(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } MipTareAxes& operator=(uint8_t val) { value = val; return *this; } - MipTareAxes& operator=(int val) { value = uint8_t(val); return *this; } + MipTareAxes& operator=(int val) { value = val; return *this; } MipTareAxes& operator|=(uint8_t val) { return *this = value | val; } MipTareAxes& operator&=(uint8_t val) { return *this = value & val; } @@ -603,8 +589,6 @@ struct TareOrientation static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "TareOrientation"; - static constexpr const char* DOC_NAME = "Tare Sensor Orientation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -637,8 +621,6 @@ struct TareOrientation static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "TareOrientation::Response"; - static constexpr const char* DOC_NAME = "Tare Sensor Orientation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -657,11 +639,11 @@ void extract(Serializer& serializer, TareOrientation& self); void insert(Serializer& serializer, const TareOrientation::Response& self); void extract(Serializer& serializer, TareOrientation::Response& self); -TypedResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes); -TypedResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut); -TypedResult saveTareOrientation(C::mip_interface& device); -TypedResult loadTareOrientation(C::mip_interface& device); -TypedResult defaultTareOrientation(C::mip_interface& device); +CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes); +CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut); +CmdResult saveTareOrientation(C::mip_interface& device); +CmdResult loadTareOrientation(C::mip_interface& device); +CmdResult defaultTareOrientation(C::mip_interface& device); ///@} /// @@ -687,8 +669,6 @@ struct VehicleDynamicsMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VehicleDynamicsMode"; - static constexpr const char* DOC_NAME = "Vehicle Dynamics Mode"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -721,8 +701,6 @@ struct VehicleDynamicsMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VehicleDynamicsMode::Response"; - static constexpr const char* DOC_NAME = "Vehicle Dynamics Mode Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -741,11 +719,11 @@ void extract(Serializer& serializer, VehicleDynamicsMode& self); void insert(Serializer& serializer, const VehicleDynamicsMode::Response& self); void extract(Serializer& serializer, VehicleDynamicsMode::Response& self); -TypedResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); -TypedResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); -TypedResult saveVehicleDynamicsMode(C::mip_interface& device); -TypedResult loadVehicleDynamicsMode(C::mip_interface& device); -TypedResult defaultVehicleDynamicsMode(C::mip_interface& device); +CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); +CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); +CmdResult saveVehicleDynamicsMode(C::mip_interface& device); +CmdResult loadVehicleDynamicsMode(C::mip_interface& device); +CmdResult defaultVehicleDynamicsMode(C::mip_interface& device); ///@} /// @@ -787,8 +765,6 @@ struct SensorToVehicleRotationEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleRotationEuler"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Euler"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -821,8 +797,6 @@ struct SensorToVehicleRotationEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleRotationEuler::Response"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Euler Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -843,11 +817,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationEuler& self); void insert(Serializer& serializer, const SensorToVehicleRotationEuler::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationEuler::Response& self); -TypedResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw); -TypedResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); -TypedResult saveSensorToVehicleRotationEuler(C::mip_interface& device); -TypedResult loadSensorToVehicleRotationEuler(C::mip_interface& device); -TypedResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); +CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw); +CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); +CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device); +CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device); +CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); ///@} /// @@ -893,8 +867,6 @@ struct SensorToVehicleRotationDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleRotationDcm"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation DCM"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -927,8 +899,6 @@ struct SensorToVehicleRotationDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleRotationDcm::Response"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation DCM Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -947,11 +917,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm& self); void insert(Serializer& serializer, const SensorToVehicleRotationDcm::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self); -TypedResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); -TypedResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); -TypedResult saveSensorToVehicleRotationDcm(C::mip_interface& device); -TypedResult loadSensorToVehicleRotationDcm(C::mip_interface& device); -TypedResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); +CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); +CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); +CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device); +CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device); +CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); ///@} /// @@ -996,8 +966,6 @@ struct SensorToVehicleRotationQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleRotationQuaternion"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Quaternion"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1030,8 +998,6 @@ struct SensorToVehicleRotationQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleRotationQuaternion::Response"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Quaternion Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1050,11 +1016,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion& self); void insert(Serializer& serializer, const SensorToVehicleRotationQuaternion::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response& self); -TypedResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); -TypedResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); -TypedResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); -TypedResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); -TypedResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); +CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); +CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); +CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); +CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); +CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); ///@} /// @@ -1080,8 +1046,6 @@ struct SensorToVehicleOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleOffset"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Offset"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1114,8 +1078,6 @@ struct SensorToVehicleOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorToVehicleOffset::Response"; - static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Offset Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1134,11 +1096,11 @@ void extract(Serializer& serializer, SensorToVehicleOffset& self); void insert(Serializer& serializer, const SensorToVehicleOffset::Response& self); void extract(Serializer& serializer, SensorToVehicleOffset::Response& self); -TypedResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); -TypedResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); -TypedResult saveSensorToVehicleOffset(C::mip_interface& device); -TypedResult loadSensorToVehicleOffset(C::mip_interface& device); -TypedResult defaultSensorToVehicleOffset(C::mip_interface& device); +CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); +CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); +CmdResult saveSensorToVehicleOffset(C::mip_interface& device); +CmdResult loadSensorToVehicleOffset(C::mip_interface& device); +CmdResult defaultSensorToVehicleOffset(C::mip_interface& device); ///@} /// @@ -1161,8 +1123,6 @@ struct AntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AntennaOffset"; - static constexpr const char* DOC_NAME = "GNSS Antenna Offset Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1195,8 +1155,6 @@ struct AntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AntennaOffset::Response"; - static constexpr const char* DOC_NAME = "GNSS Antenna Offset Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1215,11 +1173,11 @@ void extract(Serializer& serializer, AntennaOffset& self); void insert(Serializer& serializer, const AntennaOffset::Response& self); void extract(Serializer& serializer, AntennaOffset::Response& self); -TypedResult writeAntennaOffset(C::mip_interface& device, const float* offset); -TypedResult readAntennaOffset(C::mip_interface& device, float* offsetOut); -TypedResult saveAntennaOffset(C::mip_interface& device); -TypedResult loadAntennaOffset(C::mip_interface& device); -TypedResult defaultAntennaOffset(C::mip_interface& device); +CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset); +CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut); +CmdResult saveAntennaOffset(C::mip_interface& device); +CmdResult loadAntennaOffset(C::mip_interface& device); +CmdResult defaultAntennaOffset(C::mip_interface& device); ///@} /// @@ -1249,8 +1207,6 @@ struct GnssSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssSource"; - static constexpr const char* DOC_NAME = "GNSS Aiding Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1283,8 +1239,6 @@ struct GnssSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssSource::Response"; - static constexpr const char* DOC_NAME = "GNSS Aiding Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1303,11 +1257,11 @@ void extract(Serializer& serializer, GnssSource& self); void insert(Serializer& serializer, const GnssSource::Response& self); void extract(Serializer& serializer, GnssSource::Response& self); -TypedResult writeGnssSource(C::mip_interface& device, GnssSource::Source source); -TypedResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut); -TypedResult saveGnssSource(C::mip_interface& device); -TypedResult loadGnssSource(C::mip_interface& device); -TypedResult defaultGnssSource(C::mip_interface& device); +CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source); +CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut); +CmdResult saveGnssSource(C::mip_interface& device); +CmdResult loadGnssSource(C::mip_interface& device); +CmdResult defaultGnssSource(C::mip_interface& device); ///@} /// @@ -1348,8 +1302,6 @@ struct HeadingSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "HeadingSource"; - static constexpr const char* DOC_NAME = "Heading Aiding Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1382,8 +1334,6 @@ struct HeadingSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "HeadingSource::Response"; - static constexpr const char* DOC_NAME = "Heading Aiding Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1402,11 +1352,11 @@ void extract(Serializer& serializer, HeadingSource& self); void insert(Serializer& serializer, const HeadingSource::Response& self); void extract(Serializer& serializer, HeadingSource::Response& self); -TypedResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source); -TypedResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut); -TypedResult saveHeadingSource(C::mip_interface& device); -TypedResult loadHeadingSource(C::mip_interface& device); -TypedResult defaultHeadingSource(C::mip_interface& device); +CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source); +CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut); +CmdResult saveHeadingSource(C::mip_interface& device); +CmdResult loadHeadingSource(C::mip_interface& device); +CmdResult defaultHeadingSource(C::mip_interface& device); ///@} /// @@ -1432,8 +1382,6 @@ struct AutoInitControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AutoInitControl"; - static constexpr const char* DOC_NAME = "Auto-initialization Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1466,8 +1414,6 @@ struct AutoInitControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AutoInitControl::Response"; - static constexpr const char* DOC_NAME = "Auto-initialization Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1486,11 +1432,11 @@ void extract(Serializer& serializer, AutoInitControl& self); void insert(Serializer& serializer, const AutoInitControl::Response& self); void extract(Serializer& serializer, AutoInitControl::Response& self); -TypedResult writeAutoInitControl(C::mip_interface& device, uint8_t enable); -TypedResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut); -TypedResult saveAutoInitControl(C::mip_interface& device); -TypedResult loadAutoInitControl(C::mip_interface& device); -TypedResult defaultAutoInitControl(C::mip_interface& device); +CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable); +CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut); +CmdResult saveAutoInitControl(C::mip_interface& device); +CmdResult loadAutoInitControl(C::mip_interface& device); +CmdResult defaultAutoInitControl(C::mip_interface& device); ///@} /// @@ -1514,8 +1460,6 @@ struct AccelNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelNoise"; - static constexpr const char* DOC_NAME = "Accelerometer Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1548,8 +1492,6 @@ struct AccelNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelNoise::Response"; - static constexpr const char* DOC_NAME = "Accelerometer Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1568,11 +1510,11 @@ void extract(Serializer& serializer, AccelNoise& self); void insert(Serializer& serializer, const AccelNoise::Response& self); void extract(Serializer& serializer, AccelNoise::Response& self); -TypedResult writeAccelNoise(C::mip_interface& device, const float* noise); -TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut); -TypedResult saveAccelNoise(C::mip_interface& device); -TypedResult loadAccelNoise(C::mip_interface& device); -TypedResult defaultAccelNoise(C::mip_interface& device); +CmdResult writeAccelNoise(C::mip_interface& device, const float* noise); +CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveAccelNoise(C::mip_interface& device); +CmdResult loadAccelNoise(C::mip_interface& device); +CmdResult defaultAccelNoise(C::mip_interface& device); ///@} /// @@ -1596,8 +1538,6 @@ struct GyroNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroNoise"; - static constexpr const char* DOC_NAME = "Gyroscope Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1630,8 +1570,6 @@ struct GyroNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroNoise::Response"; - static constexpr const char* DOC_NAME = "Gyroscope Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1650,11 +1588,11 @@ void extract(Serializer& serializer, GyroNoise& self); void insert(Serializer& serializer, const GyroNoise::Response& self); void extract(Serializer& serializer, GyroNoise::Response& self); -TypedResult writeGyroNoise(C::mip_interface& device, const float* noise); -TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut); -TypedResult saveGyroNoise(C::mip_interface& device); -TypedResult loadGyroNoise(C::mip_interface& device); -TypedResult defaultGyroNoise(C::mip_interface& device); +CmdResult writeGyroNoise(C::mip_interface& device, const float* noise); +CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveGyroNoise(C::mip_interface& device); +CmdResult loadGyroNoise(C::mip_interface& device); +CmdResult defaultGyroNoise(C::mip_interface& device); ///@} /// @@ -1676,8 +1614,6 @@ struct AccelBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelBiasModel"; - static constexpr const char* DOC_NAME = "Accelerometer Bias Model Parameters"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1710,8 +1646,6 @@ struct AccelBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelBiasModel::Response"; - static constexpr const char* DOC_NAME = "Accelerometer Bias Model Parameters Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1731,11 +1665,11 @@ void extract(Serializer& serializer, AccelBiasModel& self); void insert(Serializer& serializer, const AccelBiasModel::Response& self); void extract(Serializer& serializer, AccelBiasModel::Response& self); -TypedResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); -TypedResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); -TypedResult saveAccelBiasModel(C::mip_interface& device); -TypedResult loadAccelBiasModel(C::mip_interface& device); -TypedResult defaultAccelBiasModel(C::mip_interface& device); +CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); +CmdResult saveAccelBiasModel(C::mip_interface& device); +CmdResult loadAccelBiasModel(C::mip_interface& device); +CmdResult defaultAccelBiasModel(C::mip_interface& device); ///@} /// @@ -1757,8 +1691,6 @@ struct GyroBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroBiasModel"; - static constexpr const char* DOC_NAME = "Gyroscope Bias Model Parameters"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1791,8 +1723,6 @@ struct GyroBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroBiasModel::Response"; - static constexpr const char* DOC_NAME = "Gyroscope Bias Model Parameters Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1812,11 +1742,11 @@ void extract(Serializer& serializer, GyroBiasModel& self); void insert(Serializer& serializer, const GyroBiasModel::Response& self); void extract(Serializer& serializer, GyroBiasModel::Response& self); -TypedResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); -TypedResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); -TypedResult saveGyroBiasModel(C::mip_interface& device); -TypedResult loadGyroBiasModel(C::mip_interface& device); -TypedResult defaultGyroBiasModel(C::mip_interface& device); +CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); +CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); +CmdResult saveGyroBiasModel(C::mip_interface& device); +CmdResult loadGyroBiasModel(C::mip_interface& device); +CmdResult defaultGyroBiasModel(C::mip_interface& device); ///@} /// @@ -1844,8 +1774,6 @@ struct AltitudeAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AltitudeAiding"; - static constexpr const char* DOC_NAME = "Altitude Aiding Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1878,8 +1806,6 @@ struct AltitudeAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AltitudeAiding::Response"; - static constexpr const char* DOC_NAME = "Altitude Aiding Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1898,11 +1824,11 @@ void extract(Serializer& serializer, AltitudeAiding& self); void insert(Serializer& serializer, const AltitudeAiding::Response& self); void extract(Serializer& serializer, AltitudeAiding::Response& self); -TypedResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); -TypedResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); -TypedResult saveAltitudeAiding(C::mip_interface& device); -TypedResult loadAltitudeAiding(C::mip_interface& device); -TypedResult defaultAltitudeAiding(C::mip_interface& device); +CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); +CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); +CmdResult saveAltitudeAiding(C::mip_interface& device); +CmdResult loadAltitudeAiding(C::mip_interface& device); +CmdResult defaultAltitudeAiding(C::mip_interface& device); ///@} /// @@ -1927,8 +1853,6 @@ struct PitchRollAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PitchRollAiding"; - static constexpr const char* DOC_NAME = "Pitch/Roll Aiding Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1961,8 +1885,6 @@ struct PitchRollAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PitchRollAiding::Response"; - static constexpr const char* DOC_NAME = "Pitch/Roll Aiding Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1981,11 +1903,11 @@ void extract(Serializer& serializer, PitchRollAiding& self); void insert(Serializer& serializer, const PitchRollAiding::Response& self); void extract(Serializer& serializer, PitchRollAiding::Response& self); -TypedResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); -TypedResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); -TypedResult savePitchRollAiding(C::mip_interface& device); -TypedResult loadPitchRollAiding(C::mip_interface& device); -TypedResult defaultPitchRollAiding(C::mip_interface& device); +CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); +CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); +CmdResult savePitchRollAiding(C::mip_interface& device); +CmdResult loadPitchRollAiding(C::mip_interface& device); +CmdResult defaultPitchRollAiding(C::mip_interface& device); ///@} /// @@ -2005,8 +1927,6 @@ struct AutoZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AutoZupt"; - static constexpr const char* DOC_NAME = "Zero Velocity Update Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2039,8 +1959,6 @@ struct AutoZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AutoZupt::Response"; - static constexpr const char* DOC_NAME = "Zero Velocity Update Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2060,11 +1978,11 @@ void extract(Serializer& serializer, AutoZupt& self); void insert(Serializer& serializer, const AutoZupt::Response& self); void extract(Serializer& serializer, AutoZupt::Response& self); -TypedResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold); -TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); -TypedResult saveAutoZupt(C::mip_interface& device); -TypedResult loadAutoZupt(C::mip_interface& device); -TypedResult defaultAutoZupt(C::mip_interface& device); +CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold); +CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); +CmdResult saveAutoZupt(C::mip_interface& device); +CmdResult loadAutoZupt(C::mip_interface& device); +CmdResult defaultAutoZupt(C::mip_interface& device); ///@} /// @@ -2085,8 +2003,6 @@ struct AutoAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AutoAngularZupt"; - static constexpr const char* DOC_NAME = "Zero Angular Rate Update Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2119,8 +2035,6 @@ struct AutoAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AutoAngularZupt::Response"; - static constexpr const char* DOC_NAME = "Zero Angular Rate Update Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2140,11 +2054,11 @@ void extract(Serializer& serializer, AutoAngularZupt& self); void insert(Serializer& serializer, const AutoAngularZupt::Response& self); void extract(Serializer& serializer, AutoAngularZupt::Response& self); -TypedResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold); -TypedResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); -TypedResult saveAutoAngularZupt(C::mip_interface& device); -TypedResult loadAutoAngularZupt(C::mip_interface& device); -TypedResult defaultAutoAngularZupt(C::mip_interface& device); +CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold); +CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); +CmdResult saveAutoAngularZupt(C::mip_interface& device); +CmdResult loadAutoAngularZupt(C::mip_interface& device); +CmdResult defaultAutoAngularZupt(C::mip_interface& device); ///@} /// @@ -2160,8 +2074,6 @@ struct CommandedZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CommandedZupt"; - static constexpr const char* DOC_NAME = "Commanded Zero Velocity Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2180,7 +2092,7 @@ struct CommandedZupt void insert(Serializer& serializer, const CommandedZupt& self); void extract(Serializer& serializer, CommandedZupt& self); -TypedResult commandedZupt(C::mip_interface& device); +CmdResult commandedZupt(C::mip_interface& device); ///@} /// @@ -2196,8 +2108,6 @@ struct CommandedAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CommandedAngularZupt"; - static constexpr const char* DOC_NAME = "Commanded Zero Angular Rate Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2216,7 +2126,7 @@ struct CommandedAngularZupt void insert(Serializer& serializer, const CommandedAngularZupt& self); void extract(Serializer& serializer, CommandedAngularZupt& self); -TypedResult commandedAngularZupt(C::mip_interface& device); +CmdResult commandedAngularZupt(C::mip_interface& device); ///@} /// @@ -2235,8 +2145,6 @@ struct MagCaptureAutoCal static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagCaptureAutoCal"; - static constexpr const char* DOC_NAME = "Magnetometer Capture Auto Calibration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8000; @@ -2268,8 +2176,8 @@ struct MagCaptureAutoCal void insert(Serializer& serializer, const MagCaptureAutoCal& self); void extract(Serializer& serializer, MagCaptureAutoCal& self); -TypedResult writeMagCaptureAutoCal(C::mip_interface& device); -TypedResult saveMagCaptureAutoCal(C::mip_interface& device); +CmdResult writeMagCaptureAutoCal(C::mip_interface& device); +CmdResult saveMagCaptureAutoCal(C::mip_interface& device); ///@} /// @@ -2292,8 +2200,6 @@ struct GravityNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GravityNoise"; - static constexpr const char* DOC_NAME = "Gravity Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2326,8 +2232,6 @@ struct GravityNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GravityNoise::Response"; - static constexpr const char* DOC_NAME = "Gravity Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2346,11 +2250,11 @@ void extract(Serializer& serializer, GravityNoise& self); void insert(Serializer& serializer, const GravityNoise::Response& self); void extract(Serializer& serializer, GravityNoise::Response& self); -TypedResult writeGravityNoise(C::mip_interface& device, const float* noise); -TypedResult readGravityNoise(C::mip_interface& device, float* noiseOut); -TypedResult saveGravityNoise(C::mip_interface& device); -TypedResult loadGravityNoise(C::mip_interface& device); -TypedResult defaultGravityNoise(C::mip_interface& device); +CmdResult writeGravityNoise(C::mip_interface& device, const float* noise); +CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveGravityNoise(C::mip_interface& device); +CmdResult loadGravityNoise(C::mip_interface& device); +CmdResult defaultGravityNoise(C::mip_interface& device); ///@} /// @@ -2373,8 +2277,6 @@ struct PressureAltitudeNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PressureAltitudeNoise"; - static constexpr const char* DOC_NAME = "Pressure Altitude Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2407,8 +2309,6 @@ struct PressureAltitudeNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PressureAltitudeNoise::Response"; - static constexpr const char* DOC_NAME = "Pressure Altitude Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2427,11 +2327,11 @@ void extract(Serializer& serializer, PressureAltitudeNoise& self); void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self); void extract(Serializer& serializer, PressureAltitudeNoise::Response& self); -TypedResult writePressureAltitudeNoise(C::mip_interface& device, float noise); -TypedResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); -TypedResult savePressureAltitudeNoise(C::mip_interface& device); -TypedResult loadPressureAltitudeNoise(C::mip_interface& device); -TypedResult defaultPressureAltitudeNoise(C::mip_interface& device); +CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise); +CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); +CmdResult savePressureAltitudeNoise(C::mip_interface& device); +CmdResult loadPressureAltitudeNoise(C::mip_interface& device); +CmdResult defaultPressureAltitudeNoise(C::mip_interface& device); ///@} /// @@ -2456,8 +2356,6 @@ struct HardIronOffsetNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "HardIronOffsetNoise"; - static constexpr const char* DOC_NAME = "Hard Iron Offset Process Noise"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2490,8 +2388,6 @@ struct HardIronOffsetNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "HardIronOffsetNoise::Response"; - static constexpr const char* DOC_NAME = "Hard Iron Offset Process Noise Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2510,11 +2406,11 @@ void extract(Serializer& serializer, HardIronOffsetNoise& self); void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); -TypedResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); -TypedResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); -TypedResult saveHardIronOffsetNoise(C::mip_interface& device); -TypedResult loadHardIronOffsetNoise(C::mip_interface& device); -TypedResult defaultHardIronOffsetNoise(C::mip_interface& device); +CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); +CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveHardIronOffsetNoise(C::mip_interface& device); +CmdResult loadHardIronOffsetNoise(C::mip_interface& device); +CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); ///@} /// @@ -2538,8 +2434,6 @@ struct SoftIronMatrixNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SoftIronMatrixNoise"; - static constexpr const char* DOC_NAME = "Soft Iron Offset Process Noise"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2572,8 +2466,6 @@ struct SoftIronMatrixNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SoftIronMatrixNoise::Response"; - static constexpr const char* DOC_NAME = "Soft Iron Offset Process Noise Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2592,11 +2484,11 @@ void extract(Serializer& serializer, SoftIronMatrixNoise& self); void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self); void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self); -TypedResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); -TypedResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); -TypedResult saveSoftIronMatrixNoise(C::mip_interface& device); -TypedResult loadSoftIronMatrixNoise(C::mip_interface& device); -TypedResult defaultSoftIronMatrixNoise(C::mip_interface& device); +CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); +CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveSoftIronMatrixNoise(C::mip_interface& device); +CmdResult loadSoftIronMatrixNoise(C::mip_interface& device); +CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); ///@} /// @@ -2620,8 +2512,6 @@ struct MagNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagNoise"; - static constexpr const char* DOC_NAME = "Magnetometer Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2654,8 +2544,6 @@ struct MagNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagNoise::Response"; - static constexpr const char* DOC_NAME = "Magnetometer Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2674,11 +2562,11 @@ void extract(Serializer& serializer, MagNoise& self); void insert(Serializer& serializer, const MagNoise::Response& self); void extract(Serializer& serializer, MagNoise::Response& self); -TypedResult writeMagNoise(C::mip_interface& device, const float* noise); -TypedResult readMagNoise(C::mip_interface& device, float* noiseOut); -TypedResult saveMagNoise(C::mip_interface& device); -TypedResult loadMagNoise(C::mip_interface& device); -TypedResult defaultMagNoise(C::mip_interface& device); +CmdResult writeMagNoise(C::mip_interface& device, const float* noise); +CmdResult readMagNoise(C::mip_interface& device, float* noiseOut); +CmdResult saveMagNoise(C::mip_interface& device); +CmdResult loadMagNoise(C::mip_interface& device); +CmdResult defaultMagNoise(C::mip_interface& device); ///@} /// @@ -2701,8 +2589,6 @@ struct InclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "InclinationSource"; - static constexpr const char* DOC_NAME = "Inclination Source"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2735,8 +2621,6 @@ struct InclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "InclinationSource::Response"; - static constexpr const char* DOC_NAME = "Inclination Source Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2756,11 +2640,11 @@ void extract(Serializer& serializer, InclinationSource& self); void insert(Serializer& serializer, const InclinationSource::Response& self); void extract(Serializer& serializer, InclinationSource::Response& self); -TypedResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); -TypedResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); -TypedResult saveInclinationSource(C::mip_interface& device); -TypedResult loadInclinationSource(C::mip_interface& device); -TypedResult defaultInclinationSource(C::mip_interface& device); +CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); +CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); +CmdResult saveInclinationSource(C::mip_interface& device); +CmdResult loadInclinationSource(C::mip_interface& device); +CmdResult defaultInclinationSource(C::mip_interface& device); ///@} /// @@ -2783,8 +2667,6 @@ struct MagneticDeclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagneticDeclinationSource"; - static constexpr const char* DOC_NAME = "Magnetic Field Declination Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2817,8 +2699,6 @@ struct MagneticDeclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagneticDeclinationSource::Response"; - static constexpr const char* DOC_NAME = "Magnetic Field Declination Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2838,11 +2718,11 @@ void extract(Serializer& serializer, MagneticDeclinationSource& self); void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); -TypedResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); -TypedResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); -TypedResult saveMagneticDeclinationSource(C::mip_interface& device); -TypedResult loadMagneticDeclinationSource(C::mip_interface& device); -TypedResult defaultMagneticDeclinationSource(C::mip_interface& device); +CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); +CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); +CmdResult saveMagneticDeclinationSource(C::mip_interface& device); +CmdResult loadMagneticDeclinationSource(C::mip_interface& device); +CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); ///@} /// @@ -2864,8 +2744,6 @@ struct MagFieldMagnitudeSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagFieldMagnitudeSource"; - static constexpr const char* DOC_NAME = "Magnetic Field Magnitude Source"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2898,8 +2776,6 @@ struct MagFieldMagnitudeSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagFieldMagnitudeSource::Response"; - static constexpr const char* DOC_NAME = "Magnetic Field Magnitude Source Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2919,11 +2795,11 @@ void extract(Serializer& serializer, MagFieldMagnitudeSource& self); void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self); void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self); -TypedResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); -TypedResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); -TypedResult saveMagFieldMagnitudeSource(C::mip_interface& device); -TypedResult loadMagFieldMagnitudeSource(C::mip_interface& device); -TypedResult defaultMagFieldMagnitudeSource(C::mip_interface& device); +CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); +CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); +CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device); +CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device); +CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device); ///@} /// @@ -2947,8 +2823,6 @@ struct ReferencePosition static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferencePosition"; - static constexpr const char* DOC_NAME = "Set Reference Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2981,8 +2855,6 @@ struct ReferencePosition static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferencePosition::Response"; - static constexpr const char* DOC_NAME = "Set Reference Position Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3004,11 +2876,11 @@ void extract(Serializer& serializer, ReferencePosition& self); void insert(Serializer& serializer, const ReferencePosition::Response& self); void extract(Serializer& serializer, ReferencePosition::Response& self); -TypedResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); -TypedResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); -TypedResult saveReferencePosition(C::mip_interface& device); -TypedResult loadReferencePosition(C::mip_interface& device); -TypedResult defaultReferencePosition(C::mip_interface& device); +CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); +CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); +CmdResult saveReferencePosition(C::mip_interface& device); +CmdResult loadReferencePosition(C::mip_interface& device); +CmdResult defaultReferencePosition(C::mip_interface& device); ///@} /// @@ -3045,8 +2917,6 @@ struct AccelMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelMagnitudeErrorAdaptiveMeasurement"; - static constexpr const char* DOC_NAME = "Gravity Magnitude Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x807F; @@ -3079,8 +2949,6 @@ struct AccelMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelMagnitudeErrorAdaptiveMeasurement::Response"; - static constexpr const char* DOC_NAME = "Gravity Magnitude Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3105,11 +2973,11 @@ void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement& sel void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Response& self); -TypedResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); -TypedResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -TypedResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -TypedResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -TypedResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3141,8 +3009,6 @@ struct MagMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagMagnitudeErrorAdaptiveMeasurement"; - static constexpr const char* DOC_NAME = "Magnetometer Magnitude Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x807F; @@ -3175,8 +3041,6 @@ struct MagMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagMagnitudeErrorAdaptiveMeasurement::Response"; - static constexpr const char* DOC_NAME = "Magnetometer Magnitude Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3201,11 +3065,11 @@ void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement& self) void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Response& self); -TypedResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); -TypedResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -TypedResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -TypedResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -TypedResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3237,8 +3101,6 @@ struct MagDipAngleErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagDipAngleErrorAdaptiveMeasurement"; - static constexpr const char* DOC_NAME = "Magnetometer Dig Angle Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -3271,8 +3133,6 @@ struct MagDipAngleErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagDipAngleErrorAdaptiveMeasurement::Response"; - static constexpr const char* DOC_NAME = "Magnetometer Dig Angle Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3295,11 +3155,11 @@ void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement& self); void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Response& self); -TypedResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); -TypedResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -TypedResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); -TypedResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); -TypedResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); +CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3322,7 +3182,8 @@ struct AidingMeasurementEnable MAGNETOMETER = 4, ///< Magnetometer (built-in sensor) EXTERNAL_HEADING = 5, ///< External heading input EXTERNAL_ALTIMETER = 6, ///< External pressure altimeter input - EXTERNAL_MAGNETOMETER = 7, ///< External magnetomer input input + EXTERNAL_MAGNETOMETER = 7, ///< External magnetomer input + VEHICLE_FRAME_VEL = 8, ///< External vehicle frame velocity input ALL = 65535, ///< Save/load/reset all options }; @@ -3333,8 +3194,6 @@ struct AidingMeasurementEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingMeasurementEnable"; - static constexpr const char* DOC_NAME = "Aiding Measurement Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3368,8 +3227,6 @@ struct AidingMeasurementEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingMeasurementEnable::Response"; - static constexpr const char* DOC_NAME = "Aiding Measurement Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3389,11 +3246,11 @@ void extract(Serializer& serializer, AidingMeasurementEnable& self); void insert(Serializer& serializer, const AidingMeasurementEnable::Response& self); void extract(Serializer& serializer, AidingMeasurementEnable::Response& self); -TypedResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable); -TypedResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut); -TypedResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); -TypedResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); -TypedResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable); +CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut); +CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); ///@} /// @@ -3411,8 +3268,6 @@ struct Run static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Run"; - static constexpr const char* DOC_NAME = "Run Navigation Filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3431,7 +3286,7 @@ struct Run void insert(Serializer& serializer, const Run& self); void extract(Serializer& serializer, Run& self); -TypedResult run(C::mip_interface& device); +CmdResult run(C::mip_interface& device); ///@} /// @@ -3453,8 +3308,6 @@ struct KinematicConstraint static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "KinematicConstraint"; - static constexpr const char* DOC_NAME = "Kinematic Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3487,8 +3340,6 @@ struct KinematicConstraint static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "KinematicConstraint::Response"; - static constexpr const char* DOC_NAME = "Kinematic Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3509,11 +3360,11 @@ void extract(Serializer& serializer, KinematicConstraint& self); void insert(Serializer& serializer, const KinematicConstraint::Response& self); void extract(Serializer& serializer, KinematicConstraint::Response& self); -TypedResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection); -TypedResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut); -TypedResult saveKinematicConstraint(C::mip_interface& device); -TypedResult loadKinematicConstraint(C::mip_interface& device); -TypedResult defaultKinematicConstraint(C::mip_interface& device); +CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection); +CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut); +CmdResult saveKinematicConstraint(C::mip_interface& device); +CmdResult loadKinematicConstraint(C::mip_interface& device); +CmdResult defaultKinematicConstraint(C::mip_interface& device); ///@} /// @@ -3546,7 +3397,7 @@ struct InitializationConfiguration AlignmentSelector(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } AlignmentSelector& operator=(uint8_t val) { value = val; return *this; } - AlignmentSelector& operator=(int val) { value = uint8_t(val); return *this; } + AlignmentSelector& operator=(int val) { value = val; return *this; } AlignmentSelector& operator|=(uint8_t val) { return *this = value | val; } AlignmentSelector& operator&=(uint8_t val) { return *this = value & val; } @@ -3585,8 +3436,6 @@ struct InitializationConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "InitializationConfiguration"; - static constexpr const char* DOC_NAME = "Navigation Filter Initialization"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x81FF; @@ -3619,8 +3468,6 @@ struct InitializationConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "InitializationConfiguration::Response"; - static constexpr const char* DOC_NAME = "Navigation Filter Initialization Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3647,11 +3494,11 @@ void extract(Serializer& serializer, InitializationConfiguration& self); void insert(Serializer& serializer, const InitializationConfiguration::Response& self); void extract(Serializer& serializer, InitializationConfiguration::Response& self); -TypedResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); -TypedResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); -TypedResult saveInitializationConfiguration(C::mip_interface& device); -TypedResult loadInitializationConfiguration(C::mip_interface& device); -TypedResult defaultInitializationConfiguration(C::mip_interface& device); +CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); +CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); +CmdResult saveInitializationConfiguration(C::mip_interface& device); +CmdResult loadInitializationConfiguration(C::mip_interface& device); +CmdResult defaultInitializationConfiguration(C::mip_interface& device); ///@} /// @@ -3670,8 +3517,6 @@ struct AdaptiveFilterOptions static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AdaptiveFilterOptions"; - static constexpr const char* DOC_NAME = "Adaptive Filter Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3704,8 +3549,6 @@ struct AdaptiveFilterOptions static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AdaptiveFilterOptions::Response"; - static constexpr const char* DOC_NAME = "Adaptive Filter Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3725,11 +3568,11 @@ void extract(Serializer& serializer, AdaptiveFilterOptions& self); void insert(Serializer& serializer, const AdaptiveFilterOptions::Response& self); void extract(Serializer& serializer, AdaptiveFilterOptions::Response& self); -TypedResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit); -TypedResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut); -TypedResult saveAdaptiveFilterOptions(C::mip_interface& device); -TypedResult loadAdaptiveFilterOptions(C::mip_interface& device); -TypedResult defaultAdaptiveFilterOptions(C::mip_interface& device); +CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit); +CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut); +CmdResult saveAdaptiveFilterOptions(C::mip_interface& device); +CmdResult loadAdaptiveFilterOptions(C::mip_interface& device); +CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device); ///@} /// @@ -3751,8 +3594,6 @@ struct MultiAntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MultiAntennaOffset"; - static constexpr const char* DOC_NAME = "GNSS Multi-Antenna Offset Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3786,8 +3627,6 @@ struct MultiAntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MultiAntennaOffset::Response"; - static constexpr const char* DOC_NAME = "GNSS Multi-Antenna Offset Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3807,11 +3646,11 @@ void extract(Serializer& serializer, MultiAntennaOffset& self); void insert(Serializer& serializer, const MultiAntennaOffset::Response& self); void extract(Serializer& serializer, MultiAntennaOffset::Response& self); -TypedResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); -TypedResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); -TypedResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); -TypedResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); -TypedResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); +CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); +CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); ///@} /// @@ -3831,8 +3670,6 @@ struct RelPosConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RelPosConfiguration"; - static constexpr const char* DOC_NAME = "Relative Position Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3865,8 +3702,6 @@ struct RelPosConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RelPosConfiguration::Response"; - static constexpr const char* DOC_NAME = "Relative Position Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3887,11 +3722,11 @@ void extract(Serializer& serializer, RelPosConfiguration& self); void insert(Serializer& serializer, const RelPosConfiguration::Response& self); void extract(Serializer& serializer, RelPosConfiguration::Response& self); -TypedResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); -TypedResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); -TypedResult saveRelPosConfiguration(C::mip_interface& device); -TypedResult loadRelPosConfiguration(C::mip_interface& device); -TypedResult defaultRelPosConfiguration(C::mip_interface& device); +CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); +CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); +CmdResult saveRelPosConfiguration(C::mip_interface& device); +CmdResult loadRelPosConfiguration(C::mip_interface& device); +CmdResult defaultRelPosConfiguration(C::mip_interface& device); ///@} /// @@ -3922,8 +3757,6 @@ struct RefPointLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RefPointLeverArm"; - static constexpr const char* DOC_NAME = "Reference point lever arm"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3956,8 +3789,6 @@ struct RefPointLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RefPointLeverArm::Response"; - static constexpr const char* DOC_NAME = "Reference point lever arm Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3977,11 +3808,11 @@ void extract(Serializer& serializer, RefPointLeverArm& self); void insert(Serializer& serializer, const RefPointLeverArm::Response& self); void extract(Serializer& serializer, RefPointLeverArm::Response& self); -TypedResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); -TypedResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); -TypedResult saveRefPointLeverArm(C::mip_interface& device); -TypedResult loadRefPointLeverArm(C::mip_interface& device); -TypedResult defaultRefPointLeverArm(C::mip_interface& device); +CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); +CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); +CmdResult saveRefPointLeverArm(C::mip_interface& device); +CmdResult loadRefPointLeverArm(C::mip_interface& device); +CmdResult defaultRefPointLeverArm(C::mip_interface& device); ///@} /// @@ -4003,8 +3834,6 @@ struct SpeedMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SpeedMeasurement"; - static constexpr const char* DOC_NAME = "Input speed measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4023,7 +3852,7 @@ struct SpeedMeasurement void insert(Serializer& serializer, const SpeedMeasurement& self); void extract(Serializer& serializer, SpeedMeasurement& self); -TypedResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty); +CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty); ///@} /// @@ -4048,8 +3877,6 @@ struct SpeedLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SpeedLeverArm"; - static constexpr const char* DOC_NAME = "Measurement speed lever arm"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -4083,8 +3910,6 @@ struct SpeedLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SpeedLeverArm::Response"; - static constexpr const char* DOC_NAME = "Measurement speed lever arm Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4104,11 +3929,11 @@ void extract(Serializer& serializer, SpeedLeverArm& self); void insert(Serializer& serializer, const SpeedLeverArm::Response& self); void extract(Serializer& serializer, SpeedLeverArm::Response& self); -TypedResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); -TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); -TypedResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); -TypedResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); -TypedResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); +CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); +CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); +CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); +CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); +CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); ///@} /// @@ -4132,8 +3957,6 @@ struct WheeledVehicleConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "WheeledVehicleConstraintControl"; - static constexpr const char* DOC_NAME = "Wheeled Vehicle Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -4166,8 +3989,6 @@ struct WheeledVehicleConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "WheeledVehicleConstraintControl::Response"; - static constexpr const char* DOC_NAME = "Wheeled Vehicle Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4186,11 +4007,11 @@ void extract(Serializer& serializer, WheeledVehicleConstraintControl& self); void insert(Serializer& serializer, const WheeledVehicleConstraintControl::Response& self); void extract(Serializer& serializer, WheeledVehicleConstraintControl::Response& self); -TypedResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable); -TypedResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut); -TypedResult saveWheeledVehicleConstraintControl(C::mip_interface& device); -TypedResult loadWheeledVehicleConstraintControl(C::mip_interface& device); -TypedResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); +CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable); +CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut); +CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device); +CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device); +CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); ///@} /// @@ -4212,8 +4033,6 @@ struct VerticalGyroConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VerticalGyroConstraintControl"; - static constexpr const char* DOC_NAME = "Vertical Gyro Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -4246,8 +4065,6 @@ struct VerticalGyroConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VerticalGyroConstraintControl::Response"; - static constexpr const char* DOC_NAME = "Vertical Gyro Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4266,11 +4083,11 @@ void extract(Serializer& serializer, VerticalGyroConstraintControl& self); void insert(Serializer& serializer, const VerticalGyroConstraintControl::Response& self); void extract(Serializer& serializer, VerticalGyroConstraintControl::Response& self); -TypedResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable); -TypedResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut); -TypedResult saveVerticalGyroConstraintControl(C::mip_interface& device); -TypedResult loadVerticalGyroConstraintControl(C::mip_interface& device); -TypedResult defaultVerticalGyroConstraintControl(C::mip_interface& device); +CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable); +CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut); +CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device); +CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device); +CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device); ///@} /// @@ -4291,8 +4108,6 @@ struct GnssAntennaCalControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssAntennaCalControl"; - static constexpr const char* DOC_NAME = "GNSS Antenna Offset Calibration Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -4325,8 +4140,6 @@ struct GnssAntennaCalControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssAntennaCalControl::Response"; - static constexpr const char* DOC_NAME = "GNSS Antenna Offset Calibration Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4346,11 +4159,11 @@ void extract(Serializer& serializer, GnssAntennaCalControl& self); void insert(Serializer& serializer, const GnssAntennaCalControl::Response& self); void extract(Serializer& serializer, GnssAntennaCalControl::Response& self); -TypedResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset); -TypedResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut); -TypedResult saveGnssAntennaCalControl(C::mip_interface& device); -TypedResult loadGnssAntennaCalControl(C::mip_interface& device); -TypedResult defaultGnssAntennaCalControl(C::mip_interface& device); +CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset); +CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut); +CmdResult saveGnssAntennaCalControl(C::mip_interface& device); +CmdResult loadGnssAntennaCalControl(C::mip_interface& device); +CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); ///@} /// @@ -4370,8 +4183,6 @@ struct SetInitialHeading static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SetInitialHeading"; - static constexpr const char* DOC_NAME = "Set Initial Heading Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4390,7 +4201,7 @@ struct SetInitialHeading void insert(Serializer& serializer, const SetInitialHeading& self); void extract(Serializer& serializer, SetInitialHeading& self); -TypedResult setInitialHeading(C::mip_interface& device, float heading); +CmdResult setInitialHeading(C::mip_interface& device, float heading); ///@} /// diff --git a/src/mip/definitions/commands_gnss.cpp b/src/mip/definitions/commands_gnss.cpp index f980f2e9c..d099323d0 100644 --- a/src/mip/definitions/commands_gnss.cpp +++ b/src/mip/definitions/commands_gnss.cpp @@ -77,12 +77,12 @@ void extract(Serializer& serializer, ReceiverInfo::Info& self) } -TypedResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut) +CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LIST_RECEIVERS, NULL, 0, REPLY_LIST_RECEIVERS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LIST_RECEIVERS, NULL, 0, REPLY_LIST_RECEIVERS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -166,7 +166,7 @@ void extract(Serializer& serializer, SignalConfiguration::Response& self) } -TypedResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved) +CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -188,7 +188,7 @@ TypedResult writeSignalConfiguration(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut) +CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -197,7 +197,7 @@ TypedResult readSignalConfiguration(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SIGNAL_CONFIGURATION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SIGNAL_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -224,7 +224,7 @@ TypedResult readSignalConfiguration(C::mip_interface& devic } return result; } -TypedResult saveSignalConfiguration(C::mip_interface& device) +CmdResult saveSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -234,7 +234,7 @@ TypedResult saveSignalConfiguration(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadSignalConfiguration(C::mip_interface& device) +CmdResult loadSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -244,7 +244,7 @@ TypedResult loadSignalConfiguration(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultSignalConfiguration(C::mip_interface& device) +CmdResult defaultSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -298,7 +298,7 @@ void extract(Serializer& serializer, RtkDongleConfiguration::Response& self) } -TypedResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved) +CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -314,7 +314,7 @@ TypedResult writeRtkDongleConfiguration(C::mip_interface return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut) +CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -323,7 +323,7 @@ TypedResult readRtkDongleConfiguration(C::mip_interface& assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_RTK_DONGLE_CONFIGURATION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_RTK_DONGLE_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -341,7 +341,7 @@ TypedResult readRtkDongleConfiguration(C::mip_interface& } return result; } -TypedResult saveRtkDongleConfiguration(C::mip_interface& device) +CmdResult saveRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -351,7 +351,7 @@ TypedResult saveRtkDongleConfiguration(C::mip_interface& return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadRtkDongleConfiguration(C::mip_interface& device) +CmdResult loadRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -361,7 +361,7 @@ TypedResult loadRtkDongleConfiguration(C::mip_interface& return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultRtkDongleConfiguration(C::mip_interface& device) +CmdResult defaultRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index 7f3471d7d..d0b7cb4b4 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -78,8 +78,6 @@ struct ReceiverInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReceiverInfo"; - static constexpr const char* DOC_NAME = "ReceiverInfo"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -99,8 +97,6 @@ struct ReceiverInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReceiverInfo::Response"; - static constexpr const char* DOC_NAME = "ReceiverInfo Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -123,7 +119,7 @@ void extract(Serializer& serializer, ReceiverInfo::Info& self); void insert(Serializer& serializer, const ReceiverInfo::Response& self); void extract(Serializer& serializer, ReceiverInfo::Response& self); -TypedResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut); +CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut); ///@} /// @@ -146,8 +142,6 @@ struct SignalConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SignalConfiguration"; - static constexpr const char* DOC_NAME = "SignalConfiguration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -180,8 +174,6 @@ struct SignalConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SignalConfiguration::Response"; - static constexpr const char* DOC_NAME = "SignalConfiguration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -204,11 +196,11 @@ void extract(Serializer& serializer, SignalConfiguration& self); void insert(Serializer& serializer, const SignalConfiguration::Response& self); void extract(Serializer& serializer, SignalConfiguration::Response& self); -TypedResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved); -TypedResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut); -TypedResult saveSignalConfiguration(C::mip_interface& device); -TypedResult loadSignalConfiguration(C::mip_interface& device); -TypedResult defaultSignalConfiguration(C::mip_interface& device); +CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved); +CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut); +CmdResult saveSignalConfiguration(C::mip_interface& device); +CmdResult loadSignalConfiguration(C::mip_interface& device); +CmdResult defaultSignalConfiguration(C::mip_interface& device); ///@} /// @@ -228,8 +220,6 @@ struct RtkDongleConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RtkDongleConfiguration"; - static constexpr const char* DOC_NAME = "RtkDongleConfiguration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -262,8 +252,6 @@ struct RtkDongleConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RtkDongleConfiguration::Response"; - static constexpr const char* DOC_NAME = "RtkDongleConfiguration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -283,11 +271,11 @@ void extract(Serializer& serializer, RtkDongleConfiguration& self); void insert(Serializer& serializer, const RtkDongleConfiguration::Response& self); void extract(Serializer& serializer, RtkDongleConfiguration::Response& self); -TypedResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved); -TypedResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut); -TypedResult saveRtkDongleConfiguration(C::mip_interface& device); -TypedResult loadRtkDongleConfiguration(C::mip_interface& device); -TypedResult defaultRtkDongleConfiguration(C::mip_interface& device); +CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved); +CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut); +CmdResult saveRtkDongleConfiguration(C::mip_interface& device); +CmdResult loadRtkDongleConfiguration(C::mip_interface& device); +CmdResult defaultRtkDongleConfiguration(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_rtk.cpp b/src/mip/definitions/commands_rtk.cpp index 2ec4b6dd2..b2997856e 100644 --- a/src/mip/definitions/commands_rtk.cpp +++ b/src/mip/definitions/commands_rtk.cpp @@ -51,12 +51,12 @@ void extract(Serializer& serializer, GetStatusFlags::Response& self) } -TypedResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut) +CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_STATUS_FLAGS, NULL, 0, REPLY_GET_STATUS_FLAGS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_STATUS_FLAGS, NULL, 0, REPLY_GET_STATUS_FLAGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -94,12 +94,12 @@ void extract(Serializer& serializer, GetImei::Response& self) } -TypedResult getImei(C::mip_interface& device, char* imeiOut) +CmdResult getImei(C::mip_interface& device, char* imeiOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMEI, NULL, 0, REPLY_GET_IMEI, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMEI, NULL, 0, REPLY_GET_IMEI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -138,12 +138,12 @@ void extract(Serializer& serializer, GetImsi::Response& self) } -TypedResult getImsi(C::mip_interface& device, char* imsiOut) +CmdResult getImsi(C::mip_interface& device, char* imsiOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMSI, NULL, 0, REPLY_GET_IMSI, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMSI, NULL, 0, REPLY_GET_IMSI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -182,12 +182,12 @@ void extract(Serializer& serializer, GetIccid::Response& self) } -TypedResult getIccid(C::mip_interface& device, char* iccidOut) +CmdResult getIccid(C::mip_interface& device, char* iccidOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ICCID, NULL, 0, REPLY_GET_ICCID, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ICCID, NULL, 0, REPLY_GET_ICCID, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -234,7 +234,7 @@ void extract(Serializer& serializer, ConnectedDeviceType::Response& self) } -TypedResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype) +CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -246,7 +246,7 @@ TypedResult writeConnectedDeviceType(C::mip_interface& devi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut) +CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -255,7 +255,7 @@ TypedResult readConnectedDeviceType(C::mip_interface& devic assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONNECTED_DEVICE_TYPE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONNECTED_DEVICE_TYPE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -269,7 +269,7 @@ TypedResult readConnectedDeviceType(C::mip_interface& devic } return result; } -TypedResult saveConnectedDeviceType(C::mip_interface& device) +CmdResult saveConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -279,7 +279,7 @@ TypedResult saveConnectedDeviceType(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadConnectedDeviceType(C::mip_interface& device) +CmdResult loadConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -289,7 +289,7 @@ TypedResult loadConnectedDeviceType(C::mip_interface& devic return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultConnectedDeviceType(C::mip_interface& device) +CmdResult defaultConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -323,12 +323,12 @@ void extract(Serializer& serializer, GetActCode::Response& self) } -TypedResult getActCode(C::mip_interface& device, char* activationcodeOut) +CmdResult getActCode(C::mip_interface& device, char* activationcodeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ACT_CODE, NULL, 0, REPLY_GET_ACT_CODE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ACT_CODE, NULL, 0, REPLY_GET_ACT_CODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -367,12 +367,12 @@ void extract(Serializer& serializer, GetModemFirmwareVersion::Response& self) } -TypedResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut) +CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_MODEM_FIRMWARE_VERSION, NULL, 0, REPLY_GET_MODEM_FIRMWARE_VERSION, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_MODEM_FIRMWARE_VERSION, NULL, 0, REPLY_GET_MODEM_FIRMWARE_VERSION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -417,12 +417,12 @@ void extract(Serializer& serializer, GetRssi::Response& self) } -TypedResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut) +CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_RSSI, NULL, 0, REPLY_GET_RSSI, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_RSSI, NULL, 0, REPLY_GET_RSSI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -480,7 +480,7 @@ void extract(Serializer& serializer, ServiceStatus::Response& self) } -TypedResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) +CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -492,7 +492,7 @@ TypedResult serviceStatus(C::mip_interface& device, uint32_t rese assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SERVICE_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SERVICE_STATUS, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SERVICE_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SERVICE_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -526,7 +526,7 @@ void extract(Serializer& serializer, ProdEraseStorage& self) } -TypedResult prodEraseStorage(C::mip_interface& device, MediaSelector media) +CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -564,7 +564,7 @@ void extract(Serializer& serializer, LedControl& self) } -TypedResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period) +CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -596,7 +596,7 @@ void extract(Serializer& serializer, ModemHardReset& self) (void)self; } -TypedResult modemHardReset(C::mip_interface& device) +CmdResult modemHardReset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MODEM_HARD_RESET, NULL, 0); } diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 5ee697759..1b7e145a6 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -109,7 +109,7 @@ struct GetStatusFlags StatusFlagsLegacy(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } StatusFlagsLegacy& operator=(uint32_t val) { value = val; return *this; } - StatusFlagsLegacy& operator=(int val) { value = uint32_t(val); return *this; } + StatusFlagsLegacy& operator=(int val) { value = val; return *this; } StatusFlagsLegacy& operator|=(uint32_t val) { return *this = value | val; } StatusFlagsLegacy& operator&=(uint32_t val) { return *this = value & val; } @@ -165,7 +165,7 @@ struct GetStatusFlags StatusFlags(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } StatusFlags& operator=(uint32_t val) { value = val; return *this; } - StatusFlags& operator=(int val) { value = uint32_t(val); return *this; } + StatusFlags& operator=(int val) { value = val; return *this; } StatusFlags& operator|=(uint32_t val) { return *this = value | val; } StatusFlags& operator&=(uint32_t val) { return *this = value & val; } @@ -202,8 +202,6 @@ struct GetStatusFlags static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetStatusFlags"; - static constexpr const char* DOC_NAME = "Get RTK Device Status Flags"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -223,8 +221,6 @@ struct GetStatusFlags static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetStatusFlags::Response"; - static constexpr const char* DOC_NAME = "Get RTK Device Status Flags Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -243,7 +239,7 @@ void extract(Serializer& serializer, GetStatusFlags& self); void insert(Serializer& serializer, const GetStatusFlags::Response& self); void extract(Serializer& serializer, GetStatusFlags::Response& self); -TypedResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut); +CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut); ///@} /// @@ -258,8 +254,6 @@ struct GetImei static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetImei"; - static constexpr const char* DOC_NAME = "Get RTK Device IMEI (International Mobile Equipment Identifier)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -279,8 +273,6 @@ struct GetImei static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetImei::Response"; - static constexpr const char* DOC_NAME = "Get RTK Device IMEI (International Mobile Equipment Identifier) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -299,7 +291,7 @@ void extract(Serializer& serializer, GetImei& self); void insert(Serializer& serializer, const GetImei::Response& self); void extract(Serializer& serializer, GetImei::Response& self); -TypedResult getImei(C::mip_interface& device, char* imeiOut); +CmdResult getImei(C::mip_interface& device, char* imeiOut); ///@} /// @@ -314,8 +306,6 @@ struct GetImsi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetImsi"; - static constexpr const char* DOC_NAME = "Get RTK Device IMSI (International Mobile Subscriber Identifier)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -335,8 +325,6 @@ struct GetImsi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetImsi::Response"; - static constexpr const char* DOC_NAME = "Get RTK Device IMSI (International Mobile Subscriber Identifier) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -355,7 +343,7 @@ void extract(Serializer& serializer, GetImsi& self); void insert(Serializer& serializer, const GetImsi::Response& self); void extract(Serializer& serializer, GetImsi::Response& self); -TypedResult getImsi(C::mip_interface& device, char* imsiOut); +CmdResult getImsi(C::mip_interface& device, char* imsiOut); ///@} /// @@ -370,8 +358,6 @@ struct GetIccid static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetIccid"; - static constexpr const char* DOC_NAME = "Get RTK Device ICCID (Integrated Circuit Card Identification [SIM Number])"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -391,8 +377,6 @@ struct GetIccid static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetIccid::Response"; - static constexpr const char* DOC_NAME = "Get RTK Device ICCID (Integrated Circuit Card Identification [SIM Number]) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -411,7 +395,7 @@ void extract(Serializer& serializer, GetIccid& self); void insert(Serializer& serializer, const GetIccid::Response& self); void extract(Serializer& serializer, GetIccid::Response& self); -TypedResult getIccid(C::mip_interface& device, char* iccidOut); +CmdResult getIccid(C::mip_interface& device, char* iccidOut); ///@} /// @@ -434,8 +418,6 @@ struct ConnectedDeviceType static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ConnectedDeviceType"; - static constexpr const char* DOC_NAME = "Configure or read the type of the connected device"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -468,8 +450,6 @@ struct ConnectedDeviceType static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ConnectedDeviceType::Response"; - static constexpr const char* DOC_NAME = "Configure or read the type of the connected device Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -488,11 +468,11 @@ void extract(Serializer& serializer, ConnectedDeviceType& self); void insert(Serializer& serializer, const ConnectedDeviceType::Response& self); void extract(Serializer& serializer, ConnectedDeviceType::Response& self); -TypedResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype); -TypedResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut); -TypedResult saveConnectedDeviceType(C::mip_interface& device); -TypedResult loadConnectedDeviceType(C::mip_interface& device); -TypedResult defaultConnectedDeviceType(C::mip_interface& device); +CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype); +CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut); +CmdResult saveConnectedDeviceType(C::mip_interface& device); +CmdResult loadConnectedDeviceType(C::mip_interface& device); +CmdResult defaultConnectedDeviceType(C::mip_interface& device); ///@} /// @@ -507,8 +487,6 @@ struct GetActCode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetActCode"; - static constexpr const char* DOC_NAME = "Get RTK Device Activation Code"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -528,8 +506,6 @@ struct GetActCode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetActCode::Response"; - static constexpr const char* DOC_NAME = "Get RTK Device Activation Code Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -548,7 +524,7 @@ void extract(Serializer& serializer, GetActCode& self); void insert(Serializer& serializer, const GetActCode::Response& self); void extract(Serializer& serializer, GetActCode::Response& self); -TypedResult getActCode(C::mip_interface& device, char* activationcodeOut); +CmdResult getActCode(C::mip_interface& device, char* activationcodeOut); ///@} /// @@ -563,8 +539,6 @@ struct GetModemFirmwareVersion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetModemFirmwareVersion"; - static constexpr const char* DOC_NAME = "Get RTK Device's Cell Modem Firmware version number"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -584,8 +558,6 @@ struct GetModemFirmwareVersion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetModemFirmwareVersion::Response"; - static constexpr const char* DOC_NAME = "Get RTK Device's Cell Modem Firmware version number Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -604,7 +576,7 @@ void extract(Serializer& serializer, GetModemFirmwareVersion& self); void insert(Serializer& serializer, const GetModemFirmwareVersion::Response& self); void extract(Serializer& serializer, GetModemFirmwareVersion::Response& self); -TypedResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut); +CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut); ///@} /// @@ -620,8 +592,6 @@ struct GetRssi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetRssi"; - static constexpr const char* DOC_NAME = "GetRssi"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -641,8 +611,6 @@ struct GetRssi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GetRssi::Response"; - static constexpr const char* DOC_NAME = "GetRssi Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -663,7 +631,7 @@ void extract(Serializer& serializer, GetRssi& self); void insert(Serializer& serializer, const GetRssi::Response& self); void extract(Serializer& serializer, GetRssi::Response& self); -TypedResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut); +CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut); ///@} /// @@ -691,7 +659,7 @@ struct ServiceStatus ServiceFlags(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } ServiceFlags& operator=(uint8_t val) { value = val; return *this; } - ServiceFlags& operator=(int val) { value = uint8_t(val); return *this; } + ServiceFlags& operator=(int val) { value = val; return *this; } ServiceFlags& operator|=(uint8_t val) { return *this = value | val; } ServiceFlags& operator&=(uint8_t val) { return *this = value & val; } @@ -712,8 +680,6 @@ struct ServiceStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ServiceStatus"; - static constexpr const char* DOC_NAME = "ServiceStatus"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -733,8 +699,6 @@ struct ServiceStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ServiceStatus::Response"; - static constexpr const char* DOC_NAME = "ServiceStatus Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -756,7 +720,7 @@ void extract(Serializer& serializer, ServiceStatus& self); void insert(Serializer& serializer, const ServiceStatus::Response& self); void extract(Serializer& serializer, ServiceStatus::Response& self); -TypedResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); +CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); ///@} /// @@ -774,8 +738,6 @@ struct ProdEraseStorage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ProdEraseStorage"; - static constexpr const char* DOC_NAME = "ProdEraseStorage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -794,7 +756,7 @@ struct ProdEraseStorage void insert(Serializer& serializer, const ProdEraseStorage& self); void extract(Serializer& serializer, ProdEraseStorage& self); -TypedResult prodEraseStorage(C::mip_interface& device, MediaSelector media); +CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media); ///@} /// @@ -814,8 +776,6 @@ struct LedControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "LedControl"; - static constexpr const char* DOC_NAME = "LedControl"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -834,7 +794,7 @@ struct LedControl void insert(Serializer& serializer, const LedControl& self); void extract(Serializer& serializer, LedControl& self); -TypedResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period); +CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period); ///@} /// @@ -851,8 +811,6 @@ struct ModemHardReset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ModemHardReset"; - static constexpr const char* DOC_NAME = "ModemHardReset"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -871,7 +829,7 @@ struct ModemHardReset void insert(Serializer& serializer, const ModemHardReset& self); void extract(Serializer& serializer, ModemHardReset& self); -TypedResult modemHardReset(C::mip_interface& device); +CmdResult modemHardReset(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_system.cpp b/src/mip/definitions/commands_system.cpp index 748a9c184..749941ce0 100644 --- a/src/mip/definitions/commands_system.cpp +++ b/src/mip/definitions/commands_system.cpp @@ -61,7 +61,7 @@ void extract(Serializer& serializer, CommMode::Response& self) } -TypedResult writeCommMode(C::mip_interface& device, uint8_t mode) +CmdResult writeCommMode(C::mip_interface& device, uint8_t mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -73,7 +73,7 @@ TypedResult writeCommMode(C::mip_interface& device, uint8_t mode) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut) +CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -82,7 +82,7 @@ TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COM_MODE, buffer, &responseLength); + CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COM_MODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -96,7 +96,7 @@ TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut) } return result; } -TypedResult defaultCommMode(C::mip_interface& device) +CmdResult defaultCommMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index 90b1e88fa..dd49bc142 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -77,8 +77,6 @@ struct CommMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CommMode"; - static constexpr const char* DOC_NAME = "CommMode"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -111,8 +109,6 @@ struct CommMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CommMode::Response"; - static constexpr const char* DOC_NAME = "CommMode Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -131,9 +127,9 @@ void extract(Serializer& serializer, CommMode& self); void insert(Serializer& serializer, const CommMode::Response& self); void extract(Serializer& serializer, CommMode::Response& self); -TypedResult writeCommMode(C::mip_interface& device, uint8_t mode); -TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut); -TypedResult defaultCommMode(C::mip_interface& device); +CmdResult writeCommMode(C::mip_interface& device, uint8_t mode); +CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut); +CmdResult defaultCommMode(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 5b2b04fc5..a6926e3c5 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -162,7 +162,7 @@ struct FilterStatusFlags : Bitfield FilterStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } FilterStatusFlags& operator=(uint16_t val) { value = val; return *this; } - FilterStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } + FilterStatusFlags& operator=(int val) { value = val; return *this; } FilterStatusFlags& operator|=(uint16_t val) { return *this = value | val; } FilterStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -262,7 +262,7 @@ struct FilterMeasurementIndicator : Bitfield FilterMeasurementIndicator(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } FilterMeasurementIndicator& operator=(uint8_t val) { value = val; return *this; } - FilterMeasurementIndicator& operator=(int val) { value = uint8_t(val); return *this; } + FilterMeasurementIndicator& operator=(int val) { value = val; return *this; } FilterMeasurementIndicator& operator|=(uint8_t val) { return *this = value | val; } FilterMeasurementIndicator& operator&=(uint8_t val) { return *this = value & val; } @@ -312,7 +312,7 @@ struct GnssAidStatusFlags : Bitfield GnssAidStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } GnssAidStatusFlags& operator=(uint16_t val) { value = val; return *this; } - GnssAidStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } + GnssAidStatusFlags& operator=(int val) { value = val; return *this; } GnssAidStatusFlags& operator|=(uint16_t val) { return *this = value | val; } GnssAidStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -374,8 +374,6 @@ struct PositionLlh static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PositionLlh"; - static constexpr const char* DOC_NAME = "LLH Position"; auto as_tuple() const @@ -410,8 +408,6 @@ struct VelocityNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VelocityNed"; - static constexpr const char* DOC_NAME = "VelocityNed"; auto as_tuple() const @@ -452,8 +448,6 @@ struct AttitudeQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AttitudeQuaternion"; - static constexpr const char* DOC_NAME = "AttitudeQuaternion"; auto as_tuple() const @@ -496,8 +490,6 @@ struct AttitudeDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AttitudeDcm"; - static constexpr const char* DOC_NAME = "AttitudeDcm"; auto as_tuple() const @@ -533,8 +525,6 @@ struct EulerAngles static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EulerAngles"; - static constexpr const char* DOC_NAME = "EulerAngles"; auto as_tuple() const @@ -567,8 +557,6 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroBias"; - static constexpr const char* DOC_NAME = "GyroBias"; auto as_tuple() const @@ -601,8 +589,6 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelBias"; - static constexpr const char* DOC_NAME = "AccelBias"; auto as_tuple() const @@ -637,8 +623,6 @@ struct PositionLlhUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PositionLlhUncertainty"; - static constexpr const char* DOC_NAME = "LLH Position Uncertainty"; auto as_tuple() const @@ -673,8 +657,6 @@ struct VelocityNedUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VelocityNedUncertainty"; - static constexpr const char* DOC_NAME = "NED Velocity Uncertainty"; auto as_tuple() const @@ -710,8 +692,6 @@ struct EulerAnglesUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EulerAnglesUncertainty"; - static constexpr const char* DOC_NAME = "EulerAnglesUncertainty"; auto as_tuple() const @@ -744,8 +724,6 @@ struct GyroBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroBiasUncertainty"; - static constexpr const char* DOC_NAME = "GyroBiasUncertainty"; auto as_tuple() const @@ -778,8 +756,6 @@ struct AccelBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelBiasUncertainty"; - static constexpr const char* DOC_NAME = "AccelBiasUncertainty"; auto as_tuple() const @@ -819,8 +795,6 @@ struct Timestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Timestamp"; - static constexpr const char* DOC_NAME = "Timestamp"; auto as_tuple() const @@ -854,8 +828,6 @@ struct Status static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Status"; - static constexpr const char* DOC_NAME = "Status"; auto as_tuple() const @@ -889,8 +861,6 @@ struct LinearAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "LinearAccel"; - static constexpr const char* DOC_NAME = "LinearAccel"; auto as_tuple() const @@ -923,8 +893,6 @@ struct GravityVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GravityVector"; - static constexpr const char* DOC_NAME = "GravityVector"; auto as_tuple() const @@ -957,8 +925,6 @@ struct CompAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CompAccel"; - static constexpr const char* DOC_NAME = "Compensated Acceleration"; auto as_tuple() const @@ -991,8 +957,6 @@ struct CompAngularRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CompAngularRate"; - static constexpr const char* DOC_NAME = "CompAngularRate"; auto as_tuple() const @@ -1025,8 +989,6 @@ struct QuaternionAttitudeUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "QuaternionAttitudeUncertainty"; - static constexpr const char* DOC_NAME = "QuaternionAttitudeUncertainty"; auto as_tuple() const @@ -1059,8 +1021,6 @@ struct Wgs84GravityMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Wgs84GravityMag"; - static constexpr const char* DOC_NAME = "Wgs84GravityMag"; auto as_tuple() const @@ -1107,8 +1067,6 @@ struct HeadingUpdateState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "HeadingUpdateState"; - static constexpr const char* DOC_NAME = "HeadingUpdateState"; auto as_tuple() const @@ -1146,8 +1104,6 @@ struct MagneticModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagneticModel"; - static constexpr const char* DOC_NAME = "MagneticModel"; auto as_tuple() const @@ -1180,8 +1136,6 @@ struct AccelScaleFactor static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelScaleFactor"; - static constexpr const char* DOC_NAME = "AccelScaleFactor"; auto as_tuple() const @@ -1214,8 +1168,6 @@ struct AccelScaleFactorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AccelScaleFactorUncertainty"; - static constexpr const char* DOC_NAME = "AccelScaleFactorUncertainty"; auto as_tuple() const @@ -1248,8 +1200,6 @@ struct GyroScaleFactor static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroScaleFactor"; - static constexpr const char* DOC_NAME = "GyroScaleFactor"; auto as_tuple() const @@ -1282,8 +1232,6 @@ struct GyroScaleFactorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GyroScaleFactorUncertainty"; - static constexpr const char* DOC_NAME = "GyroScaleFactorUncertainty"; auto as_tuple() const @@ -1316,8 +1264,6 @@ struct MagBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagBias"; - static constexpr const char* DOC_NAME = "MagBias"; auto as_tuple() const @@ -1350,8 +1296,6 @@ struct MagBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagBiasUncertainty"; - static constexpr const char* DOC_NAME = "MagBiasUncertainty"; auto as_tuple() const @@ -1390,8 +1334,6 @@ struct StandardAtmosphere static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "StandardAtmosphere"; - static constexpr const char* DOC_NAME = "StandardAtmosphere"; auto as_tuple() const @@ -1428,8 +1370,6 @@ struct PressureAltitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PressureAltitude"; - static constexpr const char* DOC_NAME = "PressureAltitude"; auto as_tuple() const @@ -1461,8 +1401,6 @@ struct DensityAltitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DensityAltitude"; - static constexpr const char* DOC_NAME = "DensityAltitude"; auto as_tuple() const @@ -1497,8 +1435,6 @@ struct AntennaOffsetCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AntennaOffsetCorrection"; - static constexpr const char* DOC_NAME = "AntennaOffsetCorrection"; auto as_tuple() const @@ -1531,8 +1467,6 @@ struct AntennaOffsetCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AntennaOffsetCorrectionUncertainty"; - static constexpr const char* DOC_NAME = "AntennaOffsetCorrectionUncertainty"; auto as_tuple() const @@ -1568,8 +1502,6 @@ struct MultiAntennaOffsetCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MultiAntennaOffsetCorrection"; - static constexpr const char* DOC_NAME = "MultiAntennaOffsetCorrection"; auto as_tuple() const @@ -1603,8 +1535,6 @@ struct MultiAntennaOffsetCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MultiAntennaOffsetCorrectionUncertainty"; - static constexpr const char* DOC_NAME = "MultiAntennaOffsetCorrectionUncertainty"; auto as_tuple() const @@ -1639,8 +1569,6 @@ struct MagnetometerOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagnetometerOffset"; - static constexpr const char* DOC_NAME = "MagnetometerOffset"; auto as_tuple() const @@ -1675,8 +1603,6 @@ struct MagnetometerMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagnetometerMatrix"; - static constexpr const char* DOC_NAME = "MagnetometerMatrix"; auto as_tuple() const @@ -1709,8 +1635,6 @@ struct MagnetometerOffsetUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagnetometerOffsetUncertainty"; - static constexpr const char* DOC_NAME = "MagnetometerOffsetUncertainty"; auto as_tuple() const @@ -1743,8 +1667,6 @@ struct MagnetometerMatrixUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagnetometerMatrixUncertainty"; - static constexpr const char* DOC_NAME = "MagnetometerMatrixUncertainty"; auto as_tuple() const @@ -1776,8 +1698,6 @@ struct MagnetometerCovarianceMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagnetometerCovarianceMatrix"; - static constexpr const char* DOC_NAME = "MagnetometerCovarianceMatrix"; auto as_tuple() const @@ -1810,8 +1730,6 @@ struct MagnetometerResidualVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "MagnetometerResidualVector"; - static constexpr const char* DOC_NAME = "MagnetometerResidualVector"; auto as_tuple() const @@ -1846,8 +1764,6 @@ struct ClockCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ClockCorrection"; - static constexpr const char* DOC_NAME = "ClockCorrection"; auto as_tuple() const @@ -1882,8 +1798,6 @@ struct ClockCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ClockCorrectionUncertainty"; - static constexpr const char* DOC_NAME = "ClockCorrectionUncertainty"; auto as_tuple() const @@ -1918,8 +1832,6 @@ struct GnssPosAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssPosAidStatus"; - static constexpr const char* DOC_NAME = "GNSS Position Aiding Status"; auto as_tuple() const @@ -1953,8 +1865,6 @@ struct GnssAttAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssAttAidStatus"; - static constexpr const char* DOC_NAME = "GNSS Attitude Aiding Status"; auto as_tuple() const @@ -1994,8 +1904,6 @@ struct HeadAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "HeadAidStatus"; - static constexpr const char* DOC_NAME = "HeadAidStatus"; auto as_tuple() const @@ -2028,8 +1936,6 @@ struct RelPosNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RelPosNed"; - static constexpr const char* DOC_NAME = "NED Relative Position"; auto as_tuple() const @@ -2062,8 +1968,6 @@ struct EcefPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefPos"; - static constexpr const char* DOC_NAME = "ECEF Position"; auto as_tuple() const @@ -2096,8 +2000,6 @@ struct EcefVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefVel"; - static constexpr const char* DOC_NAME = "ECEF Velocity"; auto as_tuple() const @@ -2130,8 +2032,6 @@ struct EcefPosUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefPosUncertainty"; - static constexpr const char* DOC_NAME = "ECEF Position Uncertainty"; auto as_tuple() const @@ -2164,8 +2064,6 @@ struct EcefVelUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefVelUncertainty"; - static constexpr const char* DOC_NAME = "ECEF Velocity Uncertainty"; auto as_tuple() const @@ -2200,8 +2098,6 @@ struct AidingMeasurementSummary static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingMeasurementSummary"; - static constexpr const char* DOC_NAME = "AidingMeasurementSummary"; auto as_tuple() const @@ -2234,8 +2130,6 @@ struct OdometerScaleFactorError static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "OdometerScaleFactorError"; - static constexpr const char* DOC_NAME = "Odometer Scale Factor Error"; auto as_tuple() const @@ -2268,8 +2162,6 @@ struct OdometerScaleFactorErrorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "OdometerScaleFactorErrorUncertainty"; - static constexpr const char* DOC_NAME = "Odometer Scale Factor Error Uncertainty"; auto as_tuple() const @@ -2319,7 +2211,7 @@ struct GnssDualAntennaStatus DualAntennaStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } DualAntennaStatusFlags& operator=(uint16_t val) { value = val; return *this; } - DualAntennaStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } + DualAntennaStatusFlags& operator=(int val) { value = val; return *this; } DualAntennaStatusFlags& operator|=(uint16_t val) { return *this = value | val; } DualAntennaStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2344,8 +2236,6 @@ struct GnssDualAntennaStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GnssDualAntennaStatus"; - static constexpr const char* DOC_NAME = "GNSS Dual Antenna Status"; auto as_tuple() const @@ -2381,8 +2271,6 @@ struct AidingFrameConfigError static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingFrameConfigError"; - static constexpr const char* DOC_NAME = "Aiding Frame Configuration Error"; auto as_tuple() const @@ -2418,8 +2306,6 @@ struct AidingFrameConfigErrorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingFrameConfigErrorUncertainty"; - static constexpr const char* DOC_NAME = "Aiding Frame Configuration Error Uncertainty"; auto as_tuple() const diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 5a59b0704..bab43b3bd 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -195,7 +195,7 @@ struct PosLlh ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -227,8 +227,6 @@ struct PosLlh static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PosLlh"; - static constexpr const char* DOC_NAME = "GNSS LLH Position"; auto as_tuple() const @@ -271,7 +269,7 @@ struct PosEcef ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -293,8 +291,6 @@ struct PosEcef static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PosEcef"; - static constexpr const char* DOC_NAME = "GNSS ECEF Position"; auto as_tuple() const @@ -341,7 +337,7 @@ struct VelNed ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -375,8 +371,6 @@ struct VelNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VelNed"; - static constexpr const char* DOC_NAME = "NED Velocity"; auto as_tuple() const @@ -419,7 +413,7 @@ struct VelEcef ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -441,8 +435,6 @@ struct VelEcef static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VelEcef"; - static constexpr const char* DOC_NAME = "GNSS ECEF Velocity"; auto as_tuple() const @@ -490,7 +482,7 @@ struct Dop ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -527,8 +519,6 @@ struct Dop static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Dop"; - static constexpr const char* DOC_NAME = "Dop"; auto as_tuple() const @@ -571,7 +561,7 @@ struct UtcTime ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -598,8 +588,6 @@ struct UtcTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "UtcTime"; - static constexpr const char* DOC_NAME = "UtcTime"; auto as_tuple() const @@ -642,7 +630,7 @@ struct GpsTime ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -664,8 +652,6 @@ struct GpsTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsTime"; - static constexpr const char* DOC_NAME = "GpsTime"; auto as_tuple() const @@ -709,7 +695,7 @@ struct ClockInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -734,8 +720,6 @@ struct ClockInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ClockInfo"; - static constexpr const char* DOC_NAME = "ClockInfo"; auto as_tuple() const @@ -788,7 +772,7 @@ struct FixInfo FixFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } FixFlags& operator=(uint16_t val) { value = val; return *this; } - FixFlags& operator=(int val) { value = uint16_t(val); return *this; } + FixFlags& operator=(int val) { value = val; return *this; } FixFlags& operator|=(uint16_t val) { return *this = value | val; } FixFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -818,7 +802,7 @@ struct FixInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -843,8 +827,6 @@ struct FixInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "FixInfo"; - static constexpr const char* DOC_NAME = "FixInfo"; auto as_tuple() const @@ -888,7 +870,7 @@ struct SvInfo SVFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } SVFlags& operator=(uint16_t val) { value = val; return *this; } - SVFlags& operator=(int val) { value = uint16_t(val); return *this; } + SVFlags& operator=(int val) { value = val; return *this; } SVFlags& operator|=(uint16_t val) { return *this = value | val; } SVFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -921,7 +903,7 @@ struct SvInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -955,8 +937,6 @@ struct SvInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SvInfo"; - static constexpr const char* DOC_NAME = "SvInfo"; auto as_tuple() const @@ -1023,7 +1003,7 @@ struct HwStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1048,8 +1028,6 @@ struct HwStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "HwStatus"; - static constexpr const char* DOC_NAME = "GNSS Hardware Status"; auto as_tuple() const @@ -1106,7 +1084,7 @@ struct DgpsInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1134,8 +1112,6 @@ struct DgpsInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DgpsInfo"; - static constexpr const char* DOC_NAME = "DgpsInfo"; auto as_tuple() const @@ -1182,7 +1158,7 @@ struct DgpsChannel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1210,8 +1186,6 @@ struct DgpsChannel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DgpsChannel"; - static constexpr const char* DOC_NAME = "DgpsChannel"; auto as_tuple() const @@ -1258,7 +1232,7 @@ struct ClockInfo2 ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1286,8 +1260,6 @@ struct ClockInfo2 static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ClockInfo2"; - static constexpr const char* DOC_NAME = "ClockInfo2"; auto as_tuple() const @@ -1328,7 +1300,7 @@ struct GpsLeapSeconds ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1345,8 +1317,6 @@ struct GpsLeapSeconds static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsLeapSeconds"; - static constexpr const char* DOC_NAME = "GpsLeapSeconds"; auto as_tuple() const @@ -1390,7 +1360,7 @@ struct SbasInfo SbasStatus(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } SbasStatus& operator=(uint8_t val) { value = val; return *this; } - SbasStatus& operator=(int val) { value = uint8_t(val); return *this; } + SbasStatus& operator=(int val) { value = val; return *this; } SbasStatus& operator|=(uint8_t val) { return *this = value | val; } SbasStatus& operator&=(uint8_t val) { return *this = value & val; } @@ -1427,7 +1397,7 @@ struct SbasInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1461,8 +1431,6 @@ struct SbasInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SbasInfo"; - static constexpr const char* DOC_NAME = "SbasInfo"; auto as_tuple() const @@ -1528,7 +1496,7 @@ struct SbasCorrection ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1559,8 +1527,6 @@ struct SbasCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SbasCorrection"; - static constexpr const char* DOC_NAME = "SbasCorrection"; auto as_tuple() const @@ -1628,7 +1594,7 @@ struct RfErrorDetection ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1654,8 +1620,6 @@ struct RfErrorDetection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RfErrorDetection"; - static constexpr const char* DOC_NAME = "RfErrorDetection"; auto as_tuple() const @@ -1706,7 +1670,7 @@ struct BaseStationInfo IndicatorFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } IndicatorFlags& operator=(uint16_t val) { value = val; return *this; } - IndicatorFlags& operator=(int val) { value = uint16_t(val); return *this; } + IndicatorFlags& operator=(int val) { value = val; return *this; } IndicatorFlags& operator|=(uint16_t val) { return *this = value | val; } IndicatorFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1753,7 +1717,7 @@ struct BaseStationInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1787,8 +1751,6 @@ struct BaseStationInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "BaseStationInfo"; - static constexpr const char* DOC_NAME = "BaseStationInfo"; auto as_tuple() const @@ -1836,7 +1798,7 @@ struct RtkCorrectionsStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1885,7 +1847,7 @@ struct RtkCorrectionsStatus EpochStatus(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } EpochStatus& operator=(uint16_t val) { value = val; return *this; } - EpochStatus& operator=(int val) { value = uint16_t(val); return *this; } + EpochStatus& operator=(int val) { value = val; return *this; } EpochStatus& operator|=(uint16_t val) { return *this = value | val; } EpochStatus& operator&=(uint16_t val) { return *this = value & val; } @@ -1926,8 +1888,6 @@ struct RtkCorrectionsStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RtkCorrectionsStatus"; - static constexpr const char* DOC_NAME = "RtkCorrectionsStatus"; auto as_tuple() const @@ -1975,7 +1935,7 @@ struct SatelliteStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2014,8 +1974,6 @@ struct SatelliteStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SatelliteStatus"; - static constexpr const char* DOC_NAME = "SatelliteStatus"; auto as_tuple() const @@ -2082,7 +2040,7 @@ struct Raw ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2148,8 +2106,6 @@ struct Raw static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Raw"; - static constexpr const char* DOC_NAME = "Raw"; auto as_tuple() const @@ -2192,7 +2148,7 @@ struct GpsEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2245,8 +2201,6 @@ struct GpsEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsEphemeris"; - static constexpr const char* DOC_NAME = "GpsEphemeris"; auto as_tuple() const @@ -2289,7 +2243,7 @@ struct GalileoEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2342,8 +2296,6 @@ struct GalileoEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GalileoEphemeris"; - static constexpr const char* DOC_NAME = "GalileoEphemeris"; auto as_tuple() const @@ -2385,7 +2337,7 @@ struct GloEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2427,8 +2379,6 @@ struct GloEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GloEphemeris"; - static constexpr const char* DOC_NAME = "Glonass Ephemeris"; auto as_tuple() const @@ -2473,7 +2423,7 @@ struct GpsIonoCorr ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2501,8 +2451,6 @@ struct GpsIonoCorr static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsIonoCorr"; - static constexpr const char* DOC_NAME = "GPS Ionospheric Correction"; auto as_tuple() const @@ -2547,7 +2495,7 @@ struct GalileoIonoCorr ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2575,8 +2523,6 @@ struct GalileoIonoCorr static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GalileoIonoCorr"; - static constexpr const char* DOC_NAME = "Galileo Ionospheric Correction"; auto as_tuple() const diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index dde268718..be97a142e 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -84,8 +84,6 @@ struct RawAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RawAccel"; - static constexpr const char* DOC_NAME = "RawAccel"; auto as_tuple() const @@ -118,8 +116,6 @@ struct RawGyro static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RawGyro"; - static constexpr const char* DOC_NAME = "RawGyro"; auto as_tuple() const @@ -152,8 +148,6 @@ struct RawMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RawMag"; - static constexpr const char* DOC_NAME = "RawMag"; auto as_tuple() const @@ -186,8 +180,6 @@ struct RawPressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "RawPressure"; - static constexpr const char* DOC_NAME = "RawPressure"; auto as_tuple() const @@ -220,8 +212,6 @@ struct ScaledAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ScaledAccel"; - static constexpr const char* DOC_NAME = "ScaledAccel"; auto as_tuple() const @@ -254,8 +244,6 @@ struct ScaledGyro static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ScaledGyro"; - static constexpr const char* DOC_NAME = "ScaledGyro"; auto as_tuple() const @@ -288,8 +276,6 @@ struct ScaledMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ScaledMag"; - static constexpr const char* DOC_NAME = "ScaledMag"; auto as_tuple() const @@ -321,8 +307,6 @@ struct ScaledPressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ScaledPressure"; - static constexpr const char* DOC_NAME = "ScaledPressure"; auto as_tuple() const @@ -355,8 +339,6 @@ struct DeltaTheta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DeltaTheta"; - static constexpr const char* DOC_NAME = "DeltaTheta"; auto as_tuple() const @@ -389,8 +371,6 @@ struct DeltaVelocity static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DeltaVelocity"; - static constexpr const char* DOC_NAME = "DeltaVelocity"; auto as_tuple() const @@ -432,8 +412,6 @@ struct CompOrientationMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CompOrientationMatrix"; - static constexpr const char* DOC_NAME = "Complementary Filter Orientation Matrix"; auto as_tuple() const @@ -473,8 +451,6 @@ struct CompQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CompQuaternion"; - static constexpr const char* DOC_NAME = "Complementary Filter Quaternion"; auto as_tuple() const @@ -509,8 +485,6 @@ struct CompEulerAngles static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CompEulerAngles"; - static constexpr const char* DOC_NAME = "Complementary Filter Euler Angles"; auto as_tuple() const @@ -542,8 +516,6 @@ struct CompOrientationUpdateMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "CompOrientationUpdateMatrix"; - static constexpr const char* DOC_NAME = "Complementary Filter Orientation Update Matrix"; auto as_tuple() const @@ -575,8 +547,6 @@ struct OrientationRawTemp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "OrientationRawTemp"; - static constexpr const char* DOC_NAME = "OrientationRawTemp"; auto as_tuple() const @@ -608,8 +578,6 @@ struct InternalTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "InternalTimestamp"; - static constexpr const char* DOC_NAME = "InternalTimestamp"; auto as_tuple() const @@ -642,8 +610,6 @@ struct PpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "PpsTimestamp"; - static constexpr const char* DOC_NAME = "PPS Timestamp"; auto as_tuple() const @@ -694,7 +660,7 @@ struct GpsTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -720,8 +686,6 @@ struct GpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsTimestamp"; - static constexpr const char* DOC_NAME = "GpsTimestamp"; auto as_tuple() const @@ -759,8 +723,6 @@ struct TemperatureAbs static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "TemperatureAbs"; - static constexpr const char* DOC_NAME = "Temperature Statistics"; auto as_tuple() const @@ -798,8 +760,6 @@ struct UpVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "UpVector"; - static constexpr const char* DOC_NAME = "UpVector"; auto as_tuple() const @@ -834,8 +794,6 @@ struct NorthVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "NorthVector"; - static constexpr const char* DOC_NAME = "NorthVector"; auto as_tuple() const @@ -884,7 +842,7 @@ struct OverrangeStatus Status(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } Status& operator=(uint16_t val) { value = val; return *this; } - Status& operator=(int val) { value = uint16_t(val); return *this; } + Status& operator=(int val) { value = val; return *this; } Status& operator|=(uint16_t val) { return *this = value | val; } Status& operator&=(uint16_t val) { return *this = value & val; } @@ -918,8 +876,6 @@ struct OverrangeStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "OverrangeStatus"; - static constexpr const char* DOC_NAME = "OverrangeStatus"; auto as_tuple() const @@ -952,8 +908,6 @@ struct OdometerData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "OdometerData"; - static constexpr const char* DOC_NAME = "OdometerData"; auto as_tuple() const diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index 05df1eddf..be95ce3fa 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -71,8 +71,6 @@ struct EventSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EventSource"; - static constexpr const char* DOC_NAME = "EventSource"; auto as_tuple() const @@ -107,8 +105,6 @@ struct Ticks static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Ticks"; - static constexpr const char* DOC_NAME = "Ticks"; auto as_tuple() const @@ -144,8 +140,6 @@ struct DeltaTicks static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DeltaTicks"; - static constexpr const char* DOC_NAME = "DeltaTicks"; auto as_tuple() const @@ -191,7 +185,7 @@ struct GpsTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -213,8 +207,6 @@ struct GpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpsTimestamp"; - static constexpr const char* DOC_NAME = "GpsTimestamp"; auto as_tuple() const @@ -255,8 +247,6 @@ struct DeltaTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "DeltaTime"; - static constexpr const char* DOC_NAME = "DeltaTime"; auto as_tuple() const @@ -295,8 +285,6 @@ struct ReferenceTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferenceTimestamp"; - static constexpr const char* DOC_NAME = "ReferenceTimestamp"; auto as_tuple() const @@ -337,8 +325,6 @@ struct ReferenceTimeDelta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferenceTimeDelta"; - static constexpr const char* DOC_NAME = "ReferenceTimeDelta"; auto as_tuple() const @@ -387,7 +373,7 @@ struct ExternalTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -404,8 +390,6 @@ struct ExternalTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ExternalTimestamp"; - static constexpr const char* DOC_NAME = "ExternalTimestamp"; auto as_tuple() const @@ -458,7 +442,7 @@ struct ExternalTimeDelta ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } + ValidFlags& operator=(int val) { value = val; return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -475,8 +459,6 @@ struct ExternalTimeDelta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ExternalTimeDelta"; - static constexpr const char* DOC_NAME = "ExternalTimeDelta"; auto as_tuple() const diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 17170999f..80b457773 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -79,8 +79,6 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "BuiltInTest"; - static constexpr const char* DOC_NAME = "BuiltInTest"; auto as_tuple() const @@ -113,8 +111,6 @@ struct TimeSyncStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "TimeSyncStatus"; - static constexpr const char* DOC_NAME = "TimeSyncStatus"; auto as_tuple() const @@ -164,8 +160,6 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpioState"; - static constexpr const char* DOC_NAME = "GpioState"; auto as_tuple() const @@ -199,8 +193,6 @@ struct GpioAnalogValue static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "GpioAnalogValue"; - static constexpr const char* DOC_NAME = "GpioAnalogValue"; auto as_tuple() const From b391004a5b7f2be10b583295d25833dffdab1fe5 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 15 Feb 2024 21:29:46 +0000 Subject: [PATCH 205/252] Generate MIP definitions from branches/dev@56014. --- src/mip/definitions/commands_3dm.cpp | 402 +++++++-------- src/mip/definitions/commands_3dm.hpp | 502 +++++++++++++------ src/mip/definitions/commands_aiding.cpp | 54 +- src/mip/definitions/commands_aiding.hpp | 90 ++-- src/mip/definitions/commands_base.cpp | 42 +- src/mip/definitions/commands_base.hpp | 66 ++- src/mip/definitions/commands_filter.cpp | 528 +++++++++---------- src/mip/definitions/commands_filter.hpp | 640 +++++++++++++++--------- src/mip/definitions/commands_gnss.cpp | 28 +- src/mip/definitions/commands_gnss.hpp | 34 +- src/mip/definitions/commands_rtk.cpp | 50 +- src/mip/definitions/commands_rtk.hpp | 80 ++- src/mip/definitions/commands_system.cpp | 8 +- src/mip/definitions/commands_system.hpp | 10 +- src/mip/definitions/data_filter.hpp | 122 ++++- src/mip/definitions/data_gnss.hpp | 118 +++-- src/mip/definitions/data_sensor.hpp | 50 +- src/mip/definitions/data_shared.hpp | 24 +- src/mip/definitions/data_system.hpp | 8 + 19 files changed, 1789 insertions(+), 1067 deletions(-) diff --git a/src/mip/definitions/commands_3dm.cpp b/src/mip/definitions/commands_3dm.cpp index 563119dcc..477119d41 100644 --- a/src/mip/definitions/commands_3dm.cpp +++ b/src/mip/definitions/commands_3dm.cpp @@ -72,7 +72,7 @@ void extract(Serializer& serializer, PollImuMessage& self) } -CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -109,7 +109,7 @@ void extract(Serializer& serializer, PollGnssMessage& self) } -CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -146,7 +146,7 @@ void extract(Serializer& serializer, PollFilterMessage& self) } -CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -205,7 +205,7 @@ void extract(Serializer& serializer, ImuMessageFormat::Response& self) } -CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -221,7 +221,7 @@ CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -230,7 +230,7 @@ CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptors assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_IMU_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_IMU_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -246,7 +246,7 @@ CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptors } return result; } -CmdResult saveImuMessageFormat(C::mip_interface& device) +TypedResult saveImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -256,7 +256,7 @@ CmdResult saveImuMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadImuMessageFormat(C::mip_interface& device) +TypedResult loadImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -266,7 +266,7 @@ CmdResult loadImuMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultImuMessageFormat(C::mip_interface& device) +TypedResult defaultImuMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -318,7 +318,7 @@ void extract(Serializer& serializer, GpsMessageFormat::Response& self) } -CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -334,7 +334,7 @@ CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -343,7 +343,7 @@ CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptors assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -359,7 +359,7 @@ CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptors } return result; } -CmdResult saveGpsMessageFormat(C::mip_interface& device) +TypedResult saveGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -369,7 +369,7 @@ CmdResult saveGpsMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGpsMessageFormat(C::mip_interface& device) +TypedResult loadGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -379,7 +379,7 @@ CmdResult loadGpsMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGpsMessageFormat(C::mip_interface& device) +TypedResult defaultGpsMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -431,7 +431,7 @@ void extract(Serializer& serializer, FilterMessageFormat::Response& self) } -CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -447,7 +447,7 @@ CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescript return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -456,7 +456,7 @@ CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescript assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FILTER_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FILTER_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -472,7 +472,7 @@ CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescript } return result; } -CmdResult saveFilterMessageFormat(C::mip_interface& device) +TypedResult saveFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -482,7 +482,7 @@ CmdResult saveFilterMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadFilterMessageFormat(C::mip_interface& device) +TypedResult loadFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -492,7 +492,7 @@ CmdResult loadFilterMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FILTER_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultFilterMessageFormat(C::mip_interface& device) +TypedResult defaultFilterMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -524,12 +524,12 @@ void extract(Serializer& serializer, ImuGetBaseRate::Response& self) } -CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +TypedResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMU_BASE_RATE, NULL, 0, REPLY_IMU_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMU_BASE_RATE, NULL, 0, REPLY_IMU_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -565,12 +565,12 @@ void extract(Serializer& serializer, GpsGetBaseRate::Response& self) } -CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +TypedResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_GNSS_BASE_RATE, NULL, 0, REPLY_GNSS_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_GNSS_BASE_RATE, NULL, 0, REPLY_GNSS_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -606,12 +606,12 @@ void extract(Serializer& serializer, FilterGetBaseRate::Response& self) } -CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut) +TypedResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_FILTER_BASE_RATE, NULL, 0, REPLY_FILTER_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_FILTER_BASE_RATE, NULL, 0, REPLY_FILTER_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -649,7 +649,7 @@ void extract(Serializer& serializer, PollData& self) } -CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors) +TypedResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -694,7 +694,7 @@ void extract(Serializer& serializer, GetBaseRate::Response& self) } -CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut) +TypedResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -704,7 +704,7 @@ CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateO assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_BASE_RATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_BASE_RATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_BASE_RATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_BASE_RATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -770,7 +770,7 @@ void extract(Serializer& serializer, MessageFormat::Response& self) } -CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors) +TypedResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -788,7 +788,7 @@ CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) +TypedResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -799,7 +799,7 @@ CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -817,7 +817,7 @@ CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* } return result; } -CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) +TypedResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -829,7 +829,7 @@ CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) +TypedResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -841,7 +841,7 @@ CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet) +TypedResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -873,7 +873,7 @@ void extract(Serializer& serializer, NmeaPollData& self) } -CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries) +TypedResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -932,7 +932,7 @@ void extract(Serializer& serializer, NmeaMessageFormat::Response& self) } -CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries) +TypedResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -948,7 +948,7 @@ CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut) +TypedResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -957,7 +957,7 @@ CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uin assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NMEA_MESSAGE_FORMAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_NMEA_MESSAGE_FORMAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -973,7 +973,7 @@ CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uin } return result; } -CmdResult saveNmeaMessageFormat(C::mip_interface& device) +TypedResult saveNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -983,7 +983,7 @@ CmdResult saveNmeaMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadNmeaMessageFormat(C::mip_interface& device) +TypedResult loadNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -993,7 +993,7 @@ CmdResult loadNmeaMessageFormat(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_NMEA_MESSAGE_FORMAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultNmeaMessageFormat(C::mip_interface& device) +TypedResult defaultNmeaMessageFormat(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1014,7 +1014,7 @@ void extract(Serializer& serializer, DeviceSettings& self) } -CmdResult saveDeviceSettings(C::mip_interface& device) +TypedResult saveDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1024,7 +1024,7 @@ CmdResult saveDeviceSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DEVICE_STARTUP_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadDeviceSettings(C::mip_interface& device) +TypedResult loadDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1034,7 +1034,7 @@ CmdResult loadDeviceSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DEVICE_STARTUP_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultDeviceSettings(C::mip_interface& device) +TypedResult defaultDeviceSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1076,7 +1076,7 @@ void extract(Serializer& serializer, UartBaudrate::Response& self) } -CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) +TypedResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1088,7 +1088,7 @@ CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) +TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1097,7 +1097,7 @@ CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_UART_BAUDRATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_UART_BAUDRATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1111,7 +1111,7 @@ CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut) } return result; } -CmdResult saveUartBaudrate(C::mip_interface& device) +TypedResult saveUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1121,7 +1121,7 @@ CmdResult saveUartBaudrate(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadUartBaudrate(C::mip_interface& device) +TypedResult loadUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1131,7 +1131,7 @@ CmdResult loadUartBaudrate(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_UART_BAUDRATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultUartBaudrate(C::mip_interface& device) +TypedResult defaultUartBaudrate(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1156,7 +1156,7 @@ void extract(Serializer& serializer, FactoryStreaming& self) } -CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved) +TypedResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1209,7 +1209,7 @@ void extract(Serializer& serializer, DatastreamControl::Response& self) } -CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable) +TypedResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1223,7 +1223,7 @@ CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut) +TypedResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1234,7 +1234,7 @@ CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DATASTREAM_ENABLE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DATASTREAM_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1250,7 +1250,7 @@ CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* } return result; } -CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) +TypedResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1262,7 +1262,7 @@ CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) +TypedResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1274,7 +1274,7 @@ CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONTROL_DATA_STREAM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) +TypedResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1367,7 +1367,7 @@ void extract(Serializer& serializer, ConstellationSettings::Settings& self) } -CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) +TypedResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1385,7 +1385,7 @@ CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChann return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) +TypedResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1394,7 +1394,7 @@ CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChann assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_CONSTELLATION_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1416,7 +1416,7 @@ CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChann } return result; } -CmdResult saveConstellationSettings(C::mip_interface& device) +TypedResult saveConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1426,7 +1426,7 @@ CmdResult saveConstellationSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadConstellationSettings(C::mip_interface& device) +TypedResult loadConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1436,7 +1436,7 @@ CmdResult loadConstellationSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_CONSTELLATION_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultConstellationSettings(C::mip_interface& device) +TypedResult defaultConstellationSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1504,7 +1504,7 @@ void extract(Serializer& serializer, GnssSbasSettings::Response& self) } -CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns) +TypedResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1524,7 +1524,7 @@ CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, Gn return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut) +TypedResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1533,7 +1533,7 @@ CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SBAS_SETTINGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SBAS_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1555,7 +1555,7 @@ CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, } return result; } -CmdResult saveGnssSbasSettings(C::mip_interface& device) +TypedResult saveGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1565,7 +1565,7 @@ CmdResult saveGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssSbasSettings(C::mip_interface& device) +TypedResult loadGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1575,7 +1575,7 @@ CmdResult loadGnssSbasSettings(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SBAS_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssSbasSettings(C::mip_interface& device) +TypedResult defaultGnssSbasSettings(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1625,7 +1625,7 @@ void extract(Serializer& serializer, GnssAssistedFix::Response& self) } -CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) +TypedResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1639,7 +1639,7 @@ CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::Assist return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) +TypedResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1648,7 +1648,7 @@ CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::Assiste assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_ASSISTED_FIX_SETTINGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1665,7 +1665,7 @@ CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::Assiste } return result; } -CmdResult saveGnssAssistedFix(C::mip_interface& device) +TypedResult saveGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1675,7 +1675,7 @@ CmdResult saveGnssAssistedFix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssAssistedFix(C::mip_interface& device) +TypedResult loadGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1685,7 +1685,7 @@ CmdResult loadGnssAssistedFix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_ASSISTED_FIX_SETTINGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssAssistedFix(C::mip_interface& device) +TypedResult defaultGnssAssistedFix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1743,7 +1743,7 @@ void extract(Serializer& serializer, GnssTimeAssistance::Response& self) } -CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy) +TypedResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1759,7 +1759,7 @@ CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut) +TypedResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1768,7 +1768,7 @@ CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint1 assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_TIME_ASSISTANCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_TIME_ASSISTANCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_TIME_ASSISTANCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1852,7 +1852,7 @@ void extract(Serializer& serializer, ImuLowpassFilter::Response& self) } -CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) +TypedResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1872,7 +1872,7 @@ CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescript return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) +TypedResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1883,7 +1883,7 @@ CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADVANCED_DATA_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1908,7 +1908,7 @@ CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto } return result; } -CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +TypedResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1920,7 +1920,7 @@ CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +TypedResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1932,7 +1932,7 @@ CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescripto return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_IMU_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) +TypedResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1976,7 +1976,7 @@ void extract(Serializer& serializer, PpsSource::Response& self) } -CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source) +TypedResult writePpsSource(C::mip_interface& device, PpsSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1988,7 +1988,7 @@ CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) +TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1997,7 +1997,7 @@ CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PPS_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PPS_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2011,7 +2011,7 @@ CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut) } return result; } -CmdResult savePpsSource(C::mip_interface& device) +TypedResult savePpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2021,7 +2021,7 @@ CmdResult savePpsSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadPpsSource(C::mip_interface& device) +TypedResult loadPpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2031,7 +2031,7 @@ CmdResult loadPpsSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PPS_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultPpsSource(C::mip_interface& device) +TypedResult defaultPpsSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2097,7 +2097,7 @@ void extract(Serializer& serializer, GpioConfig::Response& self) } -CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode) +TypedResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2115,7 +2115,7 @@ CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Fea return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut) +TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2126,7 +2126,7 @@ CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feat assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2148,7 +2148,7 @@ CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feat } return result; } -CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin) +TypedResult saveGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2160,7 +2160,7 @@ CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin) +TypedResult loadGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2172,7 +2172,7 @@ CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin) +TypedResult defaultGpioConfig(C::mip_interface& device, uint8_t pin) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2230,7 +2230,7 @@ void extract(Serializer& serializer, GpioState::Response& self) } -CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) +TypedResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2244,7 +2244,7 @@ CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) +TypedResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2255,7 +2255,7 @@ CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_STATE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GPIO_STATE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GPIO_STATE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2319,7 +2319,7 @@ void extract(Serializer& serializer, Odometer::Response& self) } -CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty) +TypedResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2335,7 +2335,7 @@ CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float sca return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut) +TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2344,7 +2344,7 @@ CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOMETER_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ODOMETER_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2364,7 +2364,7 @@ CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* } return result; } -CmdResult saveOdometer(C::mip_interface& device) +TypedResult saveOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2374,7 +2374,7 @@ CmdResult saveOdometer(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadOdometer(C::mip_interface& device) +TypedResult loadOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2384,7 +2384,7 @@ CmdResult loadOdometer(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ODOMETER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultOdometer(C::mip_interface& device) +TypedResult defaultOdometer(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2444,7 +2444,7 @@ void extract(Serializer& serializer, GetEventSupport::Info& self) } -CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut) +TypedResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2454,7 +2454,7 @@ CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_SUPPORT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_SUPPORT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_SUPPORT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_SUPPORT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2515,7 +2515,7 @@ void extract(Serializer& serializer, EventControl::Response& self) } -CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode) +TypedResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2529,7 +2529,7 @@ CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventCon return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut) +TypedResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2540,7 +2540,7 @@ CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventCont assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2556,7 +2556,7 @@ CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventCont } return result; } -CmdResult saveEventControl(C::mip_interface& device, uint8_t instance) +TypedResult saveEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2568,7 +2568,7 @@ CmdResult saveEventControl(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEventControl(C::mip_interface& device, uint8_t instance) +TypedResult loadEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2580,7 +2580,7 @@ CmdResult loadEventControl(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance) +TypedResult defaultEventControl(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2639,7 +2639,7 @@ void extract(Serializer& serializer, GetEventTriggerStatus::Entry& self) } -CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut) +TypedResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2653,7 +2653,7 @@ CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_STATUS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2716,7 +2716,7 @@ void extract(Serializer& serializer, GetEventActionStatus::Entry& self) } -CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut) +TypedResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2730,7 +2730,7 @@ CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_STATUS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2941,7 +2941,7 @@ void extract(Serializer& serializer, EventTrigger::CombinationParams& self) } -CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters) +TypedResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2970,7 +2970,7 @@ CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTri return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut) +TypedResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2981,7 +2981,7 @@ CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrig assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_TRIGGER_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3012,7 +3012,7 @@ CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrig } return result; } -CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance) +TypedResult saveEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3024,7 +3024,7 @@ CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance) +TypedResult loadEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3036,7 +3036,7 @@ CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_TRIGGER_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance) +TypedResult defaultEventTrigger(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3175,7 +3175,7 @@ void extract(Serializer& serializer, EventAction::MessageParams& self) } -CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters) +TypedResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3201,7 +3201,7 @@ CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut) +TypedResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3212,7 +3212,7 @@ CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* t assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_EVENT_ACTION_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3241,7 +3241,7 @@ CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* t } return result; } -CmdResult saveEventAction(C::mip_interface& device, uint8_t instance) +TypedResult saveEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3253,7 +3253,7 @@ CmdResult saveEventAction(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEventAction(C::mip_interface& device, uint8_t instance) +TypedResult loadEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3265,7 +3265,7 @@ CmdResult loadEventAction(C::mip_interface& device, uint8_t instance) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_EVENT_ACTION_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance) +TypedResult defaultEventAction(C::mip_interface& device, uint8_t instance) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3313,7 +3313,7 @@ void extract(Serializer& serializer, AccelBias::Response& self) } -CmdResult writeAccelBias(C::mip_interface& device, const float* bias) +TypedResult writeAccelBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3327,7 +3327,7 @@ CmdResult writeAccelBias(C::mip_interface& device, const float* bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBias(C::mip_interface& device, float* biasOut) +TypedResult readAccelBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3336,7 +3336,7 @@ CmdResult readAccelBias(C::mip_interface& device, float* biasOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3351,7 +3351,7 @@ CmdResult readAccelBias(C::mip_interface& device, float* biasOut) } return result; } -CmdResult saveAccelBias(C::mip_interface& device) +TypedResult saveAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3361,7 +3361,7 @@ CmdResult saveAccelBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelBias(C::mip_interface& device) +TypedResult loadAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3371,7 +3371,7 @@ CmdResult loadAccelBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelBias(C::mip_interface& device) +TypedResult defaultAccelBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3417,7 +3417,7 @@ void extract(Serializer& serializer, GyroBias::Response& self) } -CmdResult writeGyroBias(C::mip_interface& device, const float* bias) +TypedResult writeGyroBias(C::mip_interface& device, const float* bias) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3431,7 +3431,7 @@ CmdResult writeGyroBias(C::mip_interface& device, const float* bias) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBias(C::mip_interface& device, float* biasOut) +TypedResult readGyroBias(C::mip_interface& device, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3440,7 +3440,7 @@ CmdResult readGyroBias(C::mip_interface& device, float* biasOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3455,7 +3455,7 @@ CmdResult readGyroBias(C::mip_interface& device, float* biasOut) } return result; } -CmdResult saveGyroBias(C::mip_interface& device) +TypedResult saveGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3465,7 +3465,7 @@ CmdResult saveGyroBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGyroBias(C::mip_interface& device) +TypedResult loadGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3475,7 +3475,7 @@ CmdResult loadGyroBias(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGyroBias(C::mip_interface& device) +TypedResult defaultGyroBias(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3509,7 +3509,7 @@ void extract(Serializer& serializer, CaptureGyroBias::Response& self) } -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) +TypedResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3519,7 +3519,7 @@ CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, fl assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CAPTURE_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CAPTURE_GYRO_BIAS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3570,7 +3570,7 @@ void extract(Serializer& serializer, MagHardIronOffset::Response& self) } -CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) +TypedResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3584,7 +3584,7 @@ CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) +TypedResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3593,7 +3593,7 @@ CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_VECTOR, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_VECTOR, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3608,7 +3608,7 @@ CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveMagHardIronOffset(C::mip_interface& device) +TypedResult saveMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3618,7 +3618,7 @@ CmdResult saveMagHardIronOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagHardIronOffset(C::mip_interface& device) +TypedResult loadMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3628,7 +3628,7 @@ CmdResult loadMagHardIronOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagHardIronOffset(C::mip_interface& device) +TypedResult defaultMagHardIronOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3674,7 +3674,7 @@ void extract(Serializer& serializer, MagSoftIronMatrix::Response& self) } -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) +TypedResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3688,7 +3688,7 @@ CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) +TypedResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3697,7 +3697,7 @@ CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_COMP_MATRIX, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_COMP_MATRIX, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3712,7 +3712,7 @@ CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveMagSoftIronMatrix(C::mip_interface& device) +TypedResult saveMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3722,7 +3722,7 @@ CmdResult saveMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagSoftIronMatrix(C::mip_interface& device) +TypedResult loadMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3732,7 +3732,7 @@ CmdResult loadMagSoftIronMatrix(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagSoftIronMatrix(C::mip_interface& device) +TypedResult defaultMagSoftIronMatrix(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3774,7 +3774,7 @@ void extract(Serializer& serializer, ConingScullingEnable::Response& self) } -CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable) +TypedResult writeConingScullingEnable(C::mip_interface& device, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3786,7 +3786,7 @@ CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) +TypedResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3795,7 +3795,7 @@ CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONING_AND_SCULLING_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3809,7 +3809,7 @@ CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut) } return result; } -CmdResult saveConingScullingEnable(C::mip_interface& device) +TypedResult saveConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3819,7 +3819,7 @@ CmdResult saveConingScullingEnable(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadConingScullingEnable(C::mip_interface& device) +TypedResult loadConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3829,7 +3829,7 @@ CmdResult loadConingScullingEnable(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONING_AND_SCULLING_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultConingScullingEnable(C::mip_interface& device) +TypedResult defaultConingScullingEnable(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3887,7 +3887,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformEuler::Response& sel } -CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw) +TypedResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3903,7 +3903,7 @@ CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) +TypedResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3912,7 +3912,7 @@ CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* roll assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3932,7 +3932,7 @@ CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* roll } return result; } -CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device) +TypedResult saveSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3942,7 +3942,7 @@ CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device) +TypedResult loadSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3952,7 +3952,7 @@ CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_EUL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device) +TypedResult defaultSensor2VehicleTransformEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3998,7 +3998,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response } -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) +TypedResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4012,7 +4012,7 @@ CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) +TypedResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4021,7 +4021,7 @@ CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4036,7 +4036,7 @@ CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* } return result; } -CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) +TypedResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4046,7 +4046,7 @@ CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) +TypedResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4056,7 +4056,7 @@ CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_QUAT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device) +TypedResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4102,7 +4102,7 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self) } -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) +TypedResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4116,7 +4116,7 @@ CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) +TypedResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4125,7 +4125,7 @@ CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4140,7 +4140,7 @@ CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut } return result; } -CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device) +TypedResult saveSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4150,7 +4150,7 @@ CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device) +TypedResult loadSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4160,7 +4160,7 @@ CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_TRANSFORM_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device) +TypedResult defaultSensor2VehicleTransformDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4226,7 +4226,7 @@ void extract(Serializer& serializer, ComplementaryFilter::Response& self) } -CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant) +TypedResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4244,7 +4244,7 @@ CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnabl return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut) +TypedResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4253,7 +4253,7 @@ CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnabl assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LEGACY_COMP_FILTER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LEGACY_COMP_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4276,7 +4276,7 @@ CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnabl } return result; } -CmdResult saveComplementaryFilter(C::mip_interface& device) +TypedResult saveComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4286,7 +4286,7 @@ CmdResult saveComplementaryFilter(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadComplementaryFilter(C::mip_interface& device) +TypedResult loadComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4296,7 +4296,7 @@ CmdResult loadComplementaryFilter(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LEGACY_COMP_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultComplementaryFilter(C::mip_interface& device) +TypedResult defaultComplementaryFilter(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4346,7 +4346,7 @@ void extract(Serializer& serializer, SensorRange::Response& self) } -CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting) +TypedResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4360,7 +4360,7 @@ CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uin return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut) +TypedResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4371,7 +4371,7 @@ CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_RANGE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_RANGE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4387,7 +4387,7 @@ CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint } return result; } -CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) +TypedResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4399,7 +4399,7 @@ CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) +TypedResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4411,7 +4411,7 @@ CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_RANGE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor) +TypedResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4469,7 +4469,7 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Entry& self) } -CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut) +TypedResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4479,7 +4479,7 @@ CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType senso assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CALIBRATED_RANGES, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CALIBRATED_RANGES, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CALIBRATED_RANGES, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CALIBRATED_RANGES, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4561,7 +4561,7 @@ void extract(Serializer& serializer, LowpassFilter::Response& self) } -CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) +TypedResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4581,7 +4581,7 @@ CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) +TypedResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4594,7 +4594,7 @@ CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LOWPASS_FILTER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_LOWPASS_FILTER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4618,7 +4618,7 @@ CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f } return result; } -CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +TypedResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4632,7 +4632,7 @@ CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +TypedResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4646,7 +4646,7 @@ CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_LOWPASS_FILTER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) +TypedResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index dbbe7447a..28987157f 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -213,6 +213,8 @@ struct PollImuMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_IMU_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollImuMessage"; + static constexpr const char* DOC_NAME = "PollImuMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -231,7 +233,7 @@ struct PollImuMessage void insert(Serializer& serializer, const PollImuMessage& self); void extract(Serializer& serializer, PollImuMessage& self); -CmdResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult pollImuMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -257,6 +259,8 @@ struct PollGnssMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_GNSS_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollGnssMessage"; + static constexpr const char* DOC_NAME = "PollGnssMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -275,7 +279,7 @@ struct PollGnssMessage void insert(Serializer& serializer, const PollGnssMessage& self); void extract(Serializer& serializer, PollGnssMessage& self); -CmdResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult pollGnssMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -301,6 +305,8 @@ struct PollFilterMessage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_FILTER_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollFilterMessage"; + static constexpr const char* DOC_NAME = "PollFilterMessage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -319,7 +325,7 @@ struct PollFilterMessage void insert(Serializer& serializer, const PollFilterMessage& self); void extract(Serializer& serializer, PollFilterMessage& self); -CmdResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult pollFilterMessage(C::mip_interface& device, bool suppressAck, uint8_t numDescriptors, const DescriptorRate* descriptors); ///@} /// @@ -340,6 +346,8 @@ struct ImuMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuMessageFormat"; + static constexpr const char* DOC_NAME = "ImuMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -372,6 +380,8 @@ struct ImuMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuMessageFormat::Response"; + static constexpr const char* DOC_NAME = "ImuMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -391,11 +401,11 @@ void extract(Serializer& serializer, ImuMessageFormat& self); void insert(Serializer& serializer, const ImuMessageFormat::Response& self); void extract(Serializer& serializer, ImuMessageFormat::Response& self); -CmdResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveImuMessageFormat(C::mip_interface& device); -CmdResult loadImuMessageFormat(C::mip_interface& device); -CmdResult defaultImuMessageFormat(C::mip_interface& device); +TypedResult writeImuMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readImuMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveImuMessageFormat(C::mip_interface& device); +TypedResult loadImuMessageFormat(C::mip_interface& device); +TypedResult defaultImuMessageFormat(C::mip_interface& device); ///@} /// @@ -416,6 +426,8 @@ struct GpsMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsMessageFormat"; + static constexpr const char* DOC_NAME = "GpsMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -448,6 +460,8 @@ struct GpsMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsMessageFormat::Response"; + static constexpr const char* DOC_NAME = "GpsMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -467,11 +481,11 @@ void extract(Serializer& serializer, GpsMessageFormat& self); void insert(Serializer& serializer, const GpsMessageFormat::Response& self); void extract(Serializer& serializer, GpsMessageFormat::Response& self); -CmdResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveGpsMessageFormat(C::mip_interface& device); -CmdResult loadGpsMessageFormat(C::mip_interface& device); -CmdResult defaultGpsMessageFormat(C::mip_interface& device); +TypedResult writeGpsMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readGpsMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveGpsMessageFormat(C::mip_interface& device); +TypedResult loadGpsMessageFormat(C::mip_interface& device); +TypedResult defaultGpsMessageFormat(C::mip_interface& device); ///@} /// @@ -492,6 +506,8 @@ struct FilterMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_FILTER_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterMessageFormat"; + static constexpr const char* DOC_NAME = "FilterMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -524,6 +540,8 @@ struct FilterMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterMessageFormat::Response"; + static constexpr const char* DOC_NAME = "FilterMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -543,11 +561,11 @@ void extract(Serializer& serializer, FilterMessageFormat& self); void insert(Serializer& serializer, const FilterMessageFormat::Response& self); void extract(Serializer& serializer, FilterMessageFormat::Response& self); -CmdResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveFilterMessageFormat(C::mip_interface& device); -CmdResult loadFilterMessageFormat(C::mip_interface& device); -CmdResult defaultFilterMessageFormat(C::mip_interface& device); +TypedResult writeFilterMessageFormat(C::mip_interface& device, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readFilterMessageFormat(C::mip_interface& device, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveFilterMessageFormat(C::mip_interface& device); +TypedResult loadFilterMessageFormat(C::mip_interface& device); +TypedResult defaultFilterMessageFormat(C::mip_interface& device); ///@} /// @@ -566,6 +584,8 @@ struct ImuGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_IMU_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuGetBaseRate"; + static constexpr const char* DOC_NAME = "Get IMU Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -585,6 +605,8 @@ struct ImuGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_IMU_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuGetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get IMU Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -603,7 +625,7 @@ void extract(Serializer& serializer, ImuGetBaseRate& self); void insert(Serializer& serializer, const ImuGetBaseRate::Response& self); void extract(Serializer& serializer, ImuGetBaseRate::Response& self); -CmdResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +TypedResult imuGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -622,6 +644,8 @@ struct GpsGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_GNSS_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsGetBaseRate"; + static constexpr const char* DOC_NAME = "Get GNSS Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -641,6 +665,8 @@ struct GpsGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsGetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get GNSS Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -659,7 +685,7 @@ void extract(Serializer& serializer, GpsGetBaseRate& self); void insert(Serializer& serializer, const GpsGetBaseRate::Response& self); void extract(Serializer& serializer, GpsGetBaseRate::Response& self); -CmdResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +TypedResult gpsGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -678,6 +704,8 @@ struct FilterGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_FILTER_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterGetBaseRate"; + static constexpr const char* DOC_NAME = "Get Estimation Filter Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -697,6 +725,8 @@ struct FilterGetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_FILTER_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FilterGetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get Estimation Filter Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -715,7 +745,7 @@ void extract(Serializer& serializer, FilterGetBaseRate& self); void insert(Serializer& serializer, const FilterGetBaseRate::Response& self); void extract(Serializer& serializer, FilterGetBaseRate::Response& self); -CmdResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); +TypedResult filterGetBaseRate(C::mip_interface& device, uint16_t* rateOut); ///@} /// @@ -742,6 +772,8 @@ struct PollData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PollData"; + static constexpr const char* DOC_NAME = "PollData"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -760,7 +792,7 @@ struct PollData void insert(Serializer& serializer, const PollData& self); void extract(Serializer& serializer, PollData& self); -CmdResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors); +TypedResult pollData(C::mip_interface& device, uint8_t descSet, bool suppressAck, uint8_t numDescriptors, const uint8_t* descriptors); ///@} /// @@ -777,6 +809,8 @@ struct GetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GET_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetBaseRate"; + static constexpr const char* DOC_NAME = "Get Data Base Rate"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -796,6 +830,8 @@ struct GetBaseRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_BASE_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetBaseRate::Response"; + static constexpr const char* DOC_NAME = "Get Data Base Rate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -815,7 +851,7 @@ void extract(Serializer& serializer, GetBaseRate& self); void insert(Serializer& serializer, const GetBaseRate::Response& self); void extract(Serializer& serializer, GetBaseRate::Response& self); -CmdResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut); +TypedResult getBaseRate(C::mip_interface& device, uint8_t descSet, uint16_t* rateOut); ///@} /// @@ -837,6 +873,8 @@ struct MessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MessageFormat"; + static constexpr const char* DOC_NAME = "MessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -870,6 +908,8 @@ struct MessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MessageFormat::Response"; + static constexpr const char* DOC_NAME = "MessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -890,11 +930,11 @@ void extract(Serializer& serializer, MessageFormat& self); void insert(Serializer& serializer, const MessageFormat::Response& self); void extract(Serializer& serializer, MessageFormat::Response& self); -CmdResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors); -CmdResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); -CmdResult saveMessageFormat(C::mip_interface& device, uint8_t descSet); -CmdResult loadMessageFormat(C::mip_interface& device, uint8_t descSet); -CmdResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); +TypedResult writeMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t numDescriptors, const DescriptorRate* descriptors); +TypedResult readMessageFormat(C::mip_interface& device, uint8_t descSet, uint8_t* numDescriptorsOut, uint8_t numDescriptorsOutMax, DescriptorRate* descriptorsOut); +TypedResult saveMessageFormat(C::mip_interface& device, uint8_t descSet); +TypedResult loadMessageFormat(C::mip_interface& device, uint8_t descSet); +TypedResult defaultMessageFormat(C::mip_interface& device, uint8_t descSet); ///@} /// @@ -919,6 +959,8 @@ struct NmeaPollData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_POLL_NMEA_MESSAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NmeaPollData"; + static constexpr const char* DOC_NAME = "NmeaPollData"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -937,7 +979,7 @@ struct NmeaPollData void insert(Serializer& serializer, const NmeaPollData& self); void extract(Serializer& serializer, NmeaPollData& self); -CmdResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries); +TypedResult nmeaPollData(C::mip_interface& device, bool suppressAck, uint8_t count, const NmeaMessage* formatEntries); ///@} /// @@ -956,6 +998,8 @@ struct NmeaMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_NMEA_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NmeaMessageFormat"; + static constexpr const char* DOC_NAME = "NmeaMessageFormat"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -988,6 +1032,8 @@ struct NmeaMessageFormat static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_NMEA_MESSAGE_FORMAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NmeaMessageFormat::Response"; + static constexpr const char* DOC_NAME = "NmeaMessageFormat Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -1007,11 +1053,11 @@ void extract(Serializer& serializer, NmeaMessageFormat& self); void insert(Serializer& serializer, const NmeaMessageFormat::Response& self); void extract(Serializer& serializer, NmeaMessageFormat::Response& self); -CmdResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries); -CmdResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut); -CmdResult saveNmeaMessageFormat(C::mip_interface& device); -CmdResult loadNmeaMessageFormat(C::mip_interface& device); -CmdResult defaultNmeaMessageFormat(C::mip_interface& device); +TypedResult writeNmeaMessageFormat(C::mip_interface& device, uint8_t count, const NmeaMessage* formatEntries); +TypedResult readNmeaMessageFormat(C::mip_interface& device, uint8_t* countOut, uint8_t countOutMax, NmeaMessage* formatEntriesOut); +TypedResult saveNmeaMessageFormat(C::mip_interface& device); +TypedResult loadNmeaMessageFormat(C::mip_interface& device); +TypedResult defaultNmeaMessageFormat(C::mip_interface& device); ///@} /// @@ -1032,6 +1078,8 @@ struct DeviceSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_DEVICE_STARTUP_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeviceSettings"; + static constexpr const char* DOC_NAME = "DeviceSettings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x0000; @@ -1063,9 +1111,9 @@ struct DeviceSettings void insert(Serializer& serializer, const DeviceSettings& self); void extract(Serializer& serializer, DeviceSettings& self); -CmdResult saveDeviceSettings(C::mip_interface& device); -CmdResult loadDeviceSettings(C::mip_interface& device); -CmdResult defaultDeviceSettings(C::mip_interface& device); +TypedResult saveDeviceSettings(C::mip_interface& device); +TypedResult loadDeviceSettings(C::mip_interface& device); +TypedResult defaultDeviceSettings(C::mip_interface& device); ///@} /// @@ -1097,6 +1145,8 @@ struct UartBaudrate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_UART_BAUDRATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UartBaudrate"; + static constexpr const char* DOC_NAME = "UartBaudrate"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1129,6 +1179,8 @@ struct UartBaudrate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_UART_BAUDRATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UartBaudrate::Response"; + static constexpr const char* DOC_NAME = "UartBaudrate Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1147,11 +1199,11 @@ void extract(Serializer& serializer, UartBaudrate& self); void insert(Serializer& serializer, const UartBaudrate::Response& self); void extract(Serializer& serializer, UartBaudrate::Response& self); -CmdResult writeUartBaudrate(C::mip_interface& device, uint32_t baud); -CmdResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut); -CmdResult saveUartBaudrate(C::mip_interface& device); -CmdResult loadUartBaudrate(C::mip_interface& device); -CmdResult defaultUartBaudrate(C::mip_interface& device); +TypedResult writeUartBaudrate(C::mip_interface& device, uint32_t baud); +TypedResult readUartBaudrate(C::mip_interface& device, uint32_t* baudOut); +TypedResult saveUartBaudrate(C::mip_interface& device); +TypedResult loadUartBaudrate(C::mip_interface& device); +TypedResult defaultUartBaudrate(C::mip_interface& device); ///@} /// @@ -1179,6 +1231,8 @@ struct FactoryStreaming static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONFIGURE_FACTORY_STREAMING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FactoryStreaming"; + static constexpr const char* DOC_NAME = "FactoryStreaming"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1197,7 +1251,7 @@ struct FactoryStreaming void insert(Serializer& serializer, const FactoryStreaming& self); void extract(Serializer& serializer, FactoryStreaming& self); -CmdResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved); +TypedResult factoryStreaming(C::mip_interface& device, FactoryStreaming::Action action, uint8_t reserved); ///@} /// @@ -1225,6 +1279,8 @@ struct DatastreamControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONTROL_DATA_STREAM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DatastreamControl"; + static constexpr const char* DOC_NAME = "DatastreamControl"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1258,6 +1314,8 @@ struct DatastreamControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_DATASTREAM_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DatastreamControl::Response"; + static constexpr const char* DOC_NAME = "DatastreamControl Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1277,11 +1335,11 @@ void extract(Serializer& serializer, DatastreamControl& self); void insert(Serializer& serializer, const DatastreamControl::Response& self); void extract(Serializer& serializer, DatastreamControl::Response& self); -CmdResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable); -CmdResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut); -CmdResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet); -CmdResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet); -CmdResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); +TypedResult writeDatastreamControl(C::mip_interface& device, uint8_t descSet, bool enable); +TypedResult readDatastreamControl(C::mip_interface& device, uint8_t descSet, bool* enabledOut); +TypedResult saveDatastreamControl(C::mip_interface& device, uint8_t descSet); +TypedResult loadDatastreamControl(C::mip_interface& device, uint8_t descSet); +TypedResult defaultDatastreamControl(C::mip_interface& device, uint8_t descSet); ///@} /// @@ -1334,7 +1392,7 @@ struct ConstellationSettings OptionFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } OptionFlags& operator=(uint16_t val) { value = val; return *this; } - OptionFlags& operator=(int val) { value = val; return *this; } + OptionFlags& operator=(int val) { value = uint16_t(val); return *this; } OptionFlags& operator|=(uint16_t val) { return *this = value | val; } OptionFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1362,6 +1420,8 @@ struct ConstellationSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_CONSTELLATION_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConstellationSettings"; + static constexpr const char* DOC_NAME = "ConstellationSettings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -1394,6 +1454,8 @@ struct ConstellationSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_CONSTELLATION_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConstellationSettings::Response"; + static constexpr const char* DOC_NAME = "ConstellationSettings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -1418,11 +1480,11 @@ void extract(Serializer& serializer, ConstellationSettings::Settings& self); void insert(Serializer& serializer, const ConstellationSettings::Response& self); void extract(Serializer& serializer, ConstellationSettings::Response& self); -CmdResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); -CmdResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); -CmdResult saveConstellationSettings(C::mip_interface& device); -CmdResult loadConstellationSettings(C::mip_interface& device); -CmdResult defaultConstellationSettings(C::mip_interface& device); +TypedResult writeConstellationSettings(C::mip_interface& device, uint16_t maxChannels, uint8_t configCount, const ConstellationSettings::Settings* settings); +TypedResult readConstellationSettings(C::mip_interface& device, uint16_t* maxChannelsAvailableOut, uint16_t* maxChannelsUseOut, uint8_t* configCountOut, uint8_t configCountOutMax, ConstellationSettings::Settings* settingsOut); +TypedResult saveConstellationSettings(C::mip_interface& device); +TypedResult loadConstellationSettings(C::mip_interface& device); +TypedResult defaultConstellationSettings(C::mip_interface& device); ///@} /// @@ -1453,7 +1515,7 @@ struct GnssSbasSettings SBASOptions(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } SBASOptions& operator=(uint16_t val) { value = val; return *this; } - SBASOptions& operator=(int val) { value = val; return *this; } + SBASOptions& operator=(int val) { value = uint16_t(val); return *this; } SBASOptions& operator|=(uint16_t val) { return *this = value | val; } SBASOptions& operator&=(uint16_t val) { return *this = value & val; } @@ -1477,6 +1539,8 @@ struct GnssSbasSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_SBAS_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSbasSettings"; + static constexpr const char* DOC_NAME = "SBAS Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -1509,6 +1573,8 @@ struct GnssSbasSettings static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_SBAS_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSbasSettings::Response"; + static constexpr const char* DOC_NAME = "SBAS Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -1530,11 +1596,11 @@ void extract(Serializer& serializer, GnssSbasSettings& self); void insert(Serializer& serializer, const GnssSbasSettings::Response& self); void extract(Serializer& serializer, GnssSbasSettings::Response& self); -CmdResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns); -CmdResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut); -CmdResult saveGnssSbasSettings(C::mip_interface& device); -CmdResult loadGnssSbasSettings(C::mip_interface& device); -CmdResult defaultGnssSbasSettings(C::mip_interface& device); +TypedResult writeGnssSbasSettings(C::mip_interface& device, uint8_t enableSbas, GnssSbasSettings::SBASOptions sbasOptions, uint8_t numIncludedPrns, const uint16_t* includedPrns); +TypedResult readGnssSbasSettings(C::mip_interface& device, uint8_t* enableSbasOut, GnssSbasSettings::SBASOptions* sbasOptionsOut, uint8_t* numIncludedPrnsOut, uint8_t numIncludedPrnsOutMax, uint16_t* includedPrnsOut); +TypedResult saveGnssSbasSettings(C::mip_interface& device); +TypedResult loadGnssSbasSettings(C::mip_interface& device); +TypedResult defaultGnssSbasSettings(C::mip_interface& device); ///@} /// @@ -1569,6 +1635,8 @@ struct GnssAssistedFix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_ASSISTED_FIX_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAssistedFix"; + static constexpr const char* DOC_NAME = "GNSS Assisted Fix Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1601,6 +1669,8 @@ struct GnssAssistedFix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_ASSISTED_FIX_SETTINGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAssistedFix::Response"; + static constexpr const char* DOC_NAME = "GNSS Assisted Fix Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1620,11 +1690,11 @@ void extract(Serializer& serializer, GnssAssistedFix& self); void insert(Serializer& serializer, const GnssAssistedFix::Response& self); void extract(Serializer& serializer, GnssAssistedFix::Response& self); -CmdResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); -CmdResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); -CmdResult saveGnssAssistedFix(C::mip_interface& device); -CmdResult loadGnssAssistedFix(C::mip_interface& device); -CmdResult defaultGnssAssistedFix(C::mip_interface& device); +TypedResult writeGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption option, uint8_t flags); +TypedResult readGnssAssistedFix(C::mip_interface& device, GnssAssistedFix::AssistedFixOption* optionOut, uint8_t* flagsOut); +TypedResult saveGnssAssistedFix(C::mip_interface& device); +TypedResult loadGnssAssistedFix(C::mip_interface& device); +TypedResult defaultGnssAssistedFix(C::mip_interface& device); ///@} /// @@ -1647,6 +1717,8 @@ struct GnssTimeAssistance static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GNSS_TIME_ASSISTANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssTimeAssistance"; + static constexpr const char* DOC_NAME = "GnssTimeAssistance"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -1679,6 +1751,8 @@ struct GnssTimeAssistance static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GNSS_TIME_ASSISTANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssTimeAssistance::Response"; + static constexpr const char* DOC_NAME = "GnssTimeAssistance Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1699,8 +1773,8 @@ void extract(Serializer& serializer, GnssTimeAssistance& self); void insert(Serializer& serializer, const GnssTimeAssistance::Response& self); void extract(Serializer& serializer, GnssTimeAssistance::Response& self); -CmdResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy); -CmdResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut); +TypedResult writeGnssTimeAssistance(C::mip_interface& device, double tow, uint16_t weekNumber, float accuracy); +TypedResult readGnssTimeAssistance(C::mip_interface& device, double* towOut, uint16_t* weekNumberOut, float* accuracyOut); ///@} /// @@ -1737,6 +1811,8 @@ struct ImuLowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_IMU_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuLowpassFilter"; + static constexpr const char* DOC_NAME = "Advanced Low-Pass Filter Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -1770,6 +1846,8 @@ struct ImuLowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ADVANCED_DATA_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ImuLowpassFilter::Response"; + static constexpr const char* DOC_NAME = "Advanced Low-Pass Filter Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1792,11 +1870,11 @@ void extract(Serializer& serializer, ImuLowpassFilter& self); void insert(Serializer& serializer, const ImuLowpassFilter::Response& self); void extract(Serializer& serializer, ImuLowpassFilter::Response& self); -CmdResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); -CmdResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); -CmdResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -CmdResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); -CmdResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +TypedResult writeImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool enable, bool manual, uint16_t frequency, uint8_t reserved); +TypedResult readImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor, bool* enableOut, bool* manualOut, uint16_t* frequencyOut, uint8_t* reservedOut); +TypedResult saveImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +TypedResult loadImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); +TypedResult defaultImuLowpassFilter(C::mip_interface& device, uint8_t targetDescriptor); ///@} /// @@ -1823,6 +1901,8 @@ struct PpsSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_PPS_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PpsSource"; + static constexpr const char* DOC_NAME = "PpsSource"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1855,6 +1935,8 @@ struct PpsSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_PPS_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PpsSource::Response"; + static constexpr const char* DOC_NAME = "PpsSource Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1873,11 +1955,11 @@ void extract(Serializer& serializer, PpsSource& self); void insert(Serializer& serializer, const PpsSource::Response& self); void extract(Serializer& serializer, PpsSource::Response& self); -CmdResult writePpsSource(C::mip_interface& device, PpsSource::Source source); -CmdResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut); -CmdResult savePpsSource(C::mip_interface& device); -CmdResult loadPpsSource(C::mip_interface& device); -CmdResult defaultPpsSource(C::mip_interface& device); +TypedResult writePpsSource(C::mip_interface& device, PpsSource::Source source); +TypedResult readPpsSource(C::mip_interface& device, PpsSource::Source* sourceOut); +TypedResult savePpsSource(C::mip_interface& device); +TypedResult loadPpsSource(C::mip_interface& device); +TypedResult defaultPpsSource(C::mip_interface& device); ///@} /// @@ -1950,7 +2032,7 @@ struct GpioConfig PinMode(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } PinMode& operator=(uint8_t val) { value = val; return *this; } - PinMode& operator=(int val) { value = val; return *this; } + PinMode& operator=(int val) { value = uint8_t(val); return *this; } PinMode& operator|=(uint8_t val) { return *this = value | val; } PinMode& operator&=(uint8_t val) { return *this = value & val; } @@ -1974,6 +2056,8 @@ struct GpioConfig static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioConfig"; + static constexpr const char* DOC_NAME = "GPIO Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2007,6 +2091,8 @@ struct GpioConfig static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioConfig::Response"; + static constexpr const char* DOC_NAME = "GPIO Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2028,11 +2114,11 @@ void extract(Serializer& serializer, GpioConfig& self); void insert(Serializer& serializer, const GpioConfig::Response& self); void extract(Serializer& serializer, GpioConfig::Response& self); -CmdResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode); -CmdResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut); -CmdResult saveGpioConfig(C::mip_interface& device, uint8_t pin); -CmdResult loadGpioConfig(C::mip_interface& device, uint8_t pin); -CmdResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); +TypedResult writeGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature feature, GpioConfig::Behavior behavior, GpioConfig::PinMode pinMode); +TypedResult readGpioConfig(C::mip_interface& device, uint8_t pin, GpioConfig::Feature* featureOut, GpioConfig::Behavior* behaviorOut, GpioConfig::PinMode* pinModeOut); +TypedResult saveGpioConfig(C::mip_interface& device, uint8_t pin); +TypedResult loadGpioConfig(C::mip_interface& device, uint8_t pin); +TypedResult defaultGpioConfig(C::mip_interface& device, uint8_t pin); ///@} /// @@ -2066,6 +2152,8 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioState"; + static constexpr const char* DOC_NAME = "GPIO State"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2098,6 +2186,8 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioState::Response"; + static constexpr const char* DOC_NAME = "GPIO State Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2117,8 +2207,8 @@ void extract(Serializer& serializer, GpioState& self); void insert(Serializer& serializer, const GpioState::Response& self); void extract(Serializer& serializer, GpioState::Response& self); -CmdResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state); -CmdResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); +TypedResult writeGpioState(C::mip_interface& device, uint8_t pin, bool state); +TypedResult readGpioState(C::mip_interface& device, uint8_t pin, bool* stateOut); ///@} /// @@ -2145,6 +2235,8 @@ struct Odometer static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ODOMETER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Odometer"; + static constexpr const char* DOC_NAME = "Odometer Settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -2177,6 +2269,8 @@ struct Odometer static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ODOMETER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Odometer::Response"; + static constexpr const char* DOC_NAME = "Odometer Settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2197,11 +2291,11 @@ void extract(Serializer& serializer, Odometer& self); void insert(Serializer& serializer, const Odometer::Response& self); void extract(Serializer& serializer, Odometer::Response& self); -CmdResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty); -CmdResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut); -CmdResult saveOdometer(C::mip_interface& device); -CmdResult loadOdometer(C::mip_interface& device); -CmdResult defaultOdometer(C::mip_interface& device); +TypedResult writeOdometer(C::mip_interface& device, Odometer::Mode mode, float scaling, float uncertainty); +TypedResult readOdometer(C::mip_interface& device, Odometer::Mode* modeOut, float* scalingOut, float* uncertaintyOut); +TypedResult saveOdometer(C::mip_interface& device); +TypedResult loadOdometer(C::mip_interface& device); +TypedResult defaultOdometer(C::mip_interface& device); ///@} /// @@ -2246,6 +2340,8 @@ struct GetEventSupport static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_SUPPORT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventSupport"; + static constexpr const char* DOC_NAME = "Get Supported Events"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -2265,6 +2361,8 @@ struct GetEventSupport static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_SUPPORT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventSupport::Response"; + static constexpr const char* DOC_NAME = "Get Supported Events Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x000000C0; @@ -2289,7 +2387,7 @@ void extract(Serializer& serializer, GetEventSupport::Info& self); void insert(Serializer& serializer, const GetEventSupport::Response& self); void extract(Serializer& serializer, GetEventSupport::Response& self); -CmdResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut); +TypedResult getEventSupport(C::mip_interface& device, GetEventSupport::Query query, uint8_t* maxInstancesOut, uint8_t* numEntriesOut, uint8_t numEntriesOutMax, GetEventSupport::Info* entriesOut); ///@} /// @@ -2325,6 +2423,8 @@ struct EventControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventControl"; + static constexpr const char* DOC_NAME = "Event Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2358,6 +2458,8 @@ struct EventControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventControl::Response"; + static constexpr const char* DOC_NAME = "Event Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2377,11 +2479,11 @@ void extract(Serializer& serializer, EventControl& self); void insert(Serializer& serializer, const EventControl::Response& self); void extract(Serializer& serializer, EventControl::Response& self); -CmdResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode); -CmdResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut); -CmdResult saveEventControl(C::mip_interface& device, uint8_t instance); -CmdResult loadEventControl(C::mip_interface& device, uint8_t instance); -CmdResult defaultEventControl(C::mip_interface& device, uint8_t instance); +TypedResult writeEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode mode); +TypedResult readEventControl(C::mip_interface& device, uint8_t instance, EventControl::Mode* modeOut); +TypedResult saveEventControl(C::mip_interface& device, uint8_t instance); +TypedResult loadEventControl(C::mip_interface& device, uint8_t instance); +TypedResult defaultEventControl(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2408,7 +2510,7 @@ struct GetEventTriggerStatus Status(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } Status& operator=(uint8_t val) { value = val; return *this; } - Status& operator=(int val) { value = val; return *this; } + Status& operator=(int val) { value = uint8_t(val); return *this; } Status& operator|=(uint8_t val) { return *this = value | val; } Status& operator&=(uint8_t val) { return *this = value & val; } @@ -2435,6 +2537,8 @@ struct GetEventTriggerStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventTriggerStatus"; + static constexpr const char* DOC_NAME = "Get Trigger Status"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -2454,6 +2558,8 @@ struct GetEventTriggerStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventTriggerStatus::Response"; + static constexpr const char* DOC_NAME = "Get Trigger Status Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -2476,7 +2582,7 @@ void extract(Serializer& serializer, GetEventTriggerStatus::Entry& self); void insert(Serializer& serializer, const GetEventTriggerStatus::Response& self); void extract(Serializer& serializer, GetEventTriggerStatus::Response& self); -CmdResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut); +TypedResult getEventTriggerStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventTriggerStatus::Entry* triggersOut); ///@} /// @@ -2499,6 +2605,8 @@ struct GetEventActionStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventActionStatus"; + static constexpr const char* DOC_NAME = "Get Action Status"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -2518,6 +2626,8 @@ struct GetEventActionStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetEventActionStatus::Response"; + static constexpr const char* DOC_NAME = "Get Action Status Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -2540,7 +2650,7 @@ void extract(Serializer& serializer, GetEventActionStatus::Entry& self); void insert(Serializer& serializer, const GetEventActionStatus::Response& self); void extract(Serializer& serializer, GetEventActionStatus::Response& self); -CmdResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut); +TypedResult getEventActionStatus(C::mip_interface& device, uint8_t requestedCount, const uint8_t* requestedInstances, uint8_t* countOut, uint8_t countOutMax, GetEventActionStatus::Entry* actionsOut); ///@} /// @@ -2633,6 +2743,8 @@ struct EventTrigger static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_TRIGGER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventTrigger"; + static constexpr const char* DOC_NAME = "Event Trigger Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -2666,6 +2778,8 @@ struct EventTrigger static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_TRIGGER_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventTrigger::Response"; + static constexpr const char* DOC_NAME = "Event Trigger Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2695,11 +2809,11 @@ void extract(Serializer& serializer, EventTrigger::CombinationParams& self); void insert(Serializer& serializer, const EventTrigger::Response& self); void extract(Serializer& serializer, EventTrigger::Response& self); -CmdResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters); -CmdResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut); -CmdResult saveEventTrigger(C::mip_interface& device, uint8_t instance); -CmdResult loadEventTrigger(C::mip_interface& device, uint8_t instance); -CmdResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); +TypedResult writeEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type type, const EventTrigger::Parameters& parameters); +TypedResult readEventTrigger(C::mip_interface& device, uint8_t instance, EventTrigger::Type* typeOut, EventTrigger::Parameters* parametersOut); +TypedResult saveEventTrigger(C::mip_interface& device, uint8_t instance); +TypedResult loadEventTrigger(C::mip_interface& device, uint8_t instance); +TypedResult defaultEventTrigger(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2758,6 +2872,8 @@ struct EventAction static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_EVENT_ACTION_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventAction"; + static constexpr const char* DOC_NAME = "Event Action Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2791,6 +2907,8 @@ struct EventAction static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_EVENT_ACTION_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventAction::Response"; + static constexpr const char* DOC_NAME = "Event Action Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2818,11 +2936,11 @@ void extract(Serializer& serializer, EventAction::MessageParams& self); void insert(Serializer& serializer, const EventAction::Response& self); void extract(Serializer& serializer, EventAction::Response& self); -CmdResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters); -CmdResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut); -CmdResult saveEventAction(C::mip_interface& device, uint8_t instance); -CmdResult loadEventAction(C::mip_interface& device, uint8_t instance); -CmdResult defaultEventAction(C::mip_interface& device, uint8_t instance); +TypedResult writeEventAction(C::mip_interface& device, uint8_t instance, uint8_t trigger, EventAction::Type type, const EventAction::Parameters& parameters); +TypedResult readEventAction(C::mip_interface& device, uint8_t instance, uint8_t* triggerOut, EventAction::Type* typeOut, EventAction::Parameters* parametersOut); +TypedResult saveEventAction(C::mip_interface& device, uint8_t instance); +TypedResult loadEventAction(C::mip_interface& device, uint8_t instance); +TypedResult defaultEventAction(C::mip_interface& device, uint8_t instance); ///@} /// @@ -2842,6 +2960,8 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_ACCEL_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBias"; + static constexpr const char* DOC_NAME = "Configure Accel Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2874,6 +2994,8 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_ACCEL_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBias::Response"; + static constexpr const char* DOC_NAME = "Configure Accel Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2892,11 +3014,11 @@ void extract(Serializer& serializer, AccelBias& self); void insert(Serializer& serializer, const AccelBias::Response& self); void extract(Serializer& serializer, AccelBias::Response& self); -CmdResult writeAccelBias(C::mip_interface& device, const float* bias); -CmdResult readAccelBias(C::mip_interface& device, float* biasOut); -CmdResult saveAccelBias(C::mip_interface& device); -CmdResult loadAccelBias(C::mip_interface& device); -CmdResult defaultAccelBias(C::mip_interface& device); +TypedResult writeAccelBias(C::mip_interface& device, const float* bias); +TypedResult readAccelBias(C::mip_interface& device, float* biasOut); +TypedResult saveAccelBias(C::mip_interface& device); +TypedResult loadAccelBias(C::mip_interface& device); +TypedResult defaultAccelBias(C::mip_interface& device); ///@} /// @@ -2916,6 +3038,8 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBias"; + static constexpr const char* DOC_NAME = "Configure Gyro Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2948,6 +3072,8 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBias::Response"; + static constexpr const char* DOC_NAME = "Configure Gyro Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2966,11 +3092,11 @@ void extract(Serializer& serializer, GyroBias& self); void insert(Serializer& serializer, const GyroBias::Response& self); void extract(Serializer& serializer, GyroBias::Response& self); -CmdResult writeGyroBias(C::mip_interface& device, const float* bias); -CmdResult readGyroBias(C::mip_interface& device, float* biasOut); -CmdResult saveGyroBias(C::mip_interface& device); -CmdResult loadGyroBias(C::mip_interface& device); -CmdResult defaultGyroBias(C::mip_interface& device); +TypedResult writeGyroBias(C::mip_interface& device, const float* bias); +TypedResult readGyroBias(C::mip_interface& device, float* biasOut); +TypedResult saveGyroBias(C::mip_interface& device); +TypedResult loadGyroBias(C::mip_interface& device); +TypedResult defaultGyroBias(C::mip_interface& device); ///@} /// @@ -2992,6 +3118,8 @@ struct CaptureGyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CAPTURE_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CaptureGyroBias"; + static constexpr const char* DOC_NAME = "Capture Gyro Bias"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -3011,6 +3139,8 @@ struct CaptureGyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_GYRO_BIAS_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CaptureGyroBias::Response"; + static constexpr const char* DOC_NAME = "Capture Gyro Bias Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3029,7 +3159,7 @@ void extract(Serializer& serializer, CaptureGyroBias& self); void insert(Serializer& serializer, const CaptureGyroBias::Response& self); void extract(Serializer& serializer, CaptureGyroBias::Response& self); -CmdResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); +TypedResult captureGyroBias(C::mip_interface& device, uint16_t averagingTimeMs, float* biasOut); ///@} /// @@ -3053,6 +3183,8 @@ struct MagHardIronOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_HARD_IRON_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagHardIronOffset"; + static constexpr const char* DOC_NAME = "Magnetometer Hard Iron Offset"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3085,6 +3217,8 @@ struct MagHardIronOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_HARD_IRON_OFFSET_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagHardIronOffset::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Hard Iron Offset Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3103,11 +3237,11 @@ void extract(Serializer& serializer, MagHardIronOffset& self); void insert(Serializer& serializer, const MagHardIronOffset::Response& self); void extract(Serializer& serializer, MagHardIronOffset::Response& self); -CmdResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); -CmdResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); -CmdResult saveMagHardIronOffset(C::mip_interface& device); -CmdResult loadMagHardIronOffset(C::mip_interface& device); -CmdResult defaultMagHardIronOffset(C::mip_interface& device); +TypedResult writeMagHardIronOffset(C::mip_interface& device, const float* offset); +TypedResult readMagHardIronOffset(C::mip_interface& device, float* offsetOut); +TypedResult saveMagHardIronOffset(C::mip_interface& device); +TypedResult loadMagHardIronOffset(C::mip_interface& device); +TypedResult defaultMagHardIronOffset(C::mip_interface& device); ///@} /// @@ -3135,6 +3269,8 @@ struct MagSoftIronMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SOFT_IRON_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagSoftIronMatrix"; + static constexpr const char* DOC_NAME = "Magnetometer Soft Iron Matrix"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3167,6 +3303,8 @@ struct MagSoftIronMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SOFT_IRON_COMP_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagSoftIronMatrix::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Soft Iron Matrix Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3185,11 +3323,11 @@ void extract(Serializer& serializer, MagSoftIronMatrix& self); void insert(Serializer& serializer, const MagSoftIronMatrix::Response& self); void extract(Serializer& serializer, MagSoftIronMatrix::Response& self); -CmdResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); -CmdResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); -CmdResult saveMagSoftIronMatrix(C::mip_interface& device); -CmdResult loadMagSoftIronMatrix(C::mip_interface& device); -CmdResult defaultMagSoftIronMatrix(C::mip_interface& device); +TypedResult writeMagSoftIronMatrix(C::mip_interface& device, const float* offset); +TypedResult readMagSoftIronMatrix(C::mip_interface& device, float* offsetOut); +TypedResult saveMagSoftIronMatrix(C::mip_interface& device); +TypedResult loadMagSoftIronMatrix(C::mip_interface& device); +TypedResult defaultMagSoftIronMatrix(C::mip_interface& device); ///@} /// @@ -3207,6 +3345,8 @@ struct ConingScullingEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CONING_AND_SCULLING_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConingScullingEnable"; + static constexpr const char* DOC_NAME = "Coning and Sculling Enable"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3239,6 +3379,8 @@ struct ConingScullingEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CONING_AND_SCULLING_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConingScullingEnable::Response"; + static constexpr const char* DOC_NAME = "Coning and Sculling Enable Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3257,11 +3399,11 @@ void extract(Serializer& serializer, ConingScullingEnable& self); void insert(Serializer& serializer, const ConingScullingEnable::Response& self); void extract(Serializer& serializer, ConingScullingEnable::Response& self); -CmdResult writeConingScullingEnable(C::mip_interface& device, bool enable); -CmdResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); -CmdResult saveConingScullingEnable(C::mip_interface& device); -CmdResult loadConingScullingEnable(C::mip_interface& device); -CmdResult defaultConingScullingEnable(C::mip_interface& device); +TypedResult writeConingScullingEnable(C::mip_interface& device, bool enable); +TypedResult readConingScullingEnable(C::mip_interface& device, bool* enableOut); +TypedResult saveConingScullingEnable(C::mip_interface& device); +TypedResult loadConingScullingEnable(C::mip_interface& device); +TypedResult defaultConingScullingEnable(C::mip_interface& device); ///@} /// @@ -3305,6 +3447,8 @@ struct Sensor2VehicleTransformEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_EUL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformEuler"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Euler"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3337,6 +3481,8 @@ struct Sensor2VehicleTransformEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_EUL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformEuler::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Euler Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3357,11 +3503,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformEuler& self); void insert(Serializer& serializer, const Sensor2VehicleTransformEuler::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformEuler::Response& self); -CmdResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw); -CmdResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); -CmdResult saveSensor2VehicleTransformEuler(C::mip_interface& device); -CmdResult loadSensor2VehicleTransformEuler(C::mip_interface& device); -CmdResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); +TypedResult writeSensor2VehicleTransformEuler(C::mip_interface& device, float roll, float pitch, float yaw); +TypedResult readSensor2VehicleTransformEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); +TypedResult saveSensor2VehicleTransformEuler(C::mip_interface& device); +TypedResult loadSensor2VehicleTransformEuler(C::mip_interface& device); +TypedResult defaultSensor2VehicleTransformEuler(C::mip_interface& device); ///@} /// @@ -3411,6 +3557,8 @@ struct Sensor2VehicleTransformQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_QUAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformQuaternion"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Quaternion"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3443,6 +3591,8 @@ struct Sensor2VehicleTransformQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_QUAT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformQuaternion::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Quaternion Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3461,11 +3611,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion& self); void insert(Serializer& serializer, const Sensor2VehicleTransformQuaternion::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformQuaternion::Response& self); -CmdResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); -CmdResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); -CmdResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); -CmdResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); -CmdResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); +TypedResult writeSensor2VehicleTransformQuaternion(C::mip_interface& device, const float* q); +TypedResult readSensor2VehicleTransformQuaternion(C::mip_interface& device, float* qOut); +TypedResult saveSensor2VehicleTransformQuaternion(C::mip_interface& device); +TypedResult loadSensor2VehicleTransformQuaternion(C::mip_interface& device); +TypedResult defaultSensor2VehicleTransformQuaternion(C::mip_interface& device); ///@} /// @@ -3513,6 +3663,8 @@ struct Sensor2VehicleTransformDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR2VEHICLE_TRANSFORM_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformDcm"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Direction Cosine Matrix"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3545,6 +3697,8 @@ struct Sensor2VehicleTransformDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR2VEHICLE_TRANSFORM_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Sensor2VehicleTransformDcm::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Transformation Direction Cosine Matrix Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3563,11 +3717,11 @@ void extract(Serializer& serializer, Sensor2VehicleTransformDcm& self); void insert(Serializer& serializer, const Sensor2VehicleTransformDcm::Response& self); void extract(Serializer& serializer, Sensor2VehicleTransformDcm::Response& self); -CmdResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); -CmdResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); -CmdResult saveSensor2VehicleTransformDcm(C::mip_interface& device); -CmdResult loadSensor2VehicleTransformDcm(C::mip_interface& device); -CmdResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); +TypedResult writeSensor2VehicleTransformDcm(C::mip_interface& device, const float* dcm); +TypedResult readSensor2VehicleTransformDcm(C::mip_interface& device, float* dcmOut); +TypedResult saveSensor2VehicleTransformDcm(C::mip_interface& device); +TypedResult loadSensor2VehicleTransformDcm(C::mip_interface& device); +TypedResult defaultSensor2VehicleTransformDcm(C::mip_interface& device); ///@} /// @@ -3592,6 +3746,8 @@ struct ComplementaryFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LEGACY_COMP_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ComplementaryFilter"; + static constexpr const char* DOC_NAME = "Complementary filter settings"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -3624,6 +3780,8 @@ struct ComplementaryFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LEGACY_COMP_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ComplementaryFilter::Response"; + static constexpr const char* DOC_NAME = "Complementary filter settings Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3645,11 +3803,11 @@ void extract(Serializer& serializer, ComplementaryFilter& self); void insert(Serializer& serializer, const ComplementaryFilter::Response& self); void extract(Serializer& serializer, ComplementaryFilter::Response& self); -CmdResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant); -CmdResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut); -CmdResult saveComplementaryFilter(C::mip_interface& device); -CmdResult loadComplementaryFilter(C::mip_interface& device); -CmdResult defaultComplementaryFilter(C::mip_interface& device); +TypedResult writeComplementaryFilter(C::mip_interface& device, bool pitchRollEnable, bool headingEnable, float pitchRollTimeConstant, float headingTimeConstant); +TypedResult readComplementaryFilter(C::mip_interface& device, bool* pitchRollEnableOut, bool* headingEnableOut, float* pitchRollTimeConstantOut, float* headingTimeConstantOut); +TypedResult saveComplementaryFilter(C::mip_interface& device); +TypedResult loadComplementaryFilter(C::mip_interface& device); +TypedResult defaultComplementaryFilter(C::mip_interface& device); ///@} /// @@ -3675,6 +3833,8 @@ struct SensorRange static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_SENSOR_RANGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorRange"; + static constexpr const char* DOC_NAME = "Sensor Range"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3708,6 +3868,8 @@ struct SensorRange static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_SENSOR_RANGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorRange::Response"; + static constexpr const char* DOC_NAME = "Sensor Range Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3727,11 +3889,11 @@ void extract(Serializer& serializer, SensorRange& self); void insert(Serializer& serializer, const SensorRange::Response& self); void extract(Serializer& serializer, SensorRange::Response& self); -CmdResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting); -CmdResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut); -CmdResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor); -CmdResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor); -CmdResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); +TypedResult writeSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t setting); +TypedResult readSensorRange(C::mip_interface& device, SensorRangeType sensor, uint8_t* settingOut); +TypedResult saveSensorRange(C::mip_interface& device, SensorRangeType sensor); +TypedResult loadSensorRange(C::mip_interface& device, SensorRangeType sensor); +TypedResult defaultSensorRange(C::mip_interface& device, SensorRangeType sensor); ///@} /// @@ -3757,6 +3919,8 @@ struct CalibratedSensorRanges static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_CALIBRATED_RANGES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CalibratedSensorRanges"; + static constexpr const char* DOC_NAME = "Get Calibrated Sensor Ranges"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; @@ -3776,6 +3940,8 @@ struct CalibratedSensorRanges static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_CALIBRATED_RANGES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CalibratedSensorRanges::Response"; + static constexpr const char* DOC_NAME = "Get Calibrated Sensor Ranges Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000030; @@ -3799,7 +3965,7 @@ void extract(Serializer& serializer, CalibratedSensorRanges::Entry& self); void insert(Serializer& serializer, const CalibratedSensorRanges::Response& self); void extract(Serializer& serializer, CalibratedSensorRanges::Response& self); -CmdResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); +TypedResult calibratedSensorRanges(C::mip_interface& device, SensorRangeType sensor, uint8_t* numRangesOut, uint8_t numRangesOutMax, CalibratedSensorRanges::Entry* rangesOut); ///@} /// @@ -3834,6 +4000,8 @@ struct LowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::CMD_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LowpassFilter"; + static constexpr const char* DOC_NAME = "Low-pass anti-aliasing filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -3868,6 +4036,8 @@ struct LowpassFilter static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_3dm::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_3dm::REPLY_LOWPASS_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LowpassFilter::Response"; + static constexpr const char* DOC_NAME = "Low-pass anti-aliasing filter Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3890,11 +4060,11 @@ void extract(Serializer& serializer, LowpassFilter& self); void insert(Serializer& serializer, const LowpassFilter::Response& self); void extract(Serializer& serializer, LowpassFilter::Response& self); -CmdResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); -CmdResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); -CmdResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); -CmdResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +TypedResult writeLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool enable, bool manual, float frequency); +TypedResult readLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc, bool* enableOut, bool* manualOut, float* frequencyOut); +TypedResult saveLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +TypedResult loadLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); +TypedResult defaultLowpassFilter(C::mip_interface& device, uint8_t descSet, uint8_t fieldDesc); ///@} /// diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index a5eb60002..9981eb802 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -88,7 +88,7 @@ void extract(Serializer& serializer, SensorFrameMapping::Response& self) } -CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) +TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -102,7 +102,7 @@ CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) +TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -111,7 +111,7 @@ CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -128,7 +128,7 @@ CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, } return result; } -CmdResult saveSensorFrameMapping(C::mip_interface& device) +TypedResult saveSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -138,7 +138,7 @@ CmdResult saveSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorFrameMapping(C::mip_interface& device) +TypedResult loadSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -148,7 +148,7 @@ CmdResult loadSensorFrameMapping(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorFrameMapping(C::mip_interface& device) +TypedResult defaultSensorFrameMapping(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -256,7 +256,7 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) } } -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) +TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -284,7 +284,7 @@ CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, Referen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) +TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -297,7 +297,7 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -326,7 +326,7 @@ CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, Referenc } return result; } -CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -338,7 +338,7 @@ CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -350,7 +350,7 @@ CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -394,7 +394,7 @@ void extract(Serializer& serializer, AidingEchoControl::Response& self) } -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) +TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -406,7 +406,7 @@ CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) +TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -415,7 +415,7 @@ CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mod assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -429,7 +429,7 @@ CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mod } return result; } -CmdResult saveAidingEchoControl(C::mip_interface& device) +TypedResult saveAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -439,7 +439,7 @@ CmdResult saveAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAidingEchoControl(C::mip_interface& device) +TypedResult loadAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -449,7 +449,7 @@ CmdResult loadAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAidingEchoControl(C::mip_interface& device) +TypedResult defaultAidingEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -490,7 +490,7 @@ void extract(Serializer& serializer, EcefPos& self) } -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -550,7 +550,7 @@ void extract(Serializer& serializer, LlhPos& self) } -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -602,7 +602,7 @@ void extract(Serializer& serializer, Height& self) } -CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) +TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -648,7 +648,7 @@ void extract(Serializer& serializer, Pressure& self) } -CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -698,7 +698,7 @@ void extract(Serializer& serializer, EcefVel& self) } -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -752,7 +752,7 @@ void extract(Serializer& serializer, NedVel& self) } -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -806,7 +806,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -856,7 +856,7 @@ void extract(Serializer& serializer, TrueHeading& self) } -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -906,7 +906,7 @@ void extract(Serializer& serializer, MagneticField& self) } -CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 5a72202c9..28ffd2617 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -97,6 +97,8 @@ struct SensorFrameMapping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorFrameMapping"; + static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -129,6 +131,8 @@ struct SensorFrameMapping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorFrameMapping::Response"; + static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -148,11 +152,11 @@ void extract(Serializer& serializer, SensorFrameMapping& self); void insert(Serializer& serializer, const SensorFrameMapping::Response& self); void extract(Serializer& serializer, SensorFrameMapping::Response& self); -CmdResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); -CmdResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); -CmdResult saveSensorFrameMapping(C::mip_interface& device); -CmdResult loadSensorFrameMapping(C::mip_interface& device); -CmdResult defaultSensorFrameMapping(C::mip_interface& device); +TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); +TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); +TypedResult saveSensorFrameMapping(C::mip_interface& device); +TypedResult loadSensorFrameMapping(C::mip_interface& device); +TypedResult defaultSensorFrameMapping(C::mip_interface& device); ///@} /// @@ -207,6 +211,8 @@ struct ReferenceFrame static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceFrame"; + static constexpr const char* DOC_NAME = "Reference Frame Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -240,6 +246,8 @@ struct ReferenceFrame static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceFrame::Response"; + static constexpr const char* DOC_NAME = "Reference Frame Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -261,11 +269,11 @@ void extract(Serializer& serializer, ReferenceFrame& self); void insert(Serializer& serializer, const ReferenceFrame::Response& self); void extract(Serializer& serializer, ReferenceFrame::Response& self); -CmdResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); -CmdResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); -CmdResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); -CmdResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); -CmdResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); +TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); +TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); ///@} /// @@ -290,6 +298,8 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingEchoControl"; + static constexpr const char* DOC_NAME = "Aiding Command Echo Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -322,6 +332,8 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingEchoControl::Response"; + static constexpr const char* DOC_NAME = "Aiding Command Echo Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -340,11 +352,11 @@ void extract(Serializer& serializer, AidingEchoControl& self); void insert(Serializer& serializer, const AidingEchoControl::Response& self); void extract(Serializer& serializer, AidingEchoControl::Response& self); -CmdResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); -CmdResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); -CmdResult saveAidingEchoControl(C::mip_interface& device); -CmdResult loadAidingEchoControl(C::mip_interface& device); -CmdResult defaultAidingEchoControl(C::mip_interface& device); +TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); +TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); +TypedResult saveAidingEchoControl(C::mip_interface& device); +TypedResult loadAidingEchoControl(C::mip_interface& device); +TypedResult defaultAidingEchoControl(C::mip_interface& device); ///@} /// @@ -372,7 +384,7 @@ struct EcefPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -396,6 +408,8 @@ struct EcefPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefPos"; + static constexpr const char* DOC_NAME = "ECEF Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -414,7 +428,7 @@ struct EcefPos void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -CmdResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); ///@} /// @@ -443,7 +457,7 @@ struct LlhPos ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -469,6 +483,8 @@ struct LlhPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LlhPos"; + static constexpr const char* DOC_NAME = "LLH Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -487,7 +503,7 @@ struct LlhPos void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -CmdResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); ///@} /// @@ -508,6 +524,8 @@ struct Height static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEIGHT_ABS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Height"; + static constexpr const char* DOC_NAME = "Height"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -526,7 +544,7 @@ struct Height void insert(Serializer& serializer, const Height& self); void extract(Serializer& serializer, Height& self); -CmdResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); +TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); ///@} /// @@ -547,6 +565,8 @@ struct Pressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_PRESSURE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Pressure"; + static constexpr const char* DOC_NAME = "Pressure"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -565,7 +585,7 @@ struct Pressure void insert(Serializer& serializer, const Pressure& self); void extract(Serializer& serializer, Pressure& self); -CmdResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); ///@} /// @@ -593,7 +613,7 @@ struct EcefVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -617,6 +637,8 @@ struct EcefVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefVel"; + static constexpr const char* DOC_NAME = "ECEF Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -635,7 +657,7 @@ struct EcefVel void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -CmdResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); ///@} /// @@ -663,7 +685,7 @@ struct NedVel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -687,6 +709,8 @@ struct NedVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NedVel"; + static constexpr const char* DOC_NAME = "NED Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -705,7 +729,7 @@ struct NedVel void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -CmdResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); ///@} /// @@ -734,7 +758,7 @@ struct VehicleFixedFrameVelocity ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -758,6 +782,8 @@ struct VehicleFixedFrameVelocity static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VehicleFixedFrameVelocity"; + static constexpr const char* DOC_NAME = "Vehicle Frame Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -776,7 +802,7 @@ struct VehicleFixedFrameVelocity void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -CmdResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); ///@} /// @@ -796,6 +822,8 @@ struct TrueHeading static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TrueHeading"; + static constexpr const char* DOC_NAME = "True Heading"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -814,7 +842,7 @@ struct TrueHeading void insert(Serializer& serializer, const TrueHeading& self); void extract(Serializer& serializer, TrueHeading& self); -CmdResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); ///@} /// @@ -842,7 +870,7 @@ struct MagneticField ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -866,6 +894,8 @@ struct MagneticField static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_MAGNETIC_FIELD; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticField"; + static constexpr const char* DOC_NAME = "Magnetic Field"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -884,7 +914,7 @@ struct MagneticField void insert(Serializer& serializer, const MagneticField& self); void extract(Serializer& serializer, MagneticField& self); -CmdResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); ///@} /// diff --git a/src/mip/definitions/commands_base.cpp b/src/mip/definitions/commands_base.cpp index 035d105f7..bdd9de55d 100644 --- a/src/mip/definitions/commands_base.cpp +++ b/src/mip/definitions/commands_base.cpp @@ -81,7 +81,7 @@ void extract(Serializer& serializer, Ping& self) (void)self; } -CmdResult ping(C::mip_interface& device) +TypedResult ping(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PING, NULL, 0); } @@ -96,7 +96,7 @@ void extract(Serializer& serializer, SetIdle& self) (void)self; } -CmdResult setIdle(C::mip_interface& device) +TypedResult setIdle(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SET_TO_IDLE, NULL, 0); } @@ -122,12 +122,12 @@ void extract(Serializer& serializer, GetDeviceInfo::Response& self) } -CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) +TypedResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_INFO, NULL, 0, REPLY_DEVICE_INFO, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_INFO, NULL, 0, REPLY_DEVICE_INFO, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -165,12 +165,12 @@ void extract(Serializer& serializer, GetDeviceDescriptors::Response& self) } -CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) +TypedResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_DESCRIPTORS, NULL, 0, REPLY_DEVICE_DESCRIPTORS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_DEVICE_DESCRIPTORS, NULL, 0, REPLY_DEVICE_DESCRIPTORS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -206,12 +206,12 @@ void extract(Serializer& serializer, BuiltInTest::Response& self) } -CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut) +TypedResult builtInTest(C::mip_interface& device, uint32_t* resultOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_BUILT_IN_TEST, NULL, 0, REPLY_BUILT_IN_TEST, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_BUILT_IN_TEST, NULL, 0, REPLY_BUILT_IN_TEST, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -236,7 +236,7 @@ void extract(Serializer& serializer, Resume& self) (void)self; } -CmdResult resume(C::mip_interface& device) +TypedResult resume(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RESUME, NULL, 0); } @@ -264,12 +264,12 @@ void extract(Serializer& serializer, GetExtendedDescriptors::Response& self) } -CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) +TypedResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_EXTENDED_DESCRIPTORS, NULL, 0, REPLY_GET_EXTENDED_DESCRIPTORS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_EXTENDED_DESCRIPTORS, NULL, 0, REPLY_GET_EXTENDED_DESCRIPTORS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -307,12 +307,12 @@ void extract(Serializer& serializer, ContinuousBit::Response& self) } -CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut) +TypedResult continuousBit(C::mip_interface& device, uint8_t* resultOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTINUOUS_BIT, NULL, 0, REPLY_CONTINUOUS_BIT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONTINUOUS_BIT, NULL, 0, REPLY_CONTINUOUS_BIT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -367,7 +367,7 @@ void extract(Serializer& serializer, CommSpeed::Response& self) } -CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) +TypedResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -381,7 +381,7 @@ CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut) +TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -392,7 +392,7 @@ CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COMM_SPEED, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COMM_SPEED, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -408,7 +408,7 @@ CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOu } return result; } -CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port) +TypedResult saveCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -420,7 +420,7 @@ CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port) +TypedResult loadCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -432,7 +432,7 @@ CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMM_SPEED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port) +TypedResult defaultCommSpeed(C::mip_interface& device, uint8_t port) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -469,7 +469,7 @@ void extract(Serializer& serializer, GpsTimeUpdate& self) } } -CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value) +TypedResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -494,7 +494,7 @@ void extract(Serializer& serializer, SoftReset& self) (void)self; } -CmdResult softReset(C::mip_interface& device) +TypedResult softReset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_RESET, NULL, 0); } diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index 5665a4753..e9d1bf0c3 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -118,7 +118,7 @@ struct CommandedTestBitsGq7 : Bitfield CommandedTestBitsGq7(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } CommandedTestBitsGq7& operator=(uint32_t val) { value = val; return *this; } - CommandedTestBitsGq7& operator=(int val) { value = val; return *this; } + CommandedTestBitsGq7& operator=(int val) { value = uint32_t(val); return *this; } CommandedTestBitsGq7& operator|=(uint32_t val) { return *this = value | val; } CommandedTestBitsGq7& operator&=(uint32_t val) { return *this = value & val; } @@ -202,6 +202,8 @@ struct Ping static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_PING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Ping"; + static constexpr const char* DOC_NAME = "Ping"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -220,7 +222,7 @@ struct Ping void insert(Serializer& serializer, const Ping& self); void extract(Serializer& serializer, Ping& self); -CmdResult ping(C::mip_interface& device); +TypedResult ping(C::mip_interface& device); ///@} /// @@ -240,6 +242,8 @@ struct SetIdle static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SET_TO_IDLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SetIdle"; + static constexpr const char* DOC_NAME = "Set to idle"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -258,7 +262,7 @@ struct SetIdle void insert(Serializer& serializer, const SetIdle& self); void extract(Serializer& serializer, SetIdle& self); -CmdResult setIdle(C::mip_interface& device); +TypedResult setIdle(C::mip_interface& device); ///@} /// @@ -274,6 +278,8 @@ struct GetDeviceInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceInfo"; + static constexpr const char* DOC_NAME = "Get device information"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -293,6 +299,8 @@ struct GetDeviceInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceInfo::Response"; + static constexpr const char* DOC_NAME = "Get device information Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -311,7 +319,7 @@ void extract(Serializer& serializer, GetDeviceInfo& self); void insert(Serializer& serializer, const GetDeviceInfo::Response& self); void extract(Serializer& serializer, GetDeviceInfo::Response& self); -CmdResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut); +TypedResult getDeviceInfo(C::mip_interface& device, BaseDeviceInfo* deviceInfoOut); ///@} /// @@ -330,6 +338,8 @@ struct GetDeviceDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_DEVICE_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceDescriptors"; + static constexpr const char* DOC_NAME = "Get device descriptors"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -349,6 +359,8 @@ struct GetDeviceDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_DEVICE_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetDeviceDescriptors::Response"; + static constexpr const char* DOC_NAME = "Get device descriptors Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; @@ -368,7 +380,7 @@ void extract(Serializer& serializer, GetDeviceDescriptors& self); void insert(Serializer& serializer, const GetDeviceDescriptors::Response& self); void extract(Serializer& serializer, GetDeviceDescriptors::Response& self); -CmdResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); +TypedResult getDeviceDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); ///@} /// @@ -389,6 +401,8 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BuiltInTest"; + static constexpr const char* DOC_NAME = "Built in test"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -408,6 +422,8 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BuiltInTest::Response"; + static constexpr const char* DOC_NAME = "Built in test Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -426,7 +442,7 @@ void extract(Serializer& serializer, BuiltInTest& self); void insert(Serializer& serializer, const BuiltInTest::Response& self); void extract(Serializer& serializer, BuiltInTest::Response& self); -CmdResult builtInTest(C::mip_interface& device, uint32_t* resultOut); +TypedResult builtInTest(C::mip_interface& device, uint32_t* resultOut); ///@} /// @@ -444,6 +460,8 @@ struct Resume static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_RESUME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Resume"; + static constexpr const char* DOC_NAME = "Resume"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -462,7 +480,7 @@ struct Resume void insert(Serializer& serializer, const Resume& self); void extract(Serializer& serializer, Resume& self); -CmdResult resume(C::mip_interface& device); +TypedResult resume(C::mip_interface& device); ///@} /// @@ -481,6 +499,8 @@ struct GetExtendedDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GET_EXTENDED_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetExtendedDescriptors"; + static constexpr const char* DOC_NAME = "Get device descriptors (extended)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -500,6 +520,8 @@ struct GetExtendedDescriptors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_GET_EXTENDED_DESCRIPTORS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetExtendedDescriptors::Response"; + static constexpr const char* DOC_NAME = "Get device descriptors (extended) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000001; @@ -519,7 +541,7 @@ void extract(Serializer& serializer, GetExtendedDescriptors& self); void insert(Serializer& serializer, const GetExtendedDescriptors::Response& self); void extract(Serializer& serializer, GetExtendedDescriptors::Response& self); -CmdResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); +TypedResult getExtendedDescriptors(C::mip_interface& device, uint16_t* descriptorsOut, size_t descriptorsOutMax, uint8_t* descriptorsOutCount); ///@} /// @@ -537,6 +559,8 @@ struct ContinuousBit static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_CONTINUOUS_BIT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ContinuousBit"; + static constexpr const char* DOC_NAME = "Continuous built-in test"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -556,6 +580,8 @@ struct ContinuousBit static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_CONTINUOUS_BIT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ContinuousBit::Response"; + static constexpr const char* DOC_NAME = "Continuous built-in test Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -574,7 +600,7 @@ void extract(Serializer& serializer, ContinuousBit& self); void insert(Serializer& serializer, const ContinuousBit::Response& self); void extract(Serializer& serializer, ContinuousBit::Response& self); -CmdResult continuousBit(C::mip_interface& device, uint8_t* resultOut); +TypedResult continuousBit(C::mip_interface& device, uint8_t* resultOut); ///@} /// @@ -607,6 +633,8 @@ struct CommSpeed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_COMM_SPEED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommSpeed"; + static constexpr const char* DOC_NAME = "Comm Port Speed"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -640,6 +668,8 @@ struct CommSpeed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::REPLY_COMM_SPEED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommSpeed::Response"; + static constexpr const char* DOC_NAME = "Comm Port Speed Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -659,11 +689,11 @@ void extract(Serializer& serializer, CommSpeed& self); void insert(Serializer& serializer, const CommSpeed::Response& self); void extract(Serializer& serializer, CommSpeed::Response& self); -CmdResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud); -CmdResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut); -CmdResult saveCommSpeed(C::mip_interface& device, uint8_t port); -CmdResult loadCommSpeed(C::mip_interface& device, uint8_t port); -CmdResult defaultCommSpeed(C::mip_interface& device, uint8_t port); +TypedResult writeCommSpeed(C::mip_interface& device, uint8_t port, uint32_t baud); +TypedResult readCommSpeed(C::mip_interface& device, uint8_t port, uint32_t* baudOut); +TypedResult saveCommSpeed(C::mip_interface& device, uint8_t port); +TypedResult loadCommSpeed(C::mip_interface& device, uint8_t port); +TypedResult defaultCommSpeed(C::mip_interface& device, uint8_t port); ///@} /// @@ -691,6 +721,8 @@ struct GpsTimeUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTimeUpdate"; + static constexpr const char* DOC_NAME = "Time Broadcast Command"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -722,7 +754,7 @@ struct GpsTimeUpdate void insert(Serializer& serializer, const GpsTimeUpdate& self); void extract(Serializer& serializer, GpsTimeUpdate& self); -CmdResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value); +TypedResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeUpdate::FieldId fieldId, uint32_t value); ///@} /// @@ -740,6 +772,8 @@ struct SoftReset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_SOFT_RESET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SoftReset"; + static constexpr const char* DOC_NAME = "Reset device"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -758,7 +792,7 @@ struct SoftReset void insert(Serializer& serializer, const SoftReset& self); void extract(Serializer& serializer, SoftReset& self); -CmdResult softReset(C::mip_interface& device); +TypedResult softReset(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_filter.cpp b/src/mip/definitions/commands_filter.cpp index c88cb0eb8..7792142bf 100644 --- a/src/mip/definitions/commands_filter.cpp +++ b/src/mip/definitions/commands_filter.cpp @@ -40,7 +40,7 @@ void extract(Serializer& serializer, Reset& self) (void)self; } -CmdResult reset(C::mip_interface& device) +TypedResult reset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RESET_FILTER, NULL, 0); } @@ -63,7 +63,7 @@ void extract(Serializer& serializer, SetInitialAttitude& self) } -CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading) +TypedResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -110,7 +110,7 @@ void extract(Serializer& serializer, EstimationControl::Response& self) } -CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable) +TypedResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -122,7 +122,7 @@ CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::En return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut) +TypedResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -131,7 +131,7 @@ CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::Ena assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ESTIMATION_CONTROL_FLAGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ESTIMATION_CONTROL_FLAGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -145,7 +145,7 @@ CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::Ena } return result; } -CmdResult saveEstimationControl(C::mip_interface& device) +TypedResult saveEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -155,7 +155,7 @@ CmdResult saveEstimationControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadEstimationControl(C::mip_interface& device) +TypedResult loadEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -165,7 +165,7 @@ CmdResult loadEstimationControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ESTIMATION_CONTROL_FLAGS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultEstimationControl(C::mip_interface& device) +TypedResult defaultEstimationControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -220,7 +220,7 @@ void extract(Serializer& serializer, ExternalGnssUpdate& self) } -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) +TypedResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -270,7 +270,7 @@ void extract(Serializer& serializer, ExternalHeadingUpdate& self) } -CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type) +TypedResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -312,7 +312,7 @@ void extract(Serializer& serializer, ExternalHeadingUpdateWithTime& self) } -CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type) +TypedResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -363,7 +363,7 @@ void extract(Serializer& serializer, TareOrientation::Response& self) } -CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes) +TypedResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -375,7 +375,7 @@ CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTar return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut) +TypedResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -384,7 +384,7 @@ CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTare assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_TARE_ORIENTATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_TARE_ORIENTATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -398,7 +398,7 @@ CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTare } return result; } -CmdResult saveTareOrientation(C::mip_interface& device) +TypedResult saveTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -408,7 +408,7 @@ CmdResult saveTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadTareOrientation(C::mip_interface& device) +TypedResult loadTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -418,7 +418,7 @@ CmdResult loadTareOrientation(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_TARE_ORIENTATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultTareOrientation(C::mip_interface& device) +TypedResult defaultTareOrientation(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -460,7 +460,7 @@ void extract(Serializer& serializer, VehicleDynamicsMode::Response& self) } -CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) +TypedResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -472,7 +472,7 @@ CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) +TypedResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -481,7 +481,7 @@ CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode: assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_DYNAMICS_MODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -495,7 +495,7 @@ CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode: } return result; } -CmdResult saveVehicleDynamicsMode(C::mip_interface& device) +TypedResult saveVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -505,7 +505,7 @@ CmdResult saveVehicleDynamicsMode(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadVehicleDynamicsMode(C::mip_interface& device) +TypedResult loadVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -515,7 +515,7 @@ CmdResult loadVehicleDynamicsMode(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_DYNAMICS_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultVehicleDynamicsMode(C::mip_interface& device) +TypedResult defaultVehicleDynamicsMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -573,7 +573,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationEuler::Response& sel } -CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw) +TypedResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -589,7 +589,7 @@ CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) +TypedResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -598,7 +598,7 @@ CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* roll assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_EULER, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_EULER, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -618,7 +618,7 @@ CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* roll } return result; } -CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device) +TypedResult saveSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -628,7 +628,7 @@ CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device) +TypedResult loadSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -638,7 +638,7 @@ CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_EULER, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device) +TypedResult defaultSensorToVehicleRotationEuler(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -684,7 +684,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self) } -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) +TypedResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -698,7 +698,7 @@ CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) +TypedResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -707,7 +707,7 @@ CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_DCM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_DCM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -722,7 +722,7 @@ CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut } return result; } -CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device) +TypedResult saveSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -732,7 +732,7 @@ CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device) +TypedResult loadSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -742,7 +742,7 @@ CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_DCM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device) +TypedResult defaultSensorToVehicleRotationDcm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -788,7 +788,7 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response } -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) +TypedResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -802,7 +802,7 @@ CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) +TypedResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -811,7 +811,7 @@ CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -826,7 +826,7 @@ CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* } return result; } -CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) +TypedResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -836,7 +836,7 @@ CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) +TypedResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -846,7 +846,7 @@ CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_ROTATION_QUATERNION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device) +TypedResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -892,7 +892,7 @@ void extract(Serializer& serializer, SensorToVehicleOffset::Response& self) } -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) +TypedResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -906,7 +906,7 @@ CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offs return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) +TypedResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -915,7 +915,7 @@ CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_OFFSET, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR2VEHICLE_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -930,7 +930,7 @@ CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveSensorToVehicleOffset(C::mip_interface& device) +TypedResult saveSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -940,7 +940,7 @@ CmdResult saveSensorToVehicleOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSensorToVehicleOffset(C::mip_interface& device) +TypedResult loadSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -950,7 +950,7 @@ CmdResult loadSensorToVehicleOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR2VEHICLE_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSensorToVehicleOffset(C::mip_interface& device) +TypedResult defaultSensorToVehicleOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -996,7 +996,7 @@ void extract(Serializer& serializer, AntennaOffset::Response& self) } -CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) +TypedResult writeAntennaOffset(C::mip_interface& device, const float* offset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1010,7 +1010,7 @@ CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) +TypedResult readAntennaOffset(C::mip_interface& device, float* offsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1019,7 +1019,7 @@ CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_OFFSET, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1034,7 +1034,7 @@ CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut) } return result; } -CmdResult saveAntennaOffset(C::mip_interface& device) +TypedResult saveAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1044,7 +1044,7 @@ CmdResult saveAntennaOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAntennaOffset(C::mip_interface& device) +TypedResult loadAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1054,7 +1054,7 @@ CmdResult loadAntennaOffset(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAntennaOffset(C::mip_interface& device) +TypedResult defaultAntennaOffset(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1096,7 +1096,7 @@ void extract(Serializer& serializer, GnssSource::Response& self) } -CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) +TypedResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1108,7 +1108,7 @@ CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut) +TypedResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1117,7 +1117,7 @@ CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SOURCE_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GNSS_SOURCE_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1131,7 +1131,7 @@ CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut } return result; } -CmdResult saveGnssSource(C::mip_interface& device) +TypedResult saveGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1141,7 +1141,7 @@ CmdResult saveGnssSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssSource(C::mip_interface& device) +TypedResult loadGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1151,7 +1151,7 @@ CmdResult loadGnssSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GNSS_SOURCE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssSource(C::mip_interface& device) +TypedResult defaultGnssSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1193,7 +1193,7 @@ void extract(Serializer& serializer, HeadingSource::Response& self) } -CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source) +TypedResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1205,7 +1205,7 @@ CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source sou return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut) +TypedResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1214,7 +1214,7 @@ CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sou assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_UPDATE_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HEADING_UPDATE_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1228,7 +1228,7 @@ CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sou } return result; } -CmdResult saveHeadingSource(C::mip_interface& device) +TypedResult saveHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1238,7 +1238,7 @@ CmdResult saveHeadingSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadHeadingSource(C::mip_interface& device) +TypedResult loadHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1248,7 +1248,7 @@ CmdResult loadHeadingSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEADING_UPDATE_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultHeadingSource(C::mip_interface& device) +TypedResult defaultHeadingSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1290,7 +1290,7 @@ void extract(Serializer& serializer, AutoInitControl::Response& self) } -CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) +TypedResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1302,7 +1302,7 @@ CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) +TypedResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1311,7 +1311,7 @@ CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AUTOINIT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AUTOINIT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1325,7 +1325,7 @@ CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut) } return result; } -CmdResult saveAutoInitControl(C::mip_interface& device) +TypedResult saveAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1335,7 +1335,7 @@ CmdResult saveAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoInitControl(C::mip_interface& device) +TypedResult loadAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1345,7 +1345,7 @@ CmdResult loadAutoInitControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AUTOINIT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoInitControl(C::mip_interface& device) +TypedResult defaultAutoInitControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1391,7 +1391,7 @@ void extract(Serializer& serializer, AccelNoise::Response& self) } -CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) +TypedResult writeAccelNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1405,7 +1405,7 @@ CmdResult writeAccelNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) +TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1414,7 +1414,7 @@ CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1429,7 +1429,7 @@ CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveAccelNoise(C::mip_interface& device) +TypedResult saveAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1439,7 +1439,7 @@ CmdResult saveAccelNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelNoise(C::mip_interface& device) +TypedResult loadAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1449,7 +1449,7 @@ CmdResult loadAccelNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelNoise(C::mip_interface& device) +TypedResult defaultAccelNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1495,7 +1495,7 @@ void extract(Serializer& serializer, GyroNoise::Response& self) } -CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) +TypedResult writeGyroNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1509,7 +1509,7 @@ CmdResult writeGyroNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) +TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1518,7 +1518,7 @@ CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1533,7 +1533,7 @@ CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveGyroNoise(C::mip_interface& device) +TypedResult saveGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1543,7 +1543,7 @@ CmdResult saveGyroNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGyroNoise(C::mip_interface& device) +TypedResult loadGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1553,7 +1553,7 @@ CmdResult loadGyroNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGyroNoise(C::mip_interface& device) +TypedResult defaultGyroNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1611,7 +1611,7 @@ void extract(Serializer& serializer, AccelBiasModel::Response& self) } -CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) +TypedResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1629,7 +1629,7 @@ CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +TypedResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1638,7 +1638,7 @@ CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* no assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_BIAS_MODEL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1657,7 +1657,7 @@ CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* no } return result; } -CmdResult saveAccelBiasModel(C::mip_interface& device) +TypedResult saveAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1667,7 +1667,7 @@ CmdResult saveAccelBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelBiasModel(C::mip_interface& device) +TypedResult loadAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1677,7 +1677,7 @@ CmdResult loadAccelBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelBiasModel(C::mip_interface& device) +TypedResult defaultAccelBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1735,7 +1735,7 @@ void extract(Serializer& serializer, GyroBiasModel::Response& self) } -CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) +TypedResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1753,7 +1753,7 @@ CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) +TypedResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1762,7 +1762,7 @@ CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noi assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_BIAS_MODEL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1781,7 +1781,7 @@ CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noi } return result; } -CmdResult saveGyroBiasModel(C::mip_interface& device) +TypedResult saveGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1791,7 +1791,7 @@ CmdResult saveGyroBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGyroBiasModel(C::mip_interface& device) +TypedResult loadGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1801,7 +1801,7 @@ CmdResult loadGyroBiasModel(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_BIAS_MODEL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGyroBiasModel(C::mip_interface& device) +TypedResult defaultGyroBiasModel(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1843,7 +1843,7 @@ void extract(Serializer& serializer, AltitudeAiding::Response& self) } -CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) +TypedResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1855,7 +1855,7 @@ CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSe return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) +TypedResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1864,7 +1864,7 @@ CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSel assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ALTITUDE_AIDING_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1878,7 +1878,7 @@ CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSel } return result; } -CmdResult saveAltitudeAiding(C::mip_interface& device) +TypedResult saveAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1888,7 +1888,7 @@ CmdResult saveAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAltitudeAiding(C::mip_interface& device) +TypedResult loadAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1898,7 +1898,7 @@ CmdResult loadAltitudeAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ALTITUDE_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAltitudeAiding(C::mip_interface& device) +TypedResult defaultAltitudeAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1940,7 +1940,7 @@ void extract(Serializer& serializer, PitchRollAiding::Response& self) } -CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) +TypedResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1952,7 +1952,7 @@ CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::Aiding return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) +TypedResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1961,7 +1961,7 @@ CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingS assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -1975,7 +1975,7 @@ CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingS } return result; } -CmdResult savePitchRollAiding(C::mip_interface& device) +TypedResult savePitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1985,7 +1985,7 @@ CmdResult savePitchRollAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadPitchRollAiding(C::mip_interface& device) +TypedResult loadPitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -1995,7 +1995,7 @@ CmdResult loadPitchRollAiding(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultPitchRollAiding(C::mip_interface& device) +TypedResult defaultPitchRollAiding(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2045,7 +2045,7 @@ void extract(Serializer& serializer, AutoZupt::Response& self) } -CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) +TypedResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2059,7 +2059,7 @@ CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshol return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2068,7 +2068,7 @@ CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thre assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ZUPT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2085,7 +2085,7 @@ CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thre } return result; } -CmdResult saveAutoZupt(C::mip_interface& device) +TypedResult saveAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2095,7 +2095,7 @@ CmdResult saveAutoZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoZupt(C::mip_interface& device) +TypedResult loadAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2105,7 +2105,7 @@ CmdResult loadAutoZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoZupt(C::mip_interface& device) +TypedResult defaultAutoZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2155,7 +2155,7 @@ void extract(Serializer& serializer, AutoAngularZupt::Response& self) } -CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) +TypedResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2169,7 +2169,7 @@ CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) +TypedResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2178,7 +2178,7 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANGULAR_ZUPT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2195,7 +2195,7 @@ CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, floa } return result; } -CmdResult saveAutoAngularZupt(C::mip_interface& device) +TypedResult saveAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2205,7 +2205,7 @@ CmdResult saveAutoAngularZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAutoAngularZupt(C::mip_interface& device) +TypedResult loadAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2215,7 +2215,7 @@ CmdResult loadAutoAngularZupt(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANGULAR_ZUPT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAutoAngularZupt(C::mip_interface& device) +TypedResult defaultAutoAngularZupt(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2236,7 +2236,7 @@ void extract(Serializer& serializer, CommandedZupt& self) (void)self; } -CmdResult commandedZupt(C::mip_interface& device) +TypedResult commandedZupt(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ZUPT, NULL, 0); } @@ -2251,7 +2251,7 @@ void extract(Serializer& serializer, CommandedAngularZupt& self) (void)self; } -CmdResult commandedAngularZupt(C::mip_interface& device) +TypedResult commandedAngularZupt(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COMMANDED_ANGULAR_ZUPT, NULL, 0); } @@ -2266,7 +2266,7 @@ void extract(Serializer& serializer, MagCaptureAutoCal& self) } -CmdResult writeMagCaptureAutoCal(C::mip_interface& device) +TypedResult writeMagCaptureAutoCal(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2276,7 +2276,7 @@ CmdResult writeMagCaptureAutoCal(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_CAPTURE_AUTO_CALIBRATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult saveMagCaptureAutoCal(C::mip_interface& device) +TypedResult saveMagCaptureAutoCal(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2322,7 +2322,7 @@ void extract(Serializer& serializer, GravityNoise::Response& self) } -CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) +TypedResult writeGravityNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2336,7 +2336,7 @@ CmdResult writeGravityNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) +TypedResult readGravityNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2345,7 +2345,7 @@ CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GRAVITY_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2360,7 +2360,7 @@ CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveGravityNoise(C::mip_interface& device) +TypedResult saveGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2370,7 +2370,7 @@ CmdResult saveGravityNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGravityNoise(C::mip_interface& device) +TypedResult loadGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2380,7 +2380,7 @@ CmdResult loadGravityNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GRAVITY_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGravityNoise(C::mip_interface& device) +TypedResult defaultGravityNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2422,7 +2422,7 @@ void extract(Serializer& serializer, PressureAltitudeNoise::Response& self) } -CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) +TypedResult writePressureAltitudeNoise(C::mip_interface& device, float noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2434,7 +2434,7 @@ CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) +TypedResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2443,7 +2443,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_PRESSURE_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2457,7 +2457,7 @@ CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult savePressureAltitudeNoise(C::mip_interface& device) +TypedResult savePressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2467,7 +2467,7 @@ CmdResult savePressureAltitudeNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadPressureAltitudeNoise(C::mip_interface& device) +TypedResult loadPressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2477,7 +2477,7 @@ CmdResult loadPressureAltitudeNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultPressureAltitudeNoise(C::mip_interface& device) +TypedResult defaultPressureAltitudeNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2523,7 +2523,7 @@ void extract(Serializer& serializer, HardIronOffsetNoise::Response& self) } -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) +TypedResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2537,7 +2537,7 @@ CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) +TypedResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2546,7 +2546,7 @@ CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_HARD_IRON_OFFSET_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2561,7 +2561,7 @@ CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveHardIronOffsetNoise(C::mip_interface& device) +TypedResult saveHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2571,7 +2571,7 @@ CmdResult saveHardIronOffsetNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadHardIronOffsetNoise(C::mip_interface& device) +TypedResult loadHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2581,7 +2581,7 @@ CmdResult loadHardIronOffsetNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HARD_IRON_OFFSET_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device) +TypedResult defaultHardIronOffsetNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2627,7 +2627,7 @@ void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self) } -CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) +TypedResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2641,7 +2641,7 @@ CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) +TypedResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2650,7 +2650,7 @@ CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SOFT_IRON_MATRIX_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2665,7 +2665,7 @@ CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) +TypedResult saveSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2675,7 +2675,7 @@ CmdResult saveSoftIronMatrixNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) +TypedResult loadSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2685,7 +2685,7 @@ CmdResult loadSoftIronMatrixNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SOFT_IRON_MATRIX_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device) +TypedResult defaultSoftIronMatrixNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2731,7 +2731,7 @@ void extract(Serializer& serializer, MagNoise::Response& self) } -CmdResult writeMagNoise(C::mip_interface& device, const float* noise) +TypedResult writeMagNoise(C::mip_interface& device, const float* noise) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2745,7 +2745,7 @@ CmdResult writeMagNoise(C::mip_interface& device, const float* noise) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) +TypedResult readMagNoise(C::mip_interface& device, float* noiseOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2754,7 +2754,7 @@ CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_NOISE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2769,7 +2769,7 @@ CmdResult readMagNoise(C::mip_interface& device, float* noiseOut) } return result; } -CmdResult saveMagNoise(C::mip_interface& device) +TypedResult saveMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2779,7 +2779,7 @@ CmdResult saveMagNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagNoise(C::mip_interface& device) +TypedResult loadMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2789,7 +2789,7 @@ CmdResult loadMagNoise(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_NOISE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagNoise(C::mip_interface& device) +TypedResult defaultMagNoise(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2839,7 +2839,7 @@ void extract(Serializer& serializer, InclinationSource::Response& self) } -CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) +TypedResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2853,7 +2853,7 @@ CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) +TypedResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2862,7 +2862,7 @@ CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INCLINATION_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2879,7 +2879,7 @@ CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* } return result; } -CmdResult saveInclinationSource(C::mip_interface& device) +TypedResult saveInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2889,7 +2889,7 @@ CmdResult saveInclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadInclinationSource(C::mip_interface& device) +TypedResult loadInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2899,7 +2899,7 @@ CmdResult loadInclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INCLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultInclinationSource(C::mip_interface& device) +TypedResult defaultInclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2949,7 +2949,7 @@ void extract(Serializer& serializer, MagneticDeclinationSource::Response& self) } -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) +TypedResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2963,7 +2963,7 @@ CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagPara return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) +TypedResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2972,7 +2972,7 @@ CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParam assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_DECLINATION_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -2989,7 +2989,7 @@ CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParam } return result; } -CmdResult saveMagneticDeclinationSource(C::mip_interface& device) +TypedResult saveMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -2999,7 +2999,7 @@ CmdResult saveMagneticDeclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagneticDeclinationSource(C::mip_interface& device) +TypedResult loadMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3009,7 +3009,7 @@ CmdResult loadMagneticDeclinationSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_DECLINATION_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device) +TypedResult defaultMagneticDeclinationSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3059,7 +3059,7 @@ void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self) } -CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) +TypedResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3073,7 +3073,7 @@ CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamS return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) +TypedResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3082,7 +3082,7 @@ CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSo assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAGNETIC_MAGNITUDE_SOURCE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3099,7 +3099,7 @@ CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSo } return result; } -CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) +TypedResult saveMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3109,7 +3109,7 @@ CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) +TypedResult loadMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3119,7 +3119,7 @@ CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_MAGNITUDE_SOURCE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device) +TypedResult defaultMagFieldMagnitudeSource(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3185,7 +3185,7 @@ void extract(Serializer& serializer, ReferencePosition::Response& self) } -CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) +TypedResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3203,7 +3203,7 @@ CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double l return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) +TypedResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3212,7 +3212,7 @@ CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, doubl assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REFERENCE_POSITION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3235,7 +3235,7 @@ CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, doubl } return result; } -CmdResult saveReferencePosition(C::mip_interface& device) +TypedResult saveReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3245,7 +3245,7 @@ CmdResult saveReferencePosition(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadReferencePosition(C::mip_interface& device) +TypedResult loadReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3255,7 +3255,7 @@ CmdResult loadReferencePosition(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REFERENCE_POSITION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultReferencePosition(C::mip_interface& device) +TypedResult defaultReferencePosition(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3345,7 +3345,7 @@ void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Res } -CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) +TypedResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3369,7 +3369,7 @@ CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +TypedResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3378,7 +3378,7 @@ CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, F assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3410,7 +3410,7 @@ CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, F } return result; } -CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3420,7 +3420,7 @@ CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3430,7 +3430,7 @@ CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3520,7 +3520,7 @@ void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Respo } -CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) +TypedResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3544,7 +3544,7 @@ CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, Fi return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +TypedResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3553,7 +3553,7 @@ CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, Fil assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3585,7 +3585,7 @@ CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, Fil } return result; } -CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3595,7 +3595,7 @@ CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3605,7 +3605,7 @@ CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3679,7 +3679,7 @@ void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Respon } -CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) +TypedResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3699,7 +3699,7 @@ CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, boo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) +TypedResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3708,7 +3708,7 @@ CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3734,7 +3734,7 @@ CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool } return result; } -CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3744,7 +3744,7 @@ CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3754,7 +3754,7 @@ CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) +TypedResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3804,7 +3804,7 @@ void extract(Serializer& serializer, AidingMeasurementEnable::Response& self) } -CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable) +TypedResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3818,7 +3818,7 @@ CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasureme return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut) +TypedResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3829,7 +3829,7 @@ CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AIDING_MEASUREMENT_ENABLE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_AIDING_MEASUREMENT_ENABLE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3845,7 +3845,7 @@ CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen } return result; } -CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +TypedResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3857,7 +3857,7 @@ CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +TypedResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3869,7 +3869,7 @@ CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasuremen return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_AIDING_MEASUREMENT_ENABLE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) +TypedResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3892,7 +3892,7 @@ void extract(Serializer& serializer, Run& self) (void)self; } -CmdResult run(C::mip_interface& device) +TypedResult run(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RUN, NULL, 0); } @@ -3944,7 +3944,7 @@ void extract(Serializer& serializer, KinematicConstraint::Response& self) } -CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection) +TypedResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3960,7 +3960,7 @@ CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t acceleratio return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut) +TypedResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3969,7 +3969,7 @@ CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* acceleratio assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_KINEMATIC_CONSTRAINT, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_KINEMATIC_CONSTRAINT, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -3989,7 +3989,7 @@ CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* acceleratio } return result; } -CmdResult saveKinematicConstraint(C::mip_interface& device) +TypedResult saveKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -3999,7 +3999,7 @@ CmdResult saveKinematicConstraint(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadKinematicConstraint(C::mip_interface& device) +TypedResult loadKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4009,7 +4009,7 @@ CmdResult loadKinematicConstraint(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_KINEMATIC_CONSTRAINT, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultKinematicConstraint(C::mip_interface& device) +TypedResult defaultKinematicConstraint(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4123,7 +4123,7 @@ void extract(Serializer& serializer, InitializationConfiguration::Response& self } -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) +TypedResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4155,7 +4155,7 @@ CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t wai return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) +TypedResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4164,7 +4164,7 @@ CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* wai assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INITIALIZATION_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_INITIALIZATION_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4204,7 +4204,7 @@ CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* wai } return result; } -CmdResult saveInitializationConfiguration(C::mip_interface& device) +TypedResult saveInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4214,7 +4214,7 @@ CmdResult saveInitializationConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadInitializationConfiguration(C::mip_interface& device) +TypedResult loadInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4224,7 +4224,7 @@ CmdResult loadInitializationConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_INITIALIZATION_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultInitializationConfiguration(C::mip_interface& device) +TypedResult defaultInitializationConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4274,7 +4274,7 @@ void extract(Serializer& serializer, AdaptiveFilterOptions::Response& self) } -CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit) +TypedResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4288,7 +4288,7 @@ CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, ui return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut) +TypedResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4297,7 +4297,7 @@ CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADAPTIVE_FILTER_OPTIONS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ADAPTIVE_FILTER_OPTIONS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4314,7 +4314,7 @@ CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, } return result; } -CmdResult saveAdaptiveFilterOptions(C::mip_interface& device) +TypedResult saveAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4324,7 +4324,7 @@ CmdResult saveAdaptiveFilterOptions(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadAdaptiveFilterOptions(C::mip_interface& device) +TypedResult loadAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4334,7 +4334,7 @@ CmdResult loadAdaptiveFilterOptions(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ADAPTIVE_FILTER_OPTIONS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device) +TypedResult defaultAdaptiveFilterOptions(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4388,7 +4388,7 @@ void extract(Serializer& serializer, MultiAntennaOffset::Response& self) } -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) +TypedResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4404,7 +4404,7 @@ CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) +TypedResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4415,7 +4415,7 @@ CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, f assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MULTI_ANTENNA_OFFSET, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_MULTI_ANTENNA_OFFSET, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4432,7 +4432,7 @@ CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, f } return result; } -CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +TypedResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4444,7 +4444,7 @@ CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +TypedResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4456,7 +4456,7 @@ CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MULTI_ANTENNA_OFFSET, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) +TypedResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4520,7 +4520,7 @@ void extract(Serializer& serializer, RelPosConfiguration::Response& self) } -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) +TypedResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4538,7 +4538,7 @@ CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, Fil return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) +TypedResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4547,7 +4547,7 @@ CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REL_POS_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REL_POS_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4568,7 +4568,7 @@ CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, } return result; } -CmdResult saveRelPosConfiguration(C::mip_interface& device) +TypedResult saveRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4578,7 +4578,7 @@ CmdResult saveRelPosConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadRelPosConfiguration(C::mip_interface& device) +TypedResult loadRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4588,7 +4588,7 @@ CmdResult loadRelPosConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REL_POS_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultRelPosConfiguration(C::mip_interface& device) +TypedResult defaultRelPosConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4642,7 +4642,7 @@ void extract(Serializer& serializer, RefPointLeverArm::Response& self) } -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) +TypedResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4658,7 +4658,7 @@ CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refe return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) +TypedResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4667,7 +4667,7 @@ CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refer assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REF_POINT_LEVER_ARM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_REF_POINT_LEVER_ARM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4685,7 +4685,7 @@ CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::Refer } return result; } -CmdResult saveRefPointLeverArm(C::mip_interface& device) +TypedResult saveRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4695,7 +4695,7 @@ CmdResult saveRefPointLeverArm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadRefPointLeverArm(C::mip_interface& device) +TypedResult loadRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4705,7 +4705,7 @@ CmdResult loadRefPointLeverArm(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_REF_POINT_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultRefPointLeverArm(C::mip_interface& device) +TypedResult defaultRefPointLeverArm(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4738,7 +4738,7 @@ void extract(Serializer& serializer, SpeedMeasurement& self) } -CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty) +TypedResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4799,7 +4799,7 @@ void extract(Serializer& serializer, SpeedLeverArm::Response& self) } -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) +TypedResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4815,7 +4815,7 @@ CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const flo return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) +TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4826,7 +4826,7 @@ CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* lev assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPEED_LEVER_ARM, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SPEED_LEVER_ARM, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4843,7 +4843,7 @@ CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* lev } return result; } -CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) +TypedResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4855,7 +4855,7 @@ CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) +TypedResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4867,7 +4867,7 @@ CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SPEED_LEVER_ARM, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source) +TypedResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4911,7 +4911,7 @@ void extract(Serializer& serializer, WheeledVehicleConstraintControl::Response& } -CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable) +TypedResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4923,7 +4923,7 @@ CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut) +TypedResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4932,7 +4932,7 @@ CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_CONSTRAINT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_VEHICLE_CONSTRAINT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -4946,7 +4946,7 @@ CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* } return result; } -CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device) +TypedResult saveWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4956,7 +4956,7 @@ CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device) +TypedResult loadWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -4966,7 +4966,7 @@ CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEHICLE_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device) +TypedResult defaultWheeledVehicleConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5008,7 +5008,7 @@ void extract(Serializer& serializer, VerticalGyroConstraintControl::Response& se } -CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable) +TypedResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5020,7 +5020,7 @@ CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t e return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut) +TypedResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5029,7 +5029,7 @@ CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* e assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_CONSTRAINT_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_GYRO_CONSTRAINT_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -5043,7 +5043,7 @@ CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* e } return result; } -CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device) +TypedResult saveVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5053,7 +5053,7 @@ CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device) +TypedResult loadVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5063,7 +5063,7 @@ CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GYRO_CONSTRAINT_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device) +TypedResult defaultVerticalGyroConstraintControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5113,7 +5113,7 @@ void extract(Serializer& serializer, GnssAntennaCalControl::Response& self) } -CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset) +TypedResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5127,7 +5127,7 @@ CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut) +TypedResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5136,7 +5136,7 @@ CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_CALIBRATION_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ANTENNA_CALIBRATION_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -5153,7 +5153,7 @@ CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut } return result; } -CmdResult saveGnssAntennaCalControl(C::mip_interface& device) +TypedResult saveGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5163,7 +5163,7 @@ CmdResult saveGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadGnssAntennaCalControl(C::mip_interface& device) +TypedResult loadGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5173,7 +5173,7 @@ CmdResult loadGnssAntennaCalControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ANTENNA_CALIBRATION_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultGnssAntennaCalControl(C::mip_interface& device) +TypedResult defaultGnssAntennaCalControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -5194,7 +5194,7 @@ void extract(Serializer& serializer, SetInitialHeading& self) } -CmdResult setInitialHeading(C::mip_interface& device, float heading) +TypedResult setInitialHeading(C::mip_interface& device, float heading) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index bac15248d..8f54608a1 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -195,6 +195,8 @@ struct Reset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RESET_FILTER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Reset"; + static constexpr const char* DOC_NAME = "Reset Navigation Filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -213,7 +215,7 @@ struct Reset void insert(Serializer& serializer, const Reset& self); void extract(Serializer& serializer, Reset& self); -CmdResult reset(C::mip_interface& device); +TypedResult reset(C::mip_interface& device); ///@} /// @@ -243,6 +245,8 @@ struct SetInitialAttitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_ATTITUDE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SetInitialAttitude"; + static constexpr const char* DOC_NAME = "Set Initial Attitude"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -261,7 +265,7 @@ struct SetInitialAttitude void insert(Serializer& serializer, const SetInitialAttitude& self); void extract(Serializer& serializer, SetInitialAttitude& self); -CmdResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading); +TypedResult setInitialAttitude(C::mip_interface& device, float roll, float pitch, float heading); ///@} /// @@ -303,7 +307,7 @@ struct EstimationControl EnableFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } EnableFlags& operator=(uint16_t val) { value = val; return *this; } - EnableFlags& operator=(int val) { value = val; return *this; } + EnableFlags& operator=(int val) { value = uint16_t(val); return *this; } EnableFlags& operator|=(uint16_t val) { return *this = value | val; } EnableFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -332,6 +336,8 @@ struct EstimationControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ESTIMATION_CONTROL_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EstimationControl"; + static constexpr const char* DOC_NAME = "Estimation Control Flags"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -364,6 +370,8 @@ struct EstimationControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ESTIMATION_CONTROL_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EstimationControl::Response"; + static constexpr const char* DOC_NAME = "Estimation Control Flags Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -382,11 +390,11 @@ void extract(Serializer& serializer, EstimationControl& self); void insert(Serializer& serializer, const EstimationControl::Response& self); void extract(Serializer& serializer, EstimationControl::Response& self); -CmdResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable); -CmdResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut); -CmdResult saveEstimationControl(C::mip_interface& device); -CmdResult loadEstimationControl(C::mip_interface& device); -CmdResult defaultEstimationControl(C::mip_interface& device); +TypedResult writeEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags enable); +TypedResult readEstimationControl(C::mip_interface& device, EstimationControl::EnableFlags* enableOut); +TypedResult saveEstimationControl(C::mip_interface& device); +TypedResult loadEstimationControl(C::mip_interface& device); +TypedResult defaultEstimationControl(C::mip_interface& device); ///@} /// @@ -414,6 +422,8 @@ struct ExternalGnssUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_GNSS_UPDATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalGnssUpdate"; + static constexpr const char* DOC_NAME = "External GNSS Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -432,7 +442,7 @@ struct ExternalGnssUpdate void insert(Serializer& serializer, const ExternalGnssUpdate& self); void extract(Serializer& serializer, ExternalGnssUpdate& self); -CmdResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); +TypedResult externalGnssUpdate(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, double latitude, double longitude, double height, const float* velocity, const float* posUncertainty, const float* velUncertainty); ///@} /// @@ -463,6 +473,8 @@ struct ExternalHeadingUpdate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalHeadingUpdate"; + static constexpr const char* DOC_NAME = "External Heading Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -481,7 +493,7 @@ struct ExternalHeadingUpdate void insert(Serializer& serializer, const ExternalHeadingUpdate& self); void extract(Serializer& serializer, ExternalHeadingUpdate& self); -CmdResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type); +TypedResult externalHeadingUpdate(C::mip_interface& device, float heading, float headingUncertainty, uint8_t type); ///@} /// @@ -518,6 +530,8 @@ struct ExternalHeadingUpdateWithTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_EXTERNAL_HEADING_UPDATE_WITH_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalHeadingUpdateWithTime"; + static constexpr const char* DOC_NAME = "External Heading Update With Time"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -536,7 +550,7 @@ struct ExternalHeadingUpdateWithTime void insert(Serializer& serializer, const ExternalHeadingUpdateWithTime& self); void extract(Serializer& serializer, ExternalHeadingUpdateWithTime& self); -CmdResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type); +TypedResult externalHeadingUpdateWithTime(C::mip_interface& device, double gpsTime, uint16_t gpsWeek, float heading, float headingUncertainty, uint8_t type); ///@} /// @@ -568,7 +582,7 @@ struct TareOrientation MipTareAxes(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } MipTareAxes& operator=(uint8_t val) { value = val; return *this; } - MipTareAxes& operator=(int val) { value = val; return *this; } + MipTareAxes& operator=(int val) { value = uint8_t(val); return *this; } MipTareAxes& operator|=(uint8_t val) { return *this = value | val; } MipTareAxes& operator&=(uint8_t val) { return *this = value & val; } @@ -589,6 +603,8 @@ struct TareOrientation static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_TARE_ORIENTATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TareOrientation"; + static constexpr const char* DOC_NAME = "Tare Sensor Orientation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -621,6 +637,8 @@ struct TareOrientation static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_TARE_ORIENTATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TareOrientation::Response"; + static constexpr const char* DOC_NAME = "Tare Sensor Orientation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -639,11 +657,11 @@ void extract(Serializer& serializer, TareOrientation& self); void insert(Serializer& serializer, const TareOrientation::Response& self); void extract(Serializer& serializer, TareOrientation::Response& self); -CmdResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes); -CmdResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut); -CmdResult saveTareOrientation(C::mip_interface& device); -CmdResult loadTareOrientation(C::mip_interface& device); -CmdResult defaultTareOrientation(C::mip_interface& device); +TypedResult writeTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes axes); +TypedResult readTareOrientation(C::mip_interface& device, TareOrientation::MipTareAxes* axesOut); +TypedResult saveTareOrientation(C::mip_interface& device); +TypedResult loadTareOrientation(C::mip_interface& device); +TypedResult defaultTareOrientation(C::mip_interface& device); ///@} /// @@ -669,6 +687,8 @@ struct VehicleDynamicsMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_DYNAMICS_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VehicleDynamicsMode"; + static constexpr const char* DOC_NAME = "Vehicle Dynamics Mode"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -701,6 +721,8 @@ struct VehicleDynamicsMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_DYNAMICS_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VehicleDynamicsMode::Response"; + static constexpr const char* DOC_NAME = "Vehicle Dynamics Mode Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -719,11 +741,11 @@ void extract(Serializer& serializer, VehicleDynamicsMode& self); void insert(Serializer& serializer, const VehicleDynamicsMode::Response& self); void extract(Serializer& serializer, VehicleDynamicsMode::Response& self); -CmdResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); -CmdResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); -CmdResult saveVehicleDynamicsMode(C::mip_interface& device); -CmdResult loadVehicleDynamicsMode(C::mip_interface& device); -CmdResult defaultVehicleDynamicsMode(C::mip_interface& device); +TypedResult writeVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode mode); +TypedResult readVehicleDynamicsMode(C::mip_interface& device, VehicleDynamicsMode::DynamicsMode* modeOut); +TypedResult saveVehicleDynamicsMode(C::mip_interface& device); +TypedResult loadVehicleDynamicsMode(C::mip_interface& device); +TypedResult defaultVehicleDynamicsMode(C::mip_interface& device); ///@} /// @@ -765,6 +787,8 @@ struct SensorToVehicleRotationEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationEuler"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Euler"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -797,6 +821,8 @@ struct SensorToVehicleRotationEuler static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationEuler::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Euler Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -817,11 +843,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationEuler& self); void insert(Serializer& serializer, const SensorToVehicleRotationEuler::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationEuler::Response& self); -CmdResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw); -CmdResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); -CmdResult saveSensorToVehicleRotationEuler(C::mip_interface& device); -CmdResult loadSensorToVehicleRotationEuler(C::mip_interface& device); -CmdResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); +TypedResult writeSensorToVehicleRotationEuler(C::mip_interface& device, float roll, float pitch, float yaw); +TypedResult readSensorToVehicleRotationEuler(C::mip_interface& device, float* rollOut, float* pitchOut, float* yawOut); +TypedResult saveSensorToVehicleRotationEuler(C::mip_interface& device); +TypedResult loadSensorToVehicleRotationEuler(C::mip_interface& device); +TypedResult defaultSensorToVehicleRotationEuler(C::mip_interface& device); ///@} /// @@ -867,6 +893,8 @@ struct SensorToVehicleRotationDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationDcm"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation DCM"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -899,6 +927,8 @@ struct SensorToVehicleRotationDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_DCM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationDcm::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation DCM Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -917,11 +947,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationDcm& self); void insert(Serializer& serializer, const SensorToVehicleRotationDcm::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationDcm::Response& self); -CmdResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); -CmdResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); -CmdResult saveSensorToVehicleRotationDcm(C::mip_interface& device); -CmdResult loadSensorToVehicleRotationDcm(C::mip_interface& device); -CmdResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); +TypedResult writeSensorToVehicleRotationDcm(C::mip_interface& device, const float* dcm); +TypedResult readSensorToVehicleRotationDcm(C::mip_interface& device, float* dcmOut); +TypedResult saveSensorToVehicleRotationDcm(C::mip_interface& device); +TypedResult loadSensorToVehicleRotationDcm(C::mip_interface& device); +TypedResult defaultSensorToVehicleRotationDcm(C::mip_interface& device); ///@} /// @@ -966,6 +996,8 @@ struct SensorToVehicleRotationQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_ROTATION_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationQuaternion"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Quaternion"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -998,6 +1030,8 @@ struct SensorToVehicleRotationQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_ROTATION_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleRotationQuaternion::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Rotation Quaternion Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1016,11 +1050,11 @@ void extract(Serializer& serializer, SensorToVehicleRotationQuaternion& self); void insert(Serializer& serializer, const SensorToVehicleRotationQuaternion::Response& self); void extract(Serializer& serializer, SensorToVehicleRotationQuaternion::Response& self); -CmdResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); -CmdResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); -CmdResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); -CmdResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); -CmdResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); +TypedResult writeSensorToVehicleRotationQuaternion(C::mip_interface& device, const float* quat); +TypedResult readSensorToVehicleRotationQuaternion(C::mip_interface& device, float* quatOut); +TypedResult saveSensorToVehicleRotationQuaternion(C::mip_interface& device); +TypedResult loadSensorToVehicleRotationQuaternion(C::mip_interface& device); +TypedResult defaultSensorToVehicleRotationQuaternion(C::mip_interface& device); ///@} /// @@ -1046,6 +1080,8 @@ struct SensorToVehicleOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SENSOR2VEHICLE_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleOffset"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Offset"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1078,6 +1114,8 @@ struct SensorToVehicleOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SENSOR2VEHICLE_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SensorToVehicleOffset::Response"; + static constexpr const char* DOC_NAME = "Sensor to Vehicle Frame Offset Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1096,11 +1134,11 @@ void extract(Serializer& serializer, SensorToVehicleOffset& self); void insert(Serializer& serializer, const SensorToVehicleOffset::Response& self); void extract(Serializer& serializer, SensorToVehicleOffset::Response& self); -CmdResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); -CmdResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); -CmdResult saveSensorToVehicleOffset(C::mip_interface& device); -CmdResult loadSensorToVehicleOffset(C::mip_interface& device); -CmdResult defaultSensorToVehicleOffset(C::mip_interface& device); +TypedResult writeSensorToVehicleOffset(C::mip_interface& device, const float* offset); +TypedResult readSensorToVehicleOffset(C::mip_interface& device, float* offsetOut); +TypedResult saveSensorToVehicleOffset(C::mip_interface& device); +TypedResult loadSensorToVehicleOffset(C::mip_interface& device); +TypedResult defaultSensorToVehicleOffset(C::mip_interface& device); ///@} /// @@ -1123,6 +1161,8 @@ struct AntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffset"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1155,6 +1195,8 @@ struct AntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffset::Response"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1173,11 +1215,11 @@ void extract(Serializer& serializer, AntennaOffset& self); void insert(Serializer& serializer, const AntennaOffset::Response& self); void extract(Serializer& serializer, AntennaOffset::Response& self); -CmdResult writeAntennaOffset(C::mip_interface& device, const float* offset); -CmdResult readAntennaOffset(C::mip_interface& device, float* offsetOut); -CmdResult saveAntennaOffset(C::mip_interface& device); -CmdResult loadAntennaOffset(C::mip_interface& device); -CmdResult defaultAntennaOffset(C::mip_interface& device); +TypedResult writeAntennaOffset(C::mip_interface& device, const float* offset); +TypedResult readAntennaOffset(C::mip_interface& device, float* offsetOut); +TypedResult saveAntennaOffset(C::mip_interface& device); +TypedResult loadAntennaOffset(C::mip_interface& device); +TypedResult defaultAntennaOffset(C::mip_interface& device); ///@} /// @@ -1207,6 +1249,8 @@ struct GnssSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GNSS_SOURCE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSource"; + static constexpr const char* DOC_NAME = "GNSS Aiding Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1239,6 +1283,8 @@ struct GnssSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GNSS_SOURCE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssSource::Response"; + static constexpr const char* DOC_NAME = "GNSS Aiding Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1257,11 +1303,11 @@ void extract(Serializer& serializer, GnssSource& self); void insert(Serializer& serializer, const GnssSource::Response& self); void extract(Serializer& serializer, GnssSource::Response& self); -CmdResult writeGnssSource(C::mip_interface& device, GnssSource::Source source); -CmdResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut); -CmdResult saveGnssSource(C::mip_interface& device); -CmdResult loadGnssSource(C::mip_interface& device); -CmdResult defaultGnssSource(C::mip_interface& device); +TypedResult writeGnssSource(C::mip_interface& device, GnssSource::Source source); +TypedResult readGnssSource(C::mip_interface& device, GnssSource::Source* sourceOut); +TypedResult saveGnssSource(C::mip_interface& device); +TypedResult loadGnssSource(C::mip_interface& device); +TypedResult defaultGnssSource(C::mip_interface& device); ///@} /// @@ -1302,6 +1348,8 @@ struct HeadingSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HEADING_UPDATE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadingSource"; + static constexpr const char* DOC_NAME = "Heading Aiding Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1334,6 +1382,8 @@ struct HeadingSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HEADING_UPDATE_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadingSource::Response"; + static constexpr const char* DOC_NAME = "Heading Aiding Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1352,11 +1402,11 @@ void extract(Serializer& serializer, HeadingSource& self); void insert(Serializer& serializer, const HeadingSource::Response& self); void extract(Serializer& serializer, HeadingSource::Response& self); -CmdResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source); -CmdResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut); -CmdResult saveHeadingSource(C::mip_interface& device); -CmdResult loadHeadingSource(C::mip_interface& device); -CmdResult defaultHeadingSource(C::mip_interface& device); +TypedResult writeHeadingSource(C::mip_interface& device, HeadingSource::Source source); +TypedResult readHeadingSource(C::mip_interface& device, HeadingSource::Source* sourceOut); +TypedResult saveHeadingSource(C::mip_interface& device); +TypedResult loadHeadingSource(C::mip_interface& device); +TypedResult defaultHeadingSource(C::mip_interface& device); ///@} /// @@ -1382,6 +1432,8 @@ struct AutoInitControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AUTOINIT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoInitControl"; + static constexpr const char* DOC_NAME = "Auto-initialization Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1414,6 +1466,8 @@ struct AutoInitControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AUTOINIT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoInitControl::Response"; + static constexpr const char* DOC_NAME = "Auto-initialization Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1432,11 +1486,11 @@ void extract(Serializer& serializer, AutoInitControl& self); void insert(Serializer& serializer, const AutoInitControl::Response& self); void extract(Serializer& serializer, AutoInitControl::Response& self); -CmdResult writeAutoInitControl(C::mip_interface& device, uint8_t enable); -CmdResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut); -CmdResult saveAutoInitControl(C::mip_interface& device); -CmdResult loadAutoInitControl(C::mip_interface& device); -CmdResult defaultAutoInitControl(C::mip_interface& device); +TypedResult writeAutoInitControl(C::mip_interface& device, uint8_t enable); +TypedResult readAutoInitControl(C::mip_interface& device, uint8_t* enableOut); +TypedResult saveAutoInitControl(C::mip_interface& device); +TypedResult loadAutoInitControl(C::mip_interface& device); +TypedResult defaultAutoInitControl(C::mip_interface& device); ///@} /// @@ -1460,6 +1514,8 @@ struct AccelNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelNoise"; + static constexpr const char* DOC_NAME = "Accelerometer Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1492,6 +1548,8 @@ struct AccelNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelNoise::Response"; + static constexpr const char* DOC_NAME = "Accelerometer Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1510,11 +1568,11 @@ void extract(Serializer& serializer, AccelNoise& self); void insert(Serializer& serializer, const AccelNoise::Response& self); void extract(Serializer& serializer, AccelNoise::Response& self); -CmdResult writeAccelNoise(C::mip_interface& device, const float* noise); -CmdResult readAccelNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveAccelNoise(C::mip_interface& device); -CmdResult loadAccelNoise(C::mip_interface& device); -CmdResult defaultAccelNoise(C::mip_interface& device); +TypedResult writeAccelNoise(C::mip_interface& device, const float* noise); +TypedResult readAccelNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveAccelNoise(C::mip_interface& device); +TypedResult loadAccelNoise(C::mip_interface& device); +TypedResult defaultAccelNoise(C::mip_interface& device); ///@} /// @@ -1538,6 +1596,8 @@ struct GyroNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroNoise"; + static constexpr const char* DOC_NAME = "Gyroscope Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1570,6 +1630,8 @@ struct GyroNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroNoise::Response"; + static constexpr const char* DOC_NAME = "Gyroscope Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1588,11 +1650,11 @@ void extract(Serializer& serializer, GyroNoise& self); void insert(Serializer& serializer, const GyroNoise::Response& self); void extract(Serializer& serializer, GyroNoise::Response& self); -CmdResult writeGyroNoise(C::mip_interface& device, const float* noise); -CmdResult readGyroNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveGyroNoise(C::mip_interface& device); -CmdResult loadGyroNoise(C::mip_interface& device); -CmdResult defaultGyroNoise(C::mip_interface& device); +TypedResult writeGyroNoise(C::mip_interface& device, const float* noise); +TypedResult readGyroNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveGyroNoise(C::mip_interface& device); +TypedResult loadGyroNoise(C::mip_interface& device); +TypedResult defaultGyroNoise(C::mip_interface& device); ///@} /// @@ -1614,6 +1676,8 @@ struct AccelBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBiasModel"; + static constexpr const char* DOC_NAME = "Accelerometer Bias Model Parameters"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1646,6 +1710,8 @@ struct AccelBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBiasModel::Response"; + static constexpr const char* DOC_NAME = "Accelerometer Bias Model Parameters Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1665,11 +1731,11 @@ void extract(Serializer& serializer, AccelBiasModel& self); void insert(Serializer& serializer, const AccelBiasModel::Response& self); void extract(Serializer& serializer, AccelBiasModel::Response& self); -CmdResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); -CmdResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); -CmdResult saveAccelBiasModel(C::mip_interface& device); -CmdResult loadAccelBiasModel(C::mip_interface& device); -CmdResult defaultAccelBiasModel(C::mip_interface& device); +TypedResult writeAccelBiasModel(C::mip_interface& device, const float* beta, const float* noise); +TypedResult readAccelBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); +TypedResult saveAccelBiasModel(C::mip_interface& device); +TypedResult loadAccelBiasModel(C::mip_interface& device); +TypedResult defaultAccelBiasModel(C::mip_interface& device); ///@} /// @@ -1691,6 +1757,8 @@ struct GyroBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBiasModel"; + static constexpr const char* DOC_NAME = "Gyroscope Bias Model Parameters"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1723,6 +1791,8 @@ struct GyroBiasModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_BIAS_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBiasModel::Response"; + static constexpr const char* DOC_NAME = "Gyroscope Bias Model Parameters Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1742,11 +1812,11 @@ void extract(Serializer& serializer, GyroBiasModel& self); void insert(Serializer& serializer, const GyroBiasModel::Response& self); void extract(Serializer& serializer, GyroBiasModel::Response& self); -CmdResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); -CmdResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); -CmdResult saveGyroBiasModel(C::mip_interface& device); -CmdResult loadGyroBiasModel(C::mip_interface& device); -CmdResult defaultGyroBiasModel(C::mip_interface& device); +TypedResult writeGyroBiasModel(C::mip_interface& device, const float* beta, const float* noise); +TypedResult readGyroBiasModel(C::mip_interface& device, float* betaOut, float* noiseOut); +TypedResult saveGyroBiasModel(C::mip_interface& device); +TypedResult loadGyroBiasModel(C::mip_interface& device); +TypedResult defaultGyroBiasModel(C::mip_interface& device); ///@} /// @@ -1774,6 +1844,8 @@ struct AltitudeAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ALTITUDE_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AltitudeAiding"; + static constexpr const char* DOC_NAME = "Altitude Aiding Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1806,6 +1878,8 @@ struct AltitudeAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ALTITUDE_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AltitudeAiding::Response"; + static constexpr const char* DOC_NAME = "Altitude Aiding Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1824,11 +1898,11 @@ void extract(Serializer& serializer, AltitudeAiding& self); void insert(Serializer& serializer, const AltitudeAiding::Response& self); void extract(Serializer& serializer, AltitudeAiding::Response& self); -CmdResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); -CmdResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); -CmdResult saveAltitudeAiding(C::mip_interface& device); -CmdResult loadAltitudeAiding(C::mip_interface& device); -CmdResult defaultAltitudeAiding(C::mip_interface& device); +TypedResult writeAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector selector); +TypedResult readAltitudeAiding(C::mip_interface& device, AltitudeAiding::AidingSelector* selectorOut); +TypedResult saveAltitudeAiding(C::mip_interface& device); +TypedResult loadAltitudeAiding(C::mip_interface& device); +TypedResult defaultAltitudeAiding(C::mip_interface& device); ///@} /// @@ -1853,6 +1927,8 @@ struct PitchRollAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SECONDARY_PITCH_ROLL_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PitchRollAiding"; + static constexpr const char* DOC_NAME = "Pitch/Roll Aiding Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -1885,6 +1961,8 @@ struct PitchRollAiding static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SECONDARY_PITCH_ROLL_AIDING_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PitchRollAiding::Response"; + static constexpr const char* DOC_NAME = "Pitch/Roll Aiding Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1903,11 +1981,11 @@ void extract(Serializer& serializer, PitchRollAiding& self); void insert(Serializer& serializer, const PitchRollAiding::Response& self); void extract(Serializer& serializer, PitchRollAiding::Response& self); -CmdResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); -CmdResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); -CmdResult savePitchRollAiding(C::mip_interface& device); -CmdResult loadPitchRollAiding(C::mip_interface& device); -CmdResult defaultPitchRollAiding(C::mip_interface& device); +TypedResult writePitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource source); +TypedResult readPitchRollAiding(C::mip_interface& device, PitchRollAiding::AidingSource* sourceOut); +TypedResult savePitchRollAiding(C::mip_interface& device); +TypedResult loadPitchRollAiding(C::mip_interface& device); +TypedResult defaultPitchRollAiding(C::mip_interface& device); ///@} /// @@ -1927,6 +2005,8 @@ struct AutoZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoZupt"; + static constexpr const char* DOC_NAME = "Zero Velocity Update Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -1959,6 +2039,8 @@ struct AutoZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoZupt::Response"; + static constexpr const char* DOC_NAME = "Zero Velocity Update Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -1978,11 +2060,11 @@ void extract(Serializer& serializer, AutoZupt& self); void insert(Serializer& serializer, const AutoZupt::Response& self); void extract(Serializer& serializer, AutoZupt::Response& self); -CmdResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold); -CmdResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); -CmdResult saveAutoZupt(C::mip_interface& device); -CmdResult loadAutoZupt(C::mip_interface& device); -CmdResult defaultAutoZupt(C::mip_interface& device); +TypedResult writeAutoZupt(C::mip_interface& device, uint8_t enable, float threshold); +TypedResult readAutoZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); +TypedResult saveAutoZupt(C::mip_interface& device); +TypedResult loadAutoZupt(C::mip_interface& device); +TypedResult defaultAutoZupt(C::mip_interface& device); ///@} /// @@ -2003,6 +2085,8 @@ struct AutoAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANGULAR_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoAngularZupt"; + static constexpr const char* DOC_NAME = "Zero Angular Rate Update Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2035,6 +2119,8 @@ struct AutoAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANGULAR_ZUPT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AutoAngularZupt::Response"; + static constexpr const char* DOC_NAME = "Zero Angular Rate Update Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2054,11 +2140,11 @@ void extract(Serializer& serializer, AutoAngularZupt& self); void insert(Serializer& serializer, const AutoAngularZupt::Response& self); void extract(Serializer& serializer, AutoAngularZupt::Response& self); -CmdResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold); -CmdResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); -CmdResult saveAutoAngularZupt(C::mip_interface& device); -CmdResult loadAutoAngularZupt(C::mip_interface& device); -CmdResult defaultAutoAngularZupt(C::mip_interface& device); +TypedResult writeAutoAngularZupt(C::mip_interface& device, uint8_t enable, float threshold); +TypedResult readAutoAngularZupt(C::mip_interface& device, uint8_t* enableOut, float* thresholdOut); +TypedResult saveAutoAngularZupt(C::mip_interface& device); +TypedResult loadAutoAngularZupt(C::mip_interface& device); +TypedResult defaultAutoAngularZupt(C::mip_interface& device); ///@} /// @@ -2074,6 +2160,8 @@ struct CommandedZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ZUPT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommandedZupt"; + static constexpr const char* DOC_NAME = "Commanded Zero Velocity Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2092,7 +2180,7 @@ struct CommandedZupt void insert(Serializer& serializer, const CommandedZupt& self); void extract(Serializer& serializer, CommandedZupt& self); -CmdResult commandedZupt(C::mip_interface& device); +TypedResult commandedZupt(C::mip_interface& device); ///@} /// @@ -2108,6 +2196,8 @@ struct CommandedAngularZupt static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_COMMANDED_ANGULAR_ZUPT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommandedAngularZupt"; + static constexpr const char* DOC_NAME = "Commanded Zero Angular Rate Update"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2126,7 +2216,7 @@ struct CommandedAngularZupt void insert(Serializer& serializer, const CommandedAngularZupt& self); void extract(Serializer& serializer, CommandedAngularZupt& self); -CmdResult commandedAngularZupt(C::mip_interface& device); +TypedResult commandedAngularZupt(C::mip_interface& device); ///@} /// @@ -2145,6 +2235,8 @@ struct MagCaptureAutoCal static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_CAPTURE_AUTO_CALIBRATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagCaptureAutoCal"; + static constexpr const char* DOC_NAME = "Magnetometer Capture Auto Calibration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8000; @@ -2176,8 +2268,8 @@ struct MagCaptureAutoCal void insert(Serializer& serializer, const MagCaptureAutoCal& self); void extract(Serializer& serializer, MagCaptureAutoCal& self); -CmdResult writeMagCaptureAutoCal(C::mip_interface& device); -CmdResult saveMagCaptureAutoCal(C::mip_interface& device); +TypedResult writeMagCaptureAutoCal(C::mip_interface& device); +TypedResult saveMagCaptureAutoCal(C::mip_interface& device); ///@} /// @@ -2200,6 +2292,8 @@ struct GravityNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GRAVITY_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GravityNoise"; + static constexpr const char* DOC_NAME = "Gravity Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2232,6 +2326,8 @@ struct GravityNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GRAVITY_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GravityNoise::Response"; + static constexpr const char* DOC_NAME = "Gravity Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2250,11 +2346,11 @@ void extract(Serializer& serializer, GravityNoise& self); void insert(Serializer& serializer, const GravityNoise::Response& self); void extract(Serializer& serializer, GravityNoise::Response& self); -CmdResult writeGravityNoise(C::mip_interface& device, const float* noise); -CmdResult readGravityNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveGravityNoise(C::mip_interface& device); -CmdResult loadGravityNoise(C::mip_interface& device); -CmdResult defaultGravityNoise(C::mip_interface& device); +TypedResult writeGravityNoise(C::mip_interface& device, const float* noise); +TypedResult readGravityNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveGravityNoise(C::mip_interface& device); +TypedResult loadGravityNoise(C::mip_interface& device); +TypedResult defaultGravityNoise(C::mip_interface& device); ///@} /// @@ -2277,6 +2373,8 @@ struct PressureAltitudeNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_PRESSURE_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PressureAltitudeNoise"; + static constexpr const char* DOC_NAME = "Pressure Altitude Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2309,6 +2407,8 @@ struct PressureAltitudeNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_PRESSURE_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PressureAltitudeNoise::Response"; + static constexpr const char* DOC_NAME = "Pressure Altitude Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2327,11 +2427,11 @@ void extract(Serializer& serializer, PressureAltitudeNoise& self); void insert(Serializer& serializer, const PressureAltitudeNoise::Response& self); void extract(Serializer& serializer, PressureAltitudeNoise::Response& self); -CmdResult writePressureAltitudeNoise(C::mip_interface& device, float noise); -CmdResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); -CmdResult savePressureAltitudeNoise(C::mip_interface& device); -CmdResult loadPressureAltitudeNoise(C::mip_interface& device); -CmdResult defaultPressureAltitudeNoise(C::mip_interface& device); +TypedResult writePressureAltitudeNoise(C::mip_interface& device, float noise); +TypedResult readPressureAltitudeNoise(C::mip_interface& device, float* noiseOut); +TypedResult savePressureAltitudeNoise(C::mip_interface& device); +TypedResult loadPressureAltitudeNoise(C::mip_interface& device); +TypedResult defaultPressureAltitudeNoise(C::mip_interface& device); ///@} /// @@ -2356,6 +2456,8 @@ struct HardIronOffsetNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_HARD_IRON_OFFSET_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HardIronOffsetNoise"; + static constexpr const char* DOC_NAME = "Hard Iron Offset Process Noise"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2388,6 +2490,8 @@ struct HardIronOffsetNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_HARD_IRON_OFFSET_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HardIronOffsetNoise::Response"; + static constexpr const char* DOC_NAME = "Hard Iron Offset Process Noise Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2406,11 +2510,11 @@ void extract(Serializer& serializer, HardIronOffsetNoise& self); void insert(Serializer& serializer, const HardIronOffsetNoise::Response& self); void extract(Serializer& serializer, HardIronOffsetNoise::Response& self); -CmdResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); -CmdResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveHardIronOffsetNoise(C::mip_interface& device); -CmdResult loadHardIronOffsetNoise(C::mip_interface& device); -CmdResult defaultHardIronOffsetNoise(C::mip_interface& device); +TypedResult writeHardIronOffsetNoise(C::mip_interface& device, const float* noise); +TypedResult readHardIronOffsetNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveHardIronOffsetNoise(C::mip_interface& device); +TypedResult loadHardIronOffsetNoise(C::mip_interface& device); +TypedResult defaultHardIronOffsetNoise(C::mip_interface& device); ///@} /// @@ -2434,6 +2538,8 @@ struct SoftIronMatrixNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SOFT_IRON_MATRIX_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SoftIronMatrixNoise"; + static constexpr const char* DOC_NAME = "Soft Iron Offset Process Noise"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2466,6 +2572,8 @@ struct SoftIronMatrixNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SOFT_IRON_MATRIX_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SoftIronMatrixNoise::Response"; + static constexpr const char* DOC_NAME = "Soft Iron Offset Process Noise Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2484,11 +2592,11 @@ void extract(Serializer& serializer, SoftIronMatrixNoise& self); void insert(Serializer& serializer, const SoftIronMatrixNoise::Response& self); void extract(Serializer& serializer, SoftIronMatrixNoise::Response& self); -CmdResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); -CmdResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveSoftIronMatrixNoise(C::mip_interface& device); -CmdResult loadSoftIronMatrixNoise(C::mip_interface& device); -CmdResult defaultSoftIronMatrixNoise(C::mip_interface& device); +TypedResult writeSoftIronMatrixNoise(C::mip_interface& device, const float* noise); +TypedResult readSoftIronMatrixNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveSoftIronMatrixNoise(C::mip_interface& device); +TypedResult loadSoftIronMatrixNoise(C::mip_interface& device); +TypedResult defaultSoftIronMatrixNoise(C::mip_interface& device); ///@} /// @@ -2512,6 +2620,8 @@ struct MagNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagNoise"; + static constexpr const char* DOC_NAME = "Magnetometer Noise Standard Deviation"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -2544,6 +2654,8 @@ struct MagNoise static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_NOISE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagNoise::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Noise Standard Deviation Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2562,11 +2674,11 @@ void extract(Serializer& serializer, MagNoise& self); void insert(Serializer& serializer, const MagNoise::Response& self); void extract(Serializer& serializer, MagNoise::Response& self); -CmdResult writeMagNoise(C::mip_interface& device, const float* noise); -CmdResult readMagNoise(C::mip_interface& device, float* noiseOut); -CmdResult saveMagNoise(C::mip_interface& device); -CmdResult loadMagNoise(C::mip_interface& device); -CmdResult defaultMagNoise(C::mip_interface& device); +TypedResult writeMagNoise(C::mip_interface& device, const float* noise); +TypedResult readMagNoise(C::mip_interface& device, float* noiseOut); +TypedResult saveMagNoise(C::mip_interface& device); +TypedResult loadMagNoise(C::mip_interface& device); +TypedResult defaultMagNoise(C::mip_interface& device); ///@} /// @@ -2589,6 +2701,8 @@ struct InclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INCLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InclinationSource"; + static constexpr const char* DOC_NAME = "Inclination Source"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2621,6 +2735,8 @@ struct InclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INCLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InclinationSource::Response"; + static constexpr const char* DOC_NAME = "Inclination Source Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2640,11 +2756,11 @@ void extract(Serializer& serializer, InclinationSource& self); void insert(Serializer& serializer, const InclinationSource::Response& self); void extract(Serializer& serializer, InclinationSource::Response& self); -CmdResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); -CmdResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); -CmdResult saveInclinationSource(C::mip_interface& device); -CmdResult loadInclinationSource(C::mip_interface& device); -CmdResult defaultInclinationSource(C::mip_interface& device); +TypedResult writeInclinationSource(C::mip_interface& device, FilterMagParamSource source, float inclination); +TypedResult readInclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* inclinationOut); +TypedResult saveInclinationSource(C::mip_interface& device); +TypedResult loadInclinationSource(C::mip_interface& device); +TypedResult defaultInclinationSource(C::mip_interface& device); ///@} /// @@ -2667,6 +2783,8 @@ struct MagneticDeclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_DECLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticDeclinationSource"; + static constexpr const char* DOC_NAME = "Magnetic Field Declination Source Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2699,6 +2817,8 @@ struct MagneticDeclinationSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_DECLINATION_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticDeclinationSource::Response"; + static constexpr const char* DOC_NAME = "Magnetic Field Declination Source Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2718,11 +2838,11 @@ void extract(Serializer& serializer, MagneticDeclinationSource& self); void insert(Serializer& serializer, const MagneticDeclinationSource::Response& self); void extract(Serializer& serializer, MagneticDeclinationSource::Response& self); -CmdResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); -CmdResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); -CmdResult saveMagneticDeclinationSource(C::mip_interface& device); -CmdResult loadMagneticDeclinationSource(C::mip_interface& device); -CmdResult defaultMagneticDeclinationSource(C::mip_interface& device); +TypedResult writeMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource source, float declination); +TypedResult readMagneticDeclinationSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* declinationOut); +TypedResult saveMagneticDeclinationSource(C::mip_interface& device); +TypedResult loadMagneticDeclinationSource(C::mip_interface& device); +TypedResult defaultMagneticDeclinationSource(C::mip_interface& device); ///@} /// @@ -2744,6 +2864,8 @@ struct MagFieldMagnitudeSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAGNETIC_MAGNITUDE_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagFieldMagnitudeSource"; + static constexpr const char* DOC_NAME = "Magnetic Field Magnitude Source"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -2776,6 +2898,8 @@ struct MagFieldMagnitudeSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAGNETIC_MAGNITUDE_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagFieldMagnitudeSource::Response"; + static constexpr const char* DOC_NAME = "Magnetic Field Magnitude Source Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2795,11 +2919,11 @@ void extract(Serializer& serializer, MagFieldMagnitudeSource& self); void insert(Serializer& serializer, const MagFieldMagnitudeSource::Response& self); void extract(Serializer& serializer, MagFieldMagnitudeSource::Response& self); -CmdResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); -CmdResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); -CmdResult saveMagFieldMagnitudeSource(C::mip_interface& device); -CmdResult loadMagFieldMagnitudeSource(C::mip_interface& device); -CmdResult defaultMagFieldMagnitudeSource(C::mip_interface& device); +TypedResult writeMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource source, float magnitude); +TypedResult readMagFieldMagnitudeSource(C::mip_interface& device, FilterMagParamSource* sourceOut, float* magnitudeOut); +TypedResult saveMagFieldMagnitudeSource(C::mip_interface& device); +TypedResult loadMagFieldMagnitudeSource(C::mip_interface& device); +TypedResult defaultMagFieldMagnitudeSource(C::mip_interface& device); ///@} /// @@ -2823,6 +2947,8 @@ struct ReferencePosition static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REFERENCE_POSITION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferencePosition"; + static constexpr const char* DOC_NAME = "Set Reference Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x800F; @@ -2855,6 +2981,8 @@ struct ReferencePosition static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REFERENCE_POSITION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferencePosition::Response"; + static constexpr const char* DOC_NAME = "Set Reference Position Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2876,11 +3004,11 @@ void extract(Serializer& serializer, ReferencePosition& self); void insert(Serializer& serializer, const ReferencePosition::Response& self); void extract(Serializer& serializer, ReferencePosition::Response& self); -CmdResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); -CmdResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); -CmdResult saveReferencePosition(C::mip_interface& device); -CmdResult loadReferencePosition(C::mip_interface& device); -CmdResult defaultReferencePosition(C::mip_interface& device); +TypedResult writeReferencePosition(C::mip_interface& device, bool enable, double latitude, double longitude, double altitude); +TypedResult readReferencePosition(C::mip_interface& device, bool* enableOut, double* latitudeOut, double* longitudeOut, double* altitudeOut); +TypedResult saveReferencePosition(C::mip_interface& device); +TypedResult loadReferencePosition(C::mip_interface& device); +TypedResult defaultReferencePosition(C::mip_interface& device); ///@} /// @@ -2917,6 +3045,8 @@ struct AccelMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelMagnitudeErrorAdaptiveMeasurement"; + static constexpr const char* DOC_NAME = "Gravity Magnitude Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x807F; @@ -2949,6 +3079,8 @@ struct AccelMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ACCEL_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelMagnitudeErrorAdaptiveMeasurement::Response"; + static constexpr const char* DOC_NAME = "Gravity Magnitude Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -2973,11 +3105,11 @@ void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement& sel void insert(Serializer& serializer, const AccelMagnitudeErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, AccelMagnitudeErrorAdaptiveMeasurement::Response& self); -CmdResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); -CmdResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -CmdResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult writeAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +TypedResult readAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +TypedResult saveAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult loadAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult defaultAccelMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3009,6 +3141,8 @@ struct MagMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagMagnitudeErrorAdaptiveMeasurement"; + static constexpr const char* DOC_NAME = "Magnetometer Magnitude Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x807F; @@ -3041,6 +3175,8 @@ struct MagMagnitudeErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_MAGNITUDE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagMagnitudeErrorAdaptiveMeasurement::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Magnitude Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3065,11 +3201,11 @@ void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement& self) void insert(Serializer& serializer, const MagMagnitudeErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, MagMagnitudeErrorAdaptiveMeasurement::Response& self); -CmdResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); -CmdResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -CmdResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult writeMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement adaptiveMeasurement, float frequency, float lowLimit, float highLimit, float lowLimitUncertainty, float highLimitUncertainty, float minimumUncertainty); +TypedResult readMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device, FilterAdaptiveMeasurement* adaptiveMeasurementOut, float* frequencyOut, float* lowLimitOut, float* highLimitOut, float* lowLimitUncertaintyOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +TypedResult saveMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult loadMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult defaultMagMagnitudeErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3101,6 +3237,8 @@ struct MagDipAngleErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagDipAngleErrorAdaptiveMeasurement"; + static constexpr const char* DOC_NAME = "Magnetometer Dig Angle Error Adaptive Measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -3133,6 +3271,8 @@ struct MagDipAngleErrorAdaptiveMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MAG_DIP_ANGLE_ERROR_ADAPTIVE_MEASUREMENT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagDipAngleErrorAdaptiveMeasurement::Response"; + static constexpr const char* DOC_NAME = "Magnetometer Dig Angle Error Adaptive Measurement Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3155,11 +3295,11 @@ void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement& self); void insert(Serializer& serializer, const MagDipAngleErrorAdaptiveMeasurement::Response& self); void extract(Serializer& serializer, MagDipAngleErrorAdaptiveMeasurement::Response& self); -CmdResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); -CmdResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); -CmdResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); -CmdResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult writeMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool enable, float frequency, float highLimit, float highLimitUncertainty, float minimumUncertainty); +TypedResult readMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device, bool* enableOut, float* frequencyOut, float* highLimitOut, float* highLimitUncertaintyOut, float* minimumUncertaintyOut); +TypedResult saveMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult loadMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); +TypedResult defaultMagDipAngleErrorAdaptiveMeasurement(C::mip_interface& device); ///@} /// @@ -3194,6 +3334,8 @@ struct AidingMeasurementEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_AIDING_MEASUREMENT_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingMeasurementEnable"; + static constexpr const char* DOC_NAME = "Aiding Measurement Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3227,6 +3369,8 @@ struct AidingMeasurementEnable static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_AIDING_MEASUREMENT_ENABLE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingMeasurementEnable::Response"; + static constexpr const char* DOC_NAME = "Aiding Measurement Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3246,11 +3390,11 @@ void extract(Serializer& serializer, AidingMeasurementEnable& self); void insert(Serializer& serializer, const AidingMeasurementEnable::Response& self); void extract(Serializer& serializer, AidingMeasurementEnable::Response& self); -CmdResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable); -CmdResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut); -CmdResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); -CmdResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); -CmdResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +TypedResult writeAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool enable); +TypedResult readAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource, bool* enableOut); +TypedResult saveAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +TypedResult loadAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); +TypedResult defaultAidingMeasurementEnable(C::mip_interface& device, AidingMeasurementEnable::AidingSource aidingSource); ///@} /// @@ -3268,6 +3412,8 @@ struct Run static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_RUN; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Run"; + static constexpr const char* DOC_NAME = "Run Navigation Filter"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3286,7 +3432,7 @@ struct Run void insert(Serializer& serializer, const Run& self); void extract(Serializer& serializer, Run& self); -CmdResult run(C::mip_interface& device); +TypedResult run(C::mip_interface& device); ///@} /// @@ -3308,6 +3454,8 @@ struct KinematicConstraint static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_KINEMATIC_CONSTRAINT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "KinematicConstraint"; + static constexpr const char* DOC_NAME = "Kinematic Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3340,6 +3488,8 @@ struct KinematicConstraint static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_KINEMATIC_CONSTRAINT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "KinematicConstraint::Response"; + static constexpr const char* DOC_NAME = "Kinematic Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3360,11 +3510,11 @@ void extract(Serializer& serializer, KinematicConstraint& self); void insert(Serializer& serializer, const KinematicConstraint::Response& self); void extract(Serializer& serializer, KinematicConstraint::Response& self); -CmdResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection); -CmdResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut); -CmdResult saveKinematicConstraint(C::mip_interface& device); -CmdResult loadKinematicConstraint(C::mip_interface& device); -CmdResult defaultKinematicConstraint(C::mip_interface& device); +TypedResult writeKinematicConstraint(C::mip_interface& device, uint8_t accelerationConstraintSelection, uint8_t velocityConstraintSelection, uint8_t angularConstraintSelection); +TypedResult readKinematicConstraint(C::mip_interface& device, uint8_t* accelerationConstraintSelectionOut, uint8_t* velocityConstraintSelectionOut, uint8_t* angularConstraintSelectionOut); +TypedResult saveKinematicConstraint(C::mip_interface& device); +TypedResult loadKinematicConstraint(C::mip_interface& device); +TypedResult defaultKinematicConstraint(C::mip_interface& device); ///@} /// @@ -3397,7 +3547,7 @@ struct InitializationConfiguration AlignmentSelector(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } AlignmentSelector& operator=(uint8_t val) { value = val; return *this; } - AlignmentSelector& operator=(int val) { value = val; return *this; } + AlignmentSelector& operator=(int val) { value = uint8_t(val); return *this; } AlignmentSelector& operator|=(uint8_t val) { return *this = value | val; } AlignmentSelector& operator&=(uint8_t val) { return *this = value & val; } @@ -3436,6 +3586,8 @@ struct InitializationConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_INITIALIZATION_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InitializationConfiguration"; + static constexpr const char* DOC_NAME = "Navigation Filter Initialization"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x81FF; @@ -3468,6 +3620,8 @@ struct InitializationConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_INITIALIZATION_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InitializationConfiguration::Response"; + static constexpr const char* DOC_NAME = "Navigation Filter Initialization Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3494,11 +3648,11 @@ void extract(Serializer& serializer, InitializationConfiguration& self); void insert(Serializer& serializer, const InitializationConfiguration::Response& self); void extract(Serializer& serializer, InitializationConfiguration::Response& self); -CmdResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); -CmdResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); -CmdResult saveInitializationConfiguration(C::mip_interface& device); -CmdResult loadInitializationConfiguration(C::mip_interface& device); -CmdResult defaultInitializationConfiguration(C::mip_interface& device); +TypedResult writeInitializationConfiguration(C::mip_interface& device, uint8_t waitForRunCommand, InitializationConfiguration::InitialConditionSource initialCondSrc, InitializationConfiguration::AlignmentSelector autoHeadingAlignmentSelector, float initialHeading, float initialPitch, float initialRoll, const float* initialPosition, const float* initialVelocity, FilterReferenceFrame referenceFrameSelector); +TypedResult readInitializationConfiguration(C::mip_interface& device, uint8_t* waitForRunCommandOut, InitializationConfiguration::InitialConditionSource* initialCondSrcOut, InitializationConfiguration::AlignmentSelector* autoHeadingAlignmentSelectorOut, float* initialHeadingOut, float* initialPitchOut, float* initialRollOut, float* initialPositionOut, float* initialVelocityOut, FilterReferenceFrame* referenceFrameSelectorOut); +TypedResult saveInitializationConfiguration(C::mip_interface& device); +TypedResult loadInitializationConfiguration(C::mip_interface& device); +TypedResult defaultInitializationConfiguration(C::mip_interface& device); ///@} /// @@ -3517,6 +3671,8 @@ struct AdaptiveFilterOptions static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ADAPTIVE_FILTER_OPTIONS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AdaptiveFilterOptions"; + static constexpr const char* DOC_NAME = "Adaptive Filter Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3549,6 +3705,8 @@ struct AdaptiveFilterOptions static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ADAPTIVE_FILTER_OPTIONS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AdaptiveFilterOptions::Response"; + static constexpr const char* DOC_NAME = "Adaptive Filter Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3568,11 +3726,11 @@ void extract(Serializer& serializer, AdaptiveFilterOptions& self); void insert(Serializer& serializer, const AdaptiveFilterOptions::Response& self); void extract(Serializer& serializer, AdaptiveFilterOptions::Response& self); -CmdResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit); -CmdResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut); -CmdResult saveAdaptiveFilterOptions(C::mip_interface& device); -CmdResult loadAdaptiveFilterOptions(C::mip_interface& device); -CmdResult defaultAdaptiveFilterOptions(C::mip_interface& device); +TypedResult writeAdaptiveFilterOptions(C::mip_interface& device, uint8_t level, uint16_t timeLimit); +TypedResult readAdaptiveFilterOptions(C::mip_interface& device, uint8_t* levelOut, uint16_t* timeLimitOut); +TypedResult saveAdaptiveFilterOptions(C::mip_interface& device); +TypedResult loadAdaptiveFilterOptions(C::mip_interface& device); +TypedResult defaultAdaptiveFilterOptions(C::mip_interface& device); ///@} /// @@ -3594,6 +3752,8 @@ struct MultiAntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffset"; + static constexpr const char* DOC_NAME = "GNSS Multi-Antenna Offset Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3627,6 +3787,8 @@ struct MultiAntennaOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_MULTI_ANTENNA_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffset::Response"; + static constexpr const char* DOC_NAME = "GNSS Multi-Antenna Offset Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3646,11 +3808,11 @@ void extract(Serializer& serializer, MultiAntennaOffset& self); void insert(Serializer& serializer, const MultiAntennaOffset::Response& self); void extract(Serializer& serializer, MultiAntennaOffset::Response& self); -CmdResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); -CmdResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); -CmdResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); -CmdResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); -CmdResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +TypedResult writeMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, const float* antennaOffset); +TypedResult readMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId, float* antennaOffsetOut); +TypedResult saveMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +TypedResult loadMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); +TypedResult defaultMultiAntennaOffset(C::mip_interface& device, uint8_t receiverId); ///@} /// @@ -3670,6 +3832,8 @@ struct RelPosConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REL_POS_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RelPosConfiguration"; + static constexpr const char* DOC_NAME = "Relative Position Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8007; @@ -3702,6 +3866,8 @@ struct RelPosConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REL_POS_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RelPosConfiguration::Response"; + static constexpr const char* DOC_NAME = "Relative Position Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3722,11 +3888,11 @@ void extract(Serializer& serializer, RelPosConfiguration& self); void insert(Serializer& serializer, const RelPosConfiguration::Response& self); void extract(Serializer& serializer, RelPosConfiguration::Response& self); -CmdResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); -CmdResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); -CmdResult saveRelPosConfiguration(C::mip_interface& device); -CmdResult loadRelPosConfiguration(C::mip_interface& device); -CmdResult defaultRelPosConfiguration(C::mip_interface& device); +TypedResult writeRelPosConfiguration(C::mip_interface& device, uint8_t source, FilterReferenceFrame referenceFrameSelector, const double* referenceCoordinates); +TypedResult readRelPosConfiguration(C::mip_interface& device, uint8_t* sourceOut, FilterReferenceFrame* referenceFrameSelectorOut, double* referenceCoordinatesOut); +TypedResult saveRelPosConfiguration(C::mip_interface& device); +TypedResult loadRelPosConfiguration(C::mip_interface& device); +TypedResult defaultRelPosConfiguration(C::mip_interface& device); ///@} /// @@ -3757,6 +3923,8 @@ struct RefPointLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_REF_POINT_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RefPointLeverArm"; + static constexpr const char* DOC_NAME = "Reference point lever arm"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3789,6 +3957,8 @@ struct RefPointLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_REF_POINT_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RefPointLeverArm::Response"; + static constexpr const char* DOC_NAME = "Reference point lever arm Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3808,11 +3978,11 @@ void extract(Serializer& serializer, RefPointLeverArm& self); void insert(Serializer& serializer, const RefPointLeverArm::Response& self); void extract(Serializer& serializer, RefPointLeverArm::Response& self); -CmdResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); -CmdResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); -CmdResult saveRefPointLeverArm(C::mip_interface& device); -CmdResult loadRefPointLeverArm(C::mip_interface& device); -CmdResult defaultRefPointLeverArm(C::mip_interface& device); +TypedResult writeRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector refPointSel, const float* leverArmOffset); +TypedResult readRefPointLeverArm(C::mip_interface& device, RefPointLeverArm::ReferencePointSelector* refPointSelOut, float* leverArmOffsetOut); +TypedResult saveRefPointLeverArm(C::mip_interface& device); +TypedResult loadRefPointLeverArm(C::mip_interface& device); +TypedResult defaultRefPointLeverArm(C::mip_interface& device); ///@} /// @@ -3834,6 +4004,8 @@ struct SpeedMeasurement static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_MEASUREMENT; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SpeedMeasurement"; + static constexpr const char* DOC_NAME = "Input speed measurement"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3852,7 +4024,7 @@ struct SpeedMeasurement void insert(Serializer& serializer, const SpeedMeasurement& self); void extract(Serializer& serializer, SpeedMeasurement& self); -CmdResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty); +TypedResult speedMeasurement(C::mip_interface& device, uint8_t source, float timeOfWeek, float speed, float speedUncertainty); ///@} /// @@ -3877,6 +4049,8 @@ struct SpeedLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SPEED_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SpeedLeverArm"; + static constexpr const char* DOC_NAME = "Measurement speed lever arm"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -3910,6 +4084,8 @@ struct SpeedLeverArm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_SPEED_LEVER_ARM; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SpeedLeverArm::Response"; + static constexpr const char* DOC_NAME = "Measurement speed lever arm Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0001; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -3929,11 +4105,11 @@ void extract(Serializer& serializer, SpeedLeverArm& self); void insert(Serializer& serializer, const SpeedLeverArm::Response& self); void extract(Serializer& serializer, SpeedLeverArm::Response& self); -CmdResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); -CmdResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); -CmdResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); -CmdResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); -CmdResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); +TypedResult writeSpeedLeverArm(C::mip_interface& device, uint8_t source, const float* leverArmOffset); +TypedResult readSpeedLeverArm(C::mip_interface& device, uint8_t source, float* leverArmOffsetOut); +TypedResult saveSpeedLeverArm(C::mip_interface& device, uint8_t source); +TypedResult loadSpeedLeverArm(C::mip_interface& device, uint8_t source); +TypedResult defaultSpeedLeverArm(C::mip_interface& device, uint8_t source); ///@} /// @@ -3957,6 +4133,8 @@ struct WheeledVehicleConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_VEHICLE_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "WheeledVehicleConstraintControl"; + static constexpr const char* DOC_NAME = "Wheeled Vehicle Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -3989,6 +4167,8 @@ struct WheeledVehicleConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_VEHICLE_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "WheeledVehicleConstraintControl::Response"; + static constexpr const char* DOC_NAME = "Wheeled Vehicle Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4007,11 +4187,11 @@ void extract(Serializer& serializer, WheeledVehicleConstraintControl& self); void insert(Serializer& serializer, const WheeledVehicleConstraintControl::Response& self); void extract(Serializer& serializer, WheeledVehicleConstraintControl::Response& self); -CmdResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable); -CmdResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut); -CmdResult saveWheeledVehicleConstraintControl(C::mip_interface& device); -CmdResult loadWheeledVehicleConstraintControl(C::mip_interface& device); -CmdResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); +TypedResult writeWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t enable); +TypedResult readWheeledVehicleConstraintControl(C::mip_interface& device, uint8_t* enableOut); +TypedResult saveWheeledVehicleConstraintControl(C::mip_interface& device); +TypedResult loadWheeledVehicleConstraintControl(C::mip_interface& device); +TypedResult defaultWheeledVehicleConstraintControl(C::mip_interface& device); ///@} /// @@ -4033,6 +4213,8 @@ struct VerticalGyroConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_GYRO_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VerticalGyroConstraintControl"; + static constexpr const char* DOC_NAME = "Vertical Gyro Constraint Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -4065,6 +4247,8 @@ struct VerticalGyroConstraintControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_GYRO_CONSTRAINT_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VerticalGyroConstraintControl::Response"; + static constexpr const char* DOC_NAME = "Vertical Gyro Constraint Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4083,11 +4267,11 @@ void extract(Serializer& serializer, VerticalGyroConstraintControl& self); void insert(Serializer& serializer, const VerticalGyroConstraintControl::Response& self); void extract(Serializer& serializer, VerticalGyroConstraintControl::Response& self); -CmdResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable); -CmdResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut); -CmdResult saveVerticalGyroConstraintControl(C::mip_interface& device); -CmdResult loadVerticalGyroConstraintControl(C::mip_interface& device); -CmdResult defaultVerticalGyroConstraintControl(C::mip_interface& device); +TypedResult writeVerticalGyroConstraintControl(C::mip_interface& device, uint8_t enable); +TypedResult readVerticalGyroConstraintControl(C::mip_interface& device, uint8_t* enableOut); +TypedResult saveVerticalGyroConstraintControl(C::mip_interface& device); +TypedResult loadVerticalGyroConstraintControl(C::mip_interface& device); +TypedResult defaultVerticalGyroConstraintControl(C::mip_interface& device); ///@} /// @@ -4108,6 +4292,8 @@ struct GnssAntennaCalControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_ANTENNA_CALIBRATION_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAntennaCalControl"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Calibration Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -4140,6 +4326,8 @@ struct GnssAntennaCalControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::REPLY_ANTENNA_CALIBRATION_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAntennaCalControl::Response"; + static constexpr const char* DOC_NAME = "GNSS Antenna Offset Calibration Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4159,11 +4347,11 @@ void extract(Serializer& serializer, GnssAntennaCalControl& self); void insert(Serializer& serializer, const GnssAntennaCalControl::Response& self); void extract(Serializer& serializer, GnssAntennaCalControl::Response& self); -CmdResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset); -CmdResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut); -CmdResult saveGnssAntennaCalControl(C::mip_interface& device); -CmdResult loadGnssAntennaCalControl(C::mip_interface& device); -CmdResult defaultGnssAntennaCalControl(C::mip_interface& device); +TypedResult writeGnssAntennaCalControl(C::mip_interface& device, uint8_t enable, float maxOffset); +TypedResult readGnssAntennaCalControl(C::mip_interface& device, uint8_t* enableOut, float* maxOffsetOut); +TypedResult saveGnssAntennaCalControl(C::mip_interface& device); +TypedResult loadGnssAntennaCalControl(C::mip_interface& device); +TypedResult defaultGnssAntennaCalControl(C::mip_interface& device); ///@} /// @@ -4183,6 +4371,8 @@ struct SetInitialHeading static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_filter::CMD_SET_INITIAL_HEADING; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SetInitialHeading"; + static constexpr const char* DOC_NAME = "Set Initial Heading Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -4201,7 +4391,7 @@ struct SetInitialHeading void insert(Serializer& serializer, const SetInitialHeading& self); void extract(Serializer& serializer, SetInitialHeading& self); -CmdResult setInitialHeading(C::mip_interface& device, float heading); +TypedResult setInitialHeading(C::mip_interface& device, float heading); ///@} /// diff --git a/src/mip/definitions/commands_gnss.cpp b/src/mip/definitions/commands_gnss.cpp index d099323d0..f980f2e9c 100644 --- a/src/mip/definitions/commands_gnss.cpp +++ b/src/mip/definitions/commands_gnss.cpp @@ -77,12 +77,12 @@ void extract(Serializer& serializer, ReceiverInfo::Info& self) } -CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut) +TypedResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LIST_RECEIVERS, NULL, 0, REPLY_LIST_RECEIVERS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_LIST_RECEIVERS, NULL, 0, REPLY_LIST_RECEIVERS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -166,7 +166,7 @@ void extract(Serializer& serializer, SignalConfiguration::Response& self) } -CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved) +TypedResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -188,7 +188,7 @@ CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut) +TypedResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -197,7 +197,7 @@ CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SIGNAL_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SIGNAL_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -224,7 +224,7 @@ CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOu } return result; } -CmdResult saveSignalConfiguration(C::mip_interface& device) +TypedResult saveSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -234,7 +234,7 @@ CmdResult saveSignalConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadSignalConfiguration(C::mip_interface& device) +TypedResult loadSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -244,7 +244,7 @@ CmdResult loadSignalConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SIGNAL_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultSignalConfiguration(C::mip_interface& device) +TypedResult defaultSignalConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -298,7 +298,7 @@ void extract(Serializer& serializer, RtkDongleConfiguration::Response& self) } -CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved) +TypedResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -314,7 +314,7 @@ CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut) +TypedResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -323,7 +323,7 @@ CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOu assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_RTK_DONGLE_CONFIGURATION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_RTK_DONGLE_CONFIGURATION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -341,7 +341,7 @@ CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOu } return result; } -CmdResult saveRtkDongleConfiguration(C::mip_interface& device) +TypedResult saveRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -351,7 +351,7 @@ CmdResult saveRtkDongleConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadRtkDongleConfiguration(C::mip_interface& device) +TypedResult loadRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -361,7 +361,7 @@ CmdResult loadRtkDongleConfiguration(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_RTK_DONGLE_CONFIGURATION, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultRtkDongleConfiguration(C::mip_interface& device) +TypedResult defaultRtkDongleConfiguration(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_gnss.hpp b/src/mip/definitions/commands_gnss.hpp index d0b7cb4b4..7f3471d7d 100644 --- a/src/mip/definitions/commands_gnss.hpp +++ b/src/mip/definitions/commands_gnss.hpp @@ -78,6 +78,8 @@ struct ReceiverInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_LIST_RECEIVERS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReceiverInfo"; + static constexpr const char* DOC_NAME = "ReceiverInfo"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -97,6 +99,8 @@ struct ReceiverInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_LIST_RECEIVERS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReceiverInfo::Response"; + static constexpr const char* DOC_NAME = "ReceiverInfo Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x0000000C; @@ -119,7 +123,7 @@ void extract(Serializer& serializer, ReceiverInfo::Info& self); void insert(Serializer& serializer, const ReceiverInfo::Response& self); void extract(Serializer& serializer, ReceiverInfo::Response& self); -CmdResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut); +TypedResult receiverInfo(C::mip_interface& device, uint8_t* numReceiversOut, uint8_t numReceiversOutMax, ReceiverInfo::Info* receiverInfoOut); ///@} /// @@ -142,6 +146,8 @@ struct SignalConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_SIGNAL_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SignalConfiguration"; + static constexpr const char* DOC_NAME = "SignalConfiguration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x801F; @@ -174,6 +180,8 @@ struct SignalConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_SIGNAL_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SignalConfiguration::Response"; + static constexpr const char* DOC_NAME = "SignalConfiguration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -196,11 +204,11 @@ void extract(Serializer& serializer, SignalConfiguration& self); void insert(Serializer& serializer, const SignalConfiguration::Response& self); void extract(Serializer& serializer, SignalConfiguration::Response& self); -CmdResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved); -CmdResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut); -CmdResult saveSignalConfiguration(C::mip_interface& device); -CmdResult loadSignalConfiguration(C::mip_interface& device); -CmdResult defaultSignalConfiguration(C::mip_interface& device); +TypedResult writeSignalConfiguration(C::mip_interface& device, uint8_t gpsEnable, uint8_t glonassEnable, uint8_t galileoEnable, uint8_t beidouEnable, const uint8_t* reserved); +TypedResult readSignalConfiguration(C::mip_interface& device, uint8_t* gpsEnableOut, uint8_t* glonassEnableOut, uint8_t* galileoEnableOut, uint8_t* beidouEnableOut, uint8_t* reservedOut); +TypedResult saveSignalConfiguration(C::mip_interface& device); +TypedResult loadSignalConfiguration(C::mip_interface& device); +TypedResult defaultSignalConfiguration(C::mip_interface& device); ///@} /// @@ -220,6 +228,8 @@ struct RtkDongleConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::CMD_RTK_DONGLE_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RtkDongleConfiguration"; + static constexpr const char* DOC_NAME = "RtkDongleConfiguration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; @@ -252,6 +262,8 @@ struct RtkDongleConfiguration static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_gnss::REPLY_RTK_DONGLE_CONFIGURATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RtkDongleConfiguration::Response"; + static constexpr const char* DOC_NAME = "RtkDongleConfiguration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -271,11 +283,11 @@ void extract(Serializer& serializer, RtkDongleConfiguration& self); void insert(Serializer& serializer, const RtkDongleConfiguration::Response& self); void extract(Serializer& serializer, RtkDongleConfiguration::Response& self); -CmdResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved); -CmdResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut); -CmdResult saveRtkDongleConfiguration(C::mip_interface& device); -CmdResult loadRtkDongleConfiguration(C::mip_interface& device); -CmdResult defaultRtkDongleConfiguration(C::mip_interface& device); +TypedResult writeRtkDongleConfiguration(C::mip_interface& device, uint8_t enable, const uint8_t* reserved); +TypedResult readRtkDongleConfiguration(C::mip_interface& device, uint8_t* enableOut, uint8_t* reservedOut); +TypedResult saveRtkDongleConfiguration(C::mip_interface& device); +TypedResult loadRtkDongleConfiguration(C::mip_interface& device); +TypedResult defaultRtkDongleConfiguration(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_rtk.cpp b/src/mip/definitions/commands_rtk.cpp index b2997856e..2ec4b6dd2 100644 --- a/src/mip/definitions/commands_rtk.cpp +++ b/src/mip/definitions/commands_rtk.cpp @@ -51,12 +51,12 @@ void extract(Serializer& serializer, GetStatusFlags::Response& self) } -CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut) +TypedResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_STATUS_FLAGS, NULL, 0, REPLY_GET_STATUS_FLAGS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_STATUS_FLAGS, NULL, 0, REPLY_GET_STATUS_FLAGS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -94,12 +94,12 @@ void extract(Serializer& serializer, GetImei::Response& self) } -CmdResult getImei(C::mip_interface& device, char* imeiOut) +TypedResult getImei(C::mip_interface& device, char* imeiOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMEI, NULL, 0, REPLY_GET_IMEI, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMEI, NULL, 0, REPLY_GET_IMEI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -138,12 +138,12 @@ void extract(Serializer& serializer, GetImsi::Response& self) } -CmdResult getImsi(C::mip_interface& device, char* imsiOut) +TypedResult getImsi(C::mip_interface& device, char* imsiOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMSI, NULL, 0, REPLY_GET_IMSI, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_IMSI, NULL, 0, REPLY_GET_IMSI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -182,12 +182,12 @@ void extract(Serializer& serializer, GetIccid::Response& self) } -CmdResult getIccid(C::mip_interface& device, char* iccidOut) +TypedResult getIccid(C::mip_interface& device, char* iccidOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ICCID, NULL, 0, REPLY_GET_ICCID, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ICCID, NULL, 0, REPLY_GET_ICCID, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -234,7 +234,7 @@ void extract(Serializer& serializer, ConnectedDeviceType::Response& self) } -CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype) +TypedResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -246,7 +246,7 @@ CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut) +TypedResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -255,7 +255,7 @@ CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType: assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONNECTED_DEVICE_TYPE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_CONNECTED_DEVICE_TYPE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -269,7 +269,7 @@ CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType: } return result; } -CmdResult saveConnectedDeviceType(C::mip_interface& device) +TypedResult saveConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -279,7 +279,7 @@ CmdResult saveConnectedDeviceType(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult loadConnectedDeviceType(C::mip_interface& device) +TypedResult loadConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -289,7 +289,7 @@ CmdResult loadConnectedDeviceType(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_CONNECTED_DEVICE_TYPE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult defaultConnectedDeviceType(C::mip_interface& device) +TypedResult defaultConnectedDeviceType(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -323,12 +323,12 @@ void extract(Serializer& serializer, GetActCode::Response& self) } -CmdResult getActCode(C::mip_interface& device, char* activationcodeOut) +TypedResult getActCode(C::mip_interface& device, char* activationcodeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ACT_CODE, NULL, 0, REPLY_GET_ACT_CODE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_ACT_CODE, NULL, 0, REPLY_GET_ACT_CODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -367,12 +367,12 @@ void extract(Serializer& serializer, GetModemFirmwareVersion::Response& self) } -CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut) +TypedResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_MODEM_FIRMWARE_VERSION, NULL, 0, REPLY_GET_MODEM_FIRMWARE_VERSION, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_MODEM_FIRMWARE_VERSION, NULL, 0, REPLY_GET_MODEM_FIRMWARE_VERSION, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -417,12 +417,12 @@ void extract(Serializer& serializer, GetRssi::Response& self) } -CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut) +TypedResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_RSSI, NULL, 0, REPLY_GET_RSSI, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_GET_RSSI, NULL, 0, REPLY_GET_RSSI, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -480,7 +480,7 @@ void extract(Serializer& serializer, ServiceStatus::Response& self) } -CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) +TypedResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -492,7 +492,7 @@ CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t r assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SERVICE_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SERVICE_STATUS, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SERVICE_STATUS, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SERVICE_STATUS, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -526,7 +526,7 @@ void extract(Serializer& serializer, ProdEraseStorage& self) } -CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media) +TypedResult prodEraseStorage(C::mip_interface& device, MediaSelector media) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -564,7 +564,7 @@ void extract(Serializer& serializer, LedControl& self) } -CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period) +TypedResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -596,7 +596,7 @@ void extract(Serializer& serializer, ModemHardReset& self) (void)self; } -CmdResult modemHardReset(C::mip_interface& device) +TypedResult modemHardReset(C::mip_interface& device) { return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MODEM_HARD_RESET, NULL, 0); } diff --git a/src/mip/definitions/commands_rtk.hpp b/src/mip/definitions/commands_rtk.hpp index 1b7e145a6..5ee697759 100644 --- a/src/mip/definitions/commands_rtk.hpp +++ b/src/mip/definitions/commands_rtk.hpp @@ -109,7 +109,7 @@ struct GetStatusFlags StatusFlagsLegacy(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } StatusFlagsLegacy& operator=(uint32_t val) { value = val; return *this; } - StatusFlagsLegacy& operator=(int val) { value = val; return *this; } + StatusFlagsLegacy& operator=(int val) { value = uint32_t(val); return *this; } StatusFlagsLegacy& operator|=(uint32_t val) { return *this = value | val; } StatusFlagsLegacy& operator&=(uint32_t val) { return *this = value & val; } @@ -165,7 +165,7 @@ struct GetStatusFlags StatusFlags(int val) : value((uint32_t)val) {} operator uint32_t() const { return value; } StatusFlags& operator=(uint32_t val) { value = val; return *this; } - StatusFlags& operator=(int val) { value = val; return *this; } + StatusFlags& operator=(int val) { value = uint32_t(val); return *this; } StatusFlags& operator|=(uint32_t val) { return *this = value | val; } StatusFlags& operator&=(uint32_t val) { return *this = value & val; } @@ -202,6 +202,8 @@ struct GetStatusFlags static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_STATUS_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetStatusFlags"; + static constexpr const char* DOC_NAME = "Get RTK Device Status Flags"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -221,6 +223,8 @@ struct GetStatusFlags static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_STATUS_FLAGS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetStatusFlags::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device Status Flags Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -239,7 +243,7 @@ void extract(Serializer& serializer, GetStatusFlags& self); void insert(Serializer& serializer, const GetStatusFlags::Response& self); void extract(Serializer& serializer, GetStatusFlags::Response& self); -CmdResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut); +TypedResult getStatusFlags(C::mip_interface& device, GetStatusFlags::StatusFlags* flagsOut); ///@} /// @@ -254,6 +258,8 @@ struct GetImei static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMEI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImei"; + static constexpr const char* DOC_NAME = "Get RTK Device IMEI (International Mobile Equipment Identifier)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -273,6 +279,8 @@ struct GetImei static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMEI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImei::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device IMEI (International Mobile Equipment Identifier) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -291,7 +299,7 @@ void extract(Serializer& serializer, GetImei& self); void insert(Serializer& serializer, const GetImei::Response& self); void extract(Serializer& serializer, GetImei::Response& self); -CmdResult getImei(C::mip_interface& device, char* imeiOut); +TypedResult getImei(C::mip_interface& device, char* imeiOut); ///@} /// @@ -306,6 +314,8 @@ struct GetImsi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_IMSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImsi"; + static constexpr const char* DOC_NAME = "Get RTK Device IMSI (International Mobile Subscriber Identifier)"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -325,6 +335,8 @@ struct GetImsi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_IMSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetImsi::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device IMSI (International Mobile Subscriber Identifier) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -343,7 +355,7 @@ void extract(Serializer& serializer, GetImsi& self); void insert(Serializer& serializer, const GetImsi::Response& self); void extract(Serializer& serializer, GetImsi::Response& self); -CmdResult getImsi(C::mip_interface& device, char* imsiOut); +TypedResult getImsi(C::mip_interface& device, char* imsiOut); ///@} /// @@ -358,6 +370,8 @@ struct GetIccid static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ICCID; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetIccid"; + static constexpr const char* DOC_NAME = "Get RTK Device ICCID (Integrated Circuit Card Identification [SIM Number])"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -377,6 +391,8 @@ struct GetIccid static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ICCID; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetIccid::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device ICCID (Integrated Circuit Card Identification [SIM Number]) Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -395,7 +411,7 @@ void extract(Serializer& serializer, GetIccid& self); void insert(Serializer& serializer, const GetIccid::Response& self); void extract(Serializer& serializer, GetIccid::Response& self); -CmdResult getIccid(C::mip_interface& device, char* iccidOut); +TypedResult getIccid(C::mip_interface& device, char* iccidOut); ///@} /// @@ -418,6 +434,8 @@ struct ConnectedDeviceType static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONNECTED_DEVICE_TYPE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConnectedDeviceType"; + static constexpr const char* DOC_NAME = "Configure or read the type of the connected device"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -450,6 +468,8 @@ struct ConnectedDeviceType static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_CONNECTED_DEVICE_TYPE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ConnectedDeviceType::Response"; + static constexpr const char* DOC_NAME = "Configure or read the type of the connected device Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -468,11 +488,11 @@ void extract(Serializer& serializer, ConnectedDeviceType& self); void insert(Serializer& serializer, const ConnectedDeviceType::Response& self); void extract(Serializer& serializer, ConnectedDeviceType::Response& self); -CmdResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype); -CmdResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut); -CmdResult saveConnectedDeviceType(C::mip_interface& device); -CmdResult loadConnectedDeviceType(C::mip_interface& device); -CmdResult defaultConnectedDeviceType(C::mip_interface& device); +TypedResult writeConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type devtype); +TypedResult readConnectedDeviceType(C::mip_interface& device, ConnectedDeviceType::Type* devtypeOut); +TypedResult saveConnectedDeviceType(C::mip_interface& device); +TypedResult loadConnectedDeviceType(C::mip_interface& device); +TypedResult defaultConnectedDeviceType(C::mip_interface& device); ///@} /// @@ -487,6 +507,8 @@ struct GetActCode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_ACT_CODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetActCode"; + static constexpr const char* DOC_NAME = "Get RTK Device Activation Code"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -506,6 +528,8 @@ struct GetActCode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_ACT_CODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetActCode::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device Activation Code Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -524,7 +548,7 @@ void extract(Serializer& serializer, GetActCode& self); void insert(Serializer& serializer, const GetActCode::Response& self); void extract(Serializer& serializer, GetActCode::Response& self); -CmdResult getActCode(C::mip_interface& device, char* activationcodeOut); +TypedResult getActCode(C::mip_interface& device, char* activationcodeOut); ///@} /// @@ -539,6 +563,8 @@ struct GetModemFirmwareVersion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_MODEM_FIRMWARE_VERSION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetModemFirmwareVersion"; + static constexpr const char* DOC_NAME = "Get RTK Device's Cell Modem Firmware version number"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -558,6 +584,8 @@ struct GetModemFirmwareVersion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_MODEM_FIRMWARE_VERSION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetModemFirmwareVersion::Response"; + static constexpr const char* DOC_NAME = "Get RTK Device's Cell Modem Firmware version number Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -576,7 +604,7 @@ void extract(Serializer& serializer, GetModemFirmwareVersion& self); void insert(Serializer& serializer, const GetModemFirmwareVersion::Response& self); void extract(Serializer& serializer, GetModemFirmwareVersion::Response& self); -CmdResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut); +TypedResult getModemFirmwareVersion(C::mip_interface& device, char* modemfirmwareversionOut); ///@} /// @@ -592,6 +620,8 @@ struct GetRssi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_GET_RSSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetRssi"; + static constexpr const char* DOC_NAME = "GetRssi"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -611,6 +641,8 @@ struct GetRssi static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_GET_RSSI; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GetRssi::Response"; + static constexpr const char* DOC_NAME = "GetRssi Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -631,7 +663,7 @@ void extract(Serializer& serializer, GetRssi& self); void insert(Serializer& serializer, const GetRssi::Response& self); void extract(Serializer& serializer, GetRssi::Response& self); -CmdResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut); +TypedResult getRssi(C::mip_interface& device, bool* validOut, int32_t* rssiOut, int32_t* signalqualityOut); ///@} /// @@ -659,7 +691,7 @@ struct ServiceStatus ServiceFlags(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } ServiceFlags& operator=(uint8_t val) { value = val; return *this; } - ServiceFlags& operator=(int val) { value = val; return *this; } + ServiceFlags& operator=(int val) { value = uint8_t(val); return *this; } ServiceFlags& operator|=(uint8_t val) { return *this = value | val; } ServiceFlags& operator&=(uint8_t val) { return *this = value & val; } @@ -680,6 +712,8 @@ struct ServiceStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_SERVICE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ServiceStatus"; + static constexpr const char* DOC_NAME = "ServiceStatus"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -699,6 +733,8 @@ struct ServiceStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::REPLY_SERVICE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ServiceStatus::Response"; + static constexpr const char* DOC_NAME = "ServiceStatus Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -720,7 +756,7 @@ void extract(Serializer& serializer, ServiceStatus& self); void insert(Serializer& serializer, const ServiceStatus::Response& self); void extract(Serializer& serializer, ServiceStatus::Response& self); -CmdResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); +TypedResult serviceStatus(C::mip_interface& device, uint32_t reserved1, uint32_t reserved2, ServiceStatus::ServiceFlags* flagsOut, uint32_t* receivedbytesOut, uint32_t* lastbytesOut, uint64_t* lastbytestimeOut); ///@} /// @@ -738,6 +774,8 @@ struct ProdEraseStorage static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_PROD_ERASE_STORAGE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ProdEraseStorage"; + static constexpr const char* DOC_NAME = "ProdEraseStorage"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -756,7 +794,7 @@ struct ProdEraseStorage void insert(Serializer& serializer, const ProdEraseStorage& self); void extract(Serializer& serializer, ProdEraseStorage& self); -CmdResult prodEraseStorage(C::mip_interface& device, MediaSelector media); +TypedResult prodEraseStorage(C::mip_interface& device, MediaSelector media); ///@} /// @@ -776,6 +814,8 @@ struct LedControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LedControl"; + static constexpr const char* DOC_NAME = "LedControl"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -794,7 +834,7 @@ struct LedControl void insert(Serializer& serializer, const LedControl& self); void extract(Serializer& serializer, LedControl& self); -CmdResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period); +TypedResult ledControl(C::mip_interface& device, const uint8_t* primarycolor, const uint8_t* altcolor, LedAction act, uint32_t period); ///@} /// @@ -811,6 +851,8 @@ struct ModemHardReset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_rtk::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_rtk::CMD_MODEM_HARD_RESET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ModemHardReset"; + static constexpr const char* DOC_NAME = "ModemHardReset"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -829,7 +871,7 @@ struct ModemHardReset void insert(Serializer& serializer, const ModemHardReset& self); void extract(Serializer& serializer, ModemHardReset& self); -CmdResult modemHardReset(C::mip_interface& device); +TypedResult modemHardReset(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/commands_system.cpp b/src/mip/definitions/commands_system.cpp index 749941ce0..748a9c184 100644 --- a/src/mip/definitions/commands_system.cpp +++ b/src/mip/definitions/commands_system.cpp @@ -61,7 +61,7 @@ void extract(Serializer& serializer, CommMode::Response& self) } -CmdResult writeCommMode(C::mip_interface& device, uint8_t mode) +TypedResult writeCommMode(C::mip_interface& device, uint8_t mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -73,7 +73,7 @@ CmdResult writeCommMode(C::mip_interface& device, uint8_t mode) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer)); } -CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut) +TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -82,7 +82,7 @@ CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut) assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - CmdResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COM_MODE, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_COM_MODE, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_COM_MODE, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -96,7 +96,7 @@ CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut) } return result; } -CmdResult defaultCommMode(C::mip_interface& device) +TypedResult defaultCommMode(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_system.hpp b/src/mip/definitions/commands_system.hpp index dd49bc142..90b1e88fa 100644 --- a/src/mip/definitions/commands_system.hpp +++ b/src/mip/definitions/commands_system.hpp @@ -77,6 +77,8 @@ struct CommMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::CMD_COM_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommMode"; + static constexpr const char* DOC_NAME = "CommMode"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8001; @@ -109,6 +111,8 @@ struct CommMode static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_system::REPLY_COM_MODE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CommMode::Response"; + static constexpr const char* DOC_NAME = "CommMode Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -127,9 +131,9 @@ void extract(Serializer& serializer, CommMode& self); void insert(Serializer& serializer, const CommMode::Response& self); void extract(Serializer& serializer, CommMode::Response& self); -CmdResult writeCommMode(C::mip_interface& device, uint8_t mode); -CmdResult readCommMode(C::mip_interface& device, uint8_t* modeOut); -CmdResult defaultCommMode(C::mip_interface& device); +TypedResult writeCommMode(C::mip_interface& device, uint8_t mode); +TypedResult readCommMode(C::mip_interface& device, uint8_t* modeOut); +TypedResult defaultCommMode(C::mip_interface& device); ///@} /// diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index a6926e3c5..5b2b04fc5 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -162,7 +162,7 @@ struct FilterStatusFlags : Bitfield FilterStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } FilterStatusFlags& operator=(uint16_t val) { value = val; return *this; } - FilterStatusFlags& operator=(int val) { value = val; return *this; } + FilterStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } FilterStatusFlags& operator|=(uint16_t val) { return *this = value | val; } FilterStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -262,7 +262,7 @@ struct FilterMeasurementIndicator : Bitfield FilterMeasurementIndicator(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } FilterMeasurementIndicator& operator=(uint8_t val) { value = val; return *this; } - FilterMeasurementIndicator& operator=(int val) { value = val; return *this; } + FilterMeasurementIndicator& operator=(int val) { value = uint8_t(val); return *this; } FilterMeasurementIndicator& operator|=(uint8_t val) { return *this = value | val; } FilterMeasurementIndicator& operator&=(uint8_t val) { return *this = value & val; } @@ -312,7 +312,7 @@ struct GnssAidStatusFlags : Bitfield GnssAidStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } GnssAidStatusFlags& operator=(uint16_t val) { value = val; return *this; } - GnssAidStatusFlags& operator=(int val) { value = val; return *this; } + GnssAidStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } GnssAidStatusFlags& operator|=(uint16_t val) { return *this = value | val; } GnssAidStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -374,6 +374,8 @@ struct PositionLlh static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PositionLlh"; + static constexpr const char* DOC_NAME = "LLH Position"; auto as_tuple() const @@ -408,6 +410,8 @@ struct VelocityNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelocityNed"; + static constexpr const char* DOC_NAME = "VelocityNed"; auto as_tuple() const @@ -448,6 +452,8 @@ struct AttitudeQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AttitudeQuaternion"; + static constexpr const char* DOC_NAME = "AttitudeQuaternion"; auto as_tuple() const @@ -490,6 +496,8 @@ struct AttitudeDcm static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AttitudeDcm"; + static constexpr const char* DOC_NAME = "AttitudeDcm"; auto as_tuple() const @@ -525,6 +533,8 @@ struct EulerAngles static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_EULER_ANGLES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EulerAngles"; + static constexpr const char* DOC_NAME = "EulerAngles"; auto as_tuple() const @@ -557,6 +567,8 @@ struct GyroBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBias"; + static constexpr const char* DOC_NAME = "GyroBias"; auto as_tuple() const @@ -589,6 +601,8 @@ struct AccelBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBias"; + static constexpr const char* DOC_NAME = "AccelBias"; auto as_tuple() const @@ -623,6 +637,8 @@ struct PositionLlhUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_POS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PositionLlhUncertainty"; + static constexpr const char* DOC_NAME = "LLH Position Uncertainty"; auto as_tuple() const @@ -657,6 +673,8 @@ struct VelocityNedUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_VEL_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelocityNedUncertainty"; + static constexpr const char* DOC_NAME = "NED Velocity Uncertainty"; auto as_tuple() const @@ -692,6 +710,8 @@ struct EulerAnglesUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_EULER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EulerAnglesUncertainty"; + static constexpr const char* DOC_NAME = "EulerAnglesUncertainty"; auto as_tuple() const @@ -724,6 +744,8 @@ struct GyroBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroBiasUncertainty"; + static constexpr const char* DOC_NAME = "GyroBiasUncertainty"; auto as_tuple() const @@ -756,6 +778,8 @@ struct AccelBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelBiasUncertainty"; + static constexpr const char* DOC_NAME = "AccelBiasUncertainty"; auto as_tuple() const @@ -795,6 +819,8 @@ struct Timestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_TIMESTAMP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Timestamp"; + static constexpr const char* DOC_NAME = "Timestamp"; auto as_tuple() const @@ -828,6 +854,8 @@ struct Status static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FILTER_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Status"; + static constexpr const char* DOC_NAME = "Status"; auto as_tuple() const @@ -861,6 +889,8 @@ struct LinearAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_LINEAR_ACCELERATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "LinearAccel"; + static constexpr const char* DOC_NAME = "LinearAccel"; auto as_tuple() const @@ -893,6 +923,8 @@ struct GravityVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GRAVITY_VECTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GravityVector"; + static constexpr const char* DOC_NAME = "GravityVector"; auto as_tuple() const @@ -925,6 +957,8 @@ struct CompAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ACCELERATION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompAccel"; + static constexpr const char* DOC_NAME = "Compensated Acceleration"; auto as_tuple() const @@ -957,6 +991,8 @@ struct CompAngularRate static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_COMPENSATED_ANGULAR_RATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompAngularRate"; + static constexpr const char* DOC_NAME = "CompAngularRate"; auto as_tuple() const @@ -989,6 +1025,8 @@ struct QuaternionAttitudeUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ATT_UNCERTAINTY_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "QuaternionAttitudeUncertainty"; + static constexpr const char* DOC_NAME = "QuaternionAttitudeUncertainty"; auto as_tuple() const @@ -1021,6 +1059,8 @@ struct Wgs84GravityMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_WGS84_GRAVITY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Wgs84GravityMag"; + static constexpr const char* DOC_NAME = "Wgs84GravityMag"; auto as_tuple() const @@ -1067,6 +1107,8 @@ struct HeadingUpdateState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEADING_UPDATE_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadingUpdateState"; + static constexpr const char* DOC_NAME = "HeadingUpdateState"; auto as_tuple() const @@ -1104,6 +1146,8 @@ struct MagneticModel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAGNETIC_MODEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagneticModel"; + static constexpr const char* DOC_NAME = "MagneticModel"; auto as_tuple() const @@ -1136,6 +1180,8 @@ struct AccelScaleFactor static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelScaleFactor"; + static constexpr const char* DOC_NAME = "AccelScaleFactor"; auto as_tuple() const @@ -1168,6 +1214,8 @@ struct AccelScaleFactorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ACCEL_SCALE_FACTOR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AccelScaleFactorUncertainty"; + static constexpr const char* DOC_NAME = "AccelScaleFactorUncertainty"; auto as_tuple() const @@ -1200,6 +1248,8 @@ struct GyroScaleFactor static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroScaleFactor"; + static constexpr const char* DOC_NAME = "GyroScaleFactor"; auto as_tuple() const @@ -1232,6 +1282,8 @@ struct GyroScaleFactorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GYRO_SCALE_FACTOR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GyroScaleFactorUncertainty"; + static constexpr const char* DOC_NAME = "GyroScaleFactorUncertainty"; auto as_tuple() const @@ -1264,6 +1316,8 @@ struct MagBias static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagBias"; + static constexpr const char* DOC_NAME = "MagBias"; auto as_tuple() const @@ -1296,6 +1350,8 @@ struct MagBiasUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_BIAS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagBiasUncertainty"; + static constexpr const char* DOC_NAME = "MagBiasUncertainty"; auto as_tuple() const @@ -1334,6 +1390,8 @@ struct StandardAtmosphere static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_STANDARD_ATMOSPHERE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "StandardAtmosphere"; + static constexpr const char* DOC_NAME = "StandardAtmosphere"; auto as_tuple() const @@ -1370,6 +1428,8 @@ struct PressureAltitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_PRESSURE_ALTITUDE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PressureAltitude"; + static constexpr const char* DOC_NAME = "PressureAltitude"; auto as_tuple() const @@ -1401,6 +1461,8 @@ struct DensityAltitude static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_DENSITY_ALTITUDE_DATA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DensityAltitude"; + static constexpr const char* DOC_NAME = "DensityAltitude"; auto as_tuple() const @@ -1435,6 +1497,8 @@ struct AntennaOffsetCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffsetCorrection"; + static constexpr const char* DOC_NAME = "AntennaOffsetCorrection"; auto as_tuple() const @@ -1467,6 +1531,8 @@ struct AntennaOffsetCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AntennaOffsetCorrectionUncertainty"; + static constexpr const char* DOC_NAME = "AntennaOffsetCorrectionUncertainty"; auto as_tuple() const @@ -1502,6 +1568,8 @@ struct MultiAntennaOffsetCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffsetCorrection"; + static constexpr const char* DOC_NAME = "MultiAntennaOffsetCorrection"; auto as_tuple() const @@ -1535,6 +1603,8 @@ struct MultiAntennaOffsetCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MULTI_ANTENNA_OFFSET_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MultiAntennaOffsetCorrectionUncertainty"; + static constexpr const char* DOC_NAME = "MultiAntennaOffsetCorrectionUncertainty"; auto as_tuple() const @@ -1569,6 +1639,8 @@ struct MagnetometerOffset static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerOffset"; + static constexpr const char* DOC_NAME = "MagnetometerOffset"; auto as_tuple() const @@ -1603,6 +1675,8 @@ struct MagnetometerMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerMatrix"; + static constexpr const char* DOC_NAME = "MagnetometerMatrix"; auto as_tuple() const @@ -1635,6 +1709,8 @@ struct MagnetometerOffsetUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_OFFSET_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerOffsetUncertainty"; + static constexpr const char* DOC_NAME = "MagnetometerOffsetUncertainty"; auto as_tuple() const @@ -1667,6 +1743,8 @@ struct MagnetometerMatrixUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COMPENSATION_MATRIX_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerMatrixUncertainty"; + static constexpr const char* DOC_NAME = "MagnetometerMatrixUncertainty"; auto as_tuple() const @@ -1698,6 +1776,8 @@ struct MagnetometerCovarianceMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_COVARIANCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerCovarianceMatrix"; + static constexpr const char* DOC_NAME = "MagnetometerCovarianceMatrix"; auto as_tuple() const @@ -1730,6 +1810,8 @@ struct MagnetometerResidualVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_MAG_RESIDUAL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "MagnetometerResidualVector"; + static constexpr const char* DOC_NAME = "MagnetometerResidualVector"; auto as_tuple() const @@ -1764,6 +1846,8 @@ struct ClockCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockCorrection"; + static constexpr const char* DOC_NAME = "ClockCorrection"; auto as_tuple() const @@ -1798,6 +1882,8 @@ struct ClockCorrectionUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_CLOCK_CORRECTION_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockCorrectionUncertainty"; + static constexpr const char* DOC_NAME = "ClockCorrectionUncertainty"; auto as_tuple() const @@ -1832,6 +1918,8 @@ struct GnssPosAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_POS_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssPosAidStatus"; + static constexpr const char* DOC_NAME = "GNSS Position Aiding Status"; auto as_tuple() const @@ -1865,6 +1953,8 @@ struct GnssAttAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_ATT_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssAttAidStatus"; + static constexpr const char* DOC_NAME = "GNSS Attitude Aiding Status"; auto as_tuple() const @@ -1904,6 +1994,8 @@ struct HeadAidStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_HEAD_AID_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HeadAidStatus"; + static constexpr const char* DOC_NAME = "HeadAidStatus"; auto as_tuple() const @@ -1936,6 +2028,8 @@ struct RelPosNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_REL_POS_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RelPosNed"; + static constexpr const char* DOC_NAME = "NED Relative Position"; auto as_tuple() const @@ -1968,6 +2062,8 @@ struct EcefPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefPos"; + static constexpr const char* DOC_NAME = "ECEF Position"; auto as_tuple() const @@ -2000,6 +2096,8 @@ struct EcefVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefVel"; + static constexpr const char* DOC_NAME = "ECEF Velocity"; auto as_tuple() const @@ -2032,6 +2130,8 @@ struct EcefPosUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_POS_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefPosUncertainty"; + static constexpr const char* DOC_NAME = "ECEF Position Uncertainty"; auto as_tuple() const @@ -2064,6 +2164,8 @@ struct EcefVelUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ECEF_VEL_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EcefVelUncertainty"; + static constexpr const char* DOC_NAME = "ECEF Velocity Uncertainty"; auto as_tuple() const @@ -2098,6 +2200,8 @@ struct AidingMeasurementSummary static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_AID_MEAS_SUMMARY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingMeasurementSummary"; + static constexpr const char* DOC_NAME = "AidingMeasurementSummary"; auto as_tuple() const @@ -2130,6 +2234,8 @@ struct OdometerScaleFactorError static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OdometerScaleFactorError"; + static constexpr const char* DOC_NAME = "Odometer Scale Factor Error"; auto as_tuple() const @@ -2162,6 +2268,8 @@ struct OdometerScaleFactorErrorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_ODOMETER_SCALE_FACTOR_ERROR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OdometerScaleFactorErrorUncertainty"; + static constexpr const char* DOC_NAME = "Odometer Scale Factor Error Uncertainty"; auto as_tuple() const @@ -2211,7 +2319,7 @@ struct GnssDualAntennaStatus DualAntennaStatusFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } DualAntennaStatusFlags& operator=(uint16_t val) { value = val; return *this; } - DualAntennaStatusFlags& operator=(int val) { value = val; return *this; } + DualAntennaStatusFlags& operator=(int val) { value = uint16_t(val); return *this; } DualAntennaStatusFlags& operator|=(uint16_t val) { return *this = value | val; } DualAntennaStatusFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2236,6 +2344,8 @@ struct GnssDualAntennaStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_GNSS_DUAL_ANTENNA_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GnssDualAntennaStatus"; + static constexpr const char* DOC_NAME = "GNSS Dual Antenna Status"; auto as_tuple() const @@ -2271,6 +2381,8 @@ struct AidingFrameConfigError static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingFrameConfigError"; + static constexpr const char* DOC_NAME = "Aiding Frame Configuration Error"; auto as_tuple() const @@ -2306,6 +2418,8 @@ struct AidingFrameConfigErrorUncertainty static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_filter::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_filter::DATA_FRAME_CONFIG_ERROR_UNCERTAINTY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "AidingFrameConfigErrorUncertainty"; + static constexpr const char* DOC_NAME = "Aiding Frame Configuration Error Uncertainty"; auto as_tuple() const diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index bab43b3bd..5a59b0704 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -195,7 +195,7 @@ struct PosLlh ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -227,6 +227,8 @@ struct PosLlh static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PosLlh"; + static constexpr const char* DOC_NAME = "GNSS LLH Position"; auto as_tuple() const @@ -269,7 +271,7 @@ struct PosEcef ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -291,6 +293,8 @@ struct PosEcef static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_POSITION_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PosEcef"; + static constexpr const char* DOC_NAME = "GNSS ECEF Position"; auto as_tuple() const @@ -337,7 +341,7 @@ struct VelNed ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -371,6 +375,8 @@ struct VelNed static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelNed"; + static constexpr const char* DOC_NAME = "NED Velocity"; auto as_tuple() const @@ -413,7 +419,7 @@ struct VelEcef ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -435,6 +441,8 @@ struct VelEcef static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_VELOCITY_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "VelEcef"; + static constexpr const char* DOC_NAME = "GNSS ECEF Velocity"; auto as_tuple() const @@ -482,7 +490,7 @@ struct Dop ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -519,6 +527,8 @@ struct Dop static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DOP; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Dop"; + static constexpr const char* DOC_NAME = "Dop"; auto as_tuple() const @@ -561,7 +571,7 @@ struct UtcTime ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -588,6 +598,8 @@ struct UtcTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_UTC_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UtcTime"; + static constexpr const char* DOC_NAME = "UtcTime"; auto as_tuple() const @@ -630,7 +642,7 @@ struct GpsTime ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -652,6 +664,8 @@ struct GpsTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTime"; + static constexpr const char* DOC_NAME = "GpsTime"; auto as_tuple() const @@ -695,7 +709,7 @@ struct ClockInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -720,6 +734,8 @@ struct ClockInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockInfo"; + static constexpr const char* DOC_NAME = "ClockInfo"; auto as_tuple() const @@ -772,7 +788,7 @@ struct FixInfo FixFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } FixFlags& operator=(uint16_t val) { value = val; return *this; } - FixFlags& operator=(int val) { value = val; return *this; } + FixFlags& operator=(int val) { value = uint16_t(val); return *this; } FixFlags& operator|=(uint16_t val) { return *this = value | val; } FixFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -802,7 +818,7 @@ struct FixInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -827,6 +843,8 @@ struct FixInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_FIX_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "FixInfo"; + static constexpr const char* DOC_NAME = "FixInfo"; auto as_tuple() const @@ -870,7 +888,7 @@ struct SvInfo SVFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } SVFlags& operator=(uint16_t val) { value = val; return *this; } - SVFlags& operator=(int val) { value = val; return *this; } + SVFlags& operator=(int val) { value = uint16_t(val); return *this; } SVFlags& operator|=(uint16_t val) { return *this = value | val; } SVFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -903,7 +921,7 @@ struct SvInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -937,6 +955,8 @@ struct SvInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SV_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SvInfo"; + static constexpr const char* DOC_NAME = "SvInfo"; auto as_tuple() const @@ -1003,7 +1023,7 @@ struct HwStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1028,6 +1048,8 @@ struct HwStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_HW_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "HwStatus"; + static constexpr const char* DOC_NAME = "GNSS Hardware Status"; auto as_tuple() const @@ -1084,7 +1106,7 @@ struct DgpsInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1112,6 +1134,8 @@ struct DgpsInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DgpsInfo"; + static constexpr const char* DOC_NAME = "DgpsInfo"; auto as_tuple() const @@ -1158,7 +1182,7 @@ struct DgpsChannel ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1186,6 +1210,8 @@ struct DgpsChannel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_DGPS_CHANNEL_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DgpsChannel"; + static constexpr const char* DOC_NAME = "DgpsChannel"; auto as_tuple() const @@ -1232,7 +1258,7 @@ struct ClockInfo2 ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1260,6 +1286,8 @@ struct ClockInfo2 static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_CLOCK_INFO_2; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ClockInfo2"; + static constexpr const char* DOC_NAME = "ClockInfo2"; auto as_tuple() const @@ -1300,7 +1328,7 @@ struct GpsLeapSeconds ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1317,6 +1345,8 @@ struct GpsLeapSeconds static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_LEAP_SECONDS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsLeapSeconds"; + static constexpr const char* DOC_NAME = "GpsLeapSeconds"; auto as_tuple() const @@ -1360,7 +1390,7 @@ struct SbasInfo SbasStatus(int val) : value((uint8_t)val) {} operator uint8_t() const { return value; } SbasStatus& operator=(uint8_t val) { value = val; return *this; } - SbasStatus& operator=(int val) { value = val; return *this; } + SbasStatus& operator=(int val) { value = uint8_t(val); return *this; } SbasStatus& operator|=(uint8_t val) { return *this = value | val; } SbasStatus& operator&=(uint8_t val) { return *this = value & val; } @@ -1397,7 +1427,7 @@ struct SbasInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1431,6 +1461,8 @@ struct SbasInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SbasInfo"; + static constexpr const char* DOC_NAME = "SbasInfo"; auto as_tuple() const @@ -1496,7 +1528,7 @@ struct SbasCorrection ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1527,6 +1559,8 @@ struct SbasCorrection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SBAS_CORRECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SbasCorrection"; + static constexpr const char* DOC_NAME = "SbasCorrection"; auto as_tuple() const @@ -1594,7 +1628,7 @@ struct RfErrorDetection ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1620,6 +1654,8 @@ struct RfErrorDetection static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RF_ERROR_DETECTION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RfErrorDetection"; + static constexpr const char* DOC_NAME = "RfErrorDetection"; auto as_tuple() const @@ -1670,7 +1706,7 @@ struct BaseStationInfo IndicatorFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } IndicatorFlags& operator=(uint16_t val) { value = val; return *this; } - IndicatorFlags& operator=(int val) { value = val; return *this; } + IndicatorFlags& operator=(int val) { value = uint16_t(val); return *this; } IndicatorFlags& operator|=(uint16_t val) { return *this = value | val; } IndicatorFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1717,7 +1753,7 @@ struct BaseStationInfo ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1751,6 +1787,8 @@ struct BaseStationInfo static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_BASE_STATION_INFO; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BaseStationInfo"; + static constexpr const char* DOC_NAME = "BaseStationInfo"; auto as_tuple() const @@ -1798,7 +1836,7 @@ struct RtkCorrectionsStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1847,7 +1885,7 @@ struct RtkCorrectionsStatus EpochStatus(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } EpochStatus& operator=(uint16_t val) { value = val; return *this; } - EpochStatus& operator=(int val) { value = val; return *this; } + EpochStatus& operator=(int val) { value = uint16_t(val); return *this; } EpochStatus& operator|=(uint16_t val) { return *this = value | val; } EpochStatus& operator&=(uint16_t val) { return *this = value & val; } @@ -1888,6 +1926,8 @@ struct RtkCorrectionsStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RTK_CORRECTIONS_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RtkCorrectionsStatus"; + static constexpr const char* DOC_NAME = "RtkCorrectionsStatus"; auto as_tuple() const @@ -1935,7 +1975,7 @@ struct SatelliteStatus ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -1974,6 +2014,8 @@ struct SatelliteStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_SATELLITE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "SatelliteStatus"; + static constexpr const char* DOC_NAME = "SatelliteStatus"; auto as_tuple() const @@ -2040,7 +2082,7 @@ struct Raw ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2106,6 +2148,8 @@ struct Raw static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Raw"; + static constexpr const char* DOC_NAME = "Raw"; auto as_tuple() const @@ -2148,7 +2192,7 @@ struct GpsEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2201,6 +2245,8 @@ struct GpsEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsEphemeris"; + static constexpr const char* DOC_NAME = "GpsEphemeris"; auto as_tuple() const @@ -2243,7 +2289,7 @@ struct GalileoEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2296,6 +2342,8 @@ struct GalileoEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GalileoEphemeris"; + static constexpr const char* DOC_NAME = "GalileoEphemeris"; auto as_tuple() const @@ -2337,7 +2385,7 @@ struct GloEphemeris ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2379,6 +2427,8 @@ struct GloEphemeris static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GLONASS_EPHEMERIS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GloEphemeris"; + static constexpr const char* DOC_NAME = "Glonass Ephemeris"; auto as_tuple() const @@ -2423,7 +2473,7 @@ struct GpsIonoCorr ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2451,6 +2501,8 @@ struct GpsIonoCorr static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GPS_IONO_CORR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsIonoCorr"; + static constexpr const char* DOC_NAME = "GPS Ionospheric Correction"; auto as_tuple() const @@ -2495,7 +2547,7 @@ struct GalileoIonoCorr ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -2523,6 +2575,8 @@ struct GalileoIonoCorr static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_gnss::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_gnss::DATA_GALILEO_IONO_CORR; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GalileoIonoCorr"; + static constexpr const char* DOC_NAME = "Galileo Ionospheric Correction"; auto as_tuple() const diff --git a/src/mip/definitions/data_sensor.hpp b/src/mip/definitions/data_sensor.hpp index be97a142e..dde268718 100644 --- a/src/mip/definitions/data_sensor.hpp +++ b/src/mip/definitions/data_sensor.hpp @@ -84,6 +84,8 @@ struct RawAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawAccel"; + static constexpr const char* DOC_NAME = "RawAccel"; auto as_tuple() const @@ -116,6 +118,8 @@ struct RawGyro static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawGyro"; + static constexpr const char* DOC_NAME = "RawGyro"; auto as_tuple() const @@ -148,6 +152,8 @@ struct RawMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawMag"; + static constexpr const char* DOC_NAME = "RawMag"; auto as_tuple() const @@ -180,6 +186,8 @@ struct RawPressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "RawPressure"; + static constexpr const char* DOC_NAME = "RawPressure"; auto as_tuple() const @@ -212,6 +220,8 @@ struct ScaledAccel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ACCEL_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledAccel"; + static constexpr const char* DOC_NAME = "ScaledAccel"; auto as_tuple() const @@ -244,6 +254,8 @@ struct ScaledGyro static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_GYRO_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledGyro"; + static constexpr const char* DOC_NAME = "ScaledGyro"; auto as_tuple() const @@ -276,6 +288,8 @@ struct ScaledMag static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_MAG_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledMag"; + static constexpr const char* DOC_NAME = "ScaledMag"; auto as_tuple() const @@ -307,6 +321,8 @@ struct ScaledPressure static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_PRESSURE_SCALED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ScaledPressure"; + static constexpr const char* DOC_NAME = "ScaledPressure"; auto as_tuple() const @@ -339,6 +355,8 @@ struct DeltaTheta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_THETA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaTheta"; + static constexpr const char* DOC_NAME = "DeltaTheta"; auto as_tuple() const @@ -371,6 +389,8 @@ struct DeltaVelocity static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_DELTA_VELOCITY; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaVelocity"; + static constexpr const char* DOC_NAME = "DeltaVelocity"; auto as_tuple() const @@ -412,6 +432,8 @@ struct CompOrientationMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompOrientationMatrix"; + static constexpr const char* DOC_NAME = "Complementary Filter Orientation Matrix"; auto as_tuple() const @@ -451,6 +473,8 @@ struct CompQuaternion static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_QUATERNION; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompQuaternion"; + static constexpr const char* DOC_NAME = "Complementary Filter Quaternion"; auto as_tuple() const @@ -485,6 +509,8 @@ struct CompEulerAngles static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_EULER_ANGLES; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompEulerAngles"; + static constexpr const char* DOC_NAME = "Complementary Filter Euler Angles"; auto as_tuple() const @@ -516,6 +542,8 @@ struct CompOrientationUpdateMatrix static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_COMP_ORIENTATION_UPDATE_MATRIX; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "CompOrientationUpdateMatrix"; + static constexpr const char* DOC_NAME = "Complementary Filter Orientation Update Matrix"; auto as_tuple() const @@ -547,6 +575,8 @@ struct OrientationRawTemp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_RAW; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OrientationRawTemp"; + static constexpr const char* DOC_NAME = "OrientationRawTemp"; auto as_tuple() const @@ -578,6 +608,8 @@ struct InternalTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_INTERNAL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "InternalTimestamp"; + static constexpr const char* DOC_NAME = "InternalTimestamp"; auto as_tuple() const @@ -610,6 +642,8 @@ struct PpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_PPS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "PpsTimestamp"; + static constexpr const char* DOC_NAME = "PPS Timestamp"; auto as_tuple() const @@ -660,7 +694,7 @@ struct GpsTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -686,6 +720,8 @@ struct GpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TIME_STAMP_GPS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTimestamp"; + static constexpr const char* DOC_NAME = "GpsTimestamp"; auto as_tuple() const @@ -723,6 +759,8 @@ struct TemperatureAbs static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_TEMPERATURE_ABS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TemperatureAbs"; + static constexpr const char* DOC_NAME = "Temperature Statistics"; auto as_tuple() const @@ -760,6 +798,8 @@ struct UpVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_ACCEL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "UpVector"; + static constexpr const char* DOC_NAME = "UpVector"; auto as_tuple() const @@ -794,6 +834,8 @@ struct NorthVector static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_STAB_MAG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "NorthVector"; + static constexpr const char* DOC_NAME = "NorthVector"; auto as_tuple() const @@ -842,7 +884,7 @@ struct OverrangeStatus Status(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } Status& operator=(uint16_t val) { value = val; return *this; } - Status& operator=(int val) { value = val; return *this; } + Status& operator=(int val) { value = uint16_t(val); return *this; } Status& operator|=(uint16_t val) { return *this = value | val; } Status& operator&=(uint16_t val) { return *this = value & val; } @@ -876,6 +918,8 @@ struct OverrangeStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_OVERRANGE_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OverrangeStatus"; + static constexpr const char* DOC_NAME = "OverrangeStatus"; auto as_tuple() const @@ -908,6 +952,8 @@ struct OdometerData static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_sensor::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_sensor::DATA_ODOMETER; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "OdometerData"; + static constexpr const char* DOC_NAME = "OdometerData"; auto as_tuple() const diff --git a/src/mip/definitions/data_shared.hpp b/src/mip/definitions/data_shared.hpp index be95ce3fa..05df1eddf 100644 --- a/src/mip/definitions/data_shared.hpp +++ b/src/mip/definitions/data_shared.hpp @@ -71,6 +71,8 @@ struct EventSource static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EVENT_SOURCE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "EventSource"; + static constexpr const char* DOC_NAME = "EventSource"; auto as_tuple() const @@ -105,6 +107,8 @@ struct Ticks static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_TICKS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Ticks"; + static constexpr const char* DOC_NAME = "Ticks"; auto as_tuple() const @@ -140,6 +144,8 @@ struct DeltaTicks static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TICKS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaTicks"; + static constexpr const char* DOC_NAME = "DeltaTicks"; auto as_tuple() const @@ -185,7 +191,7 @@ struct GpsTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -207,6 +213,8 @@ struct GpsTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_GPS_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpsTimestamp"; + static constexpr const char* DOC_NAME = "GpsTimestamp"; auto as_tuple() const @@ -247,6 +255,8 @@ struct DeltaTime static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_DELTA_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "DeltaTime"; + static constexpr const char* DOC_NAME = "DeltaTime"; auto as_tuple() const @@ -285,6 +295,8 @@ struct ReferenceTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REFERENCE_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceTimestamp"; + static constexpr const char* DOC_NAME = "ReferenceTimestamp"; auto as_tuple() const @@ -325,6 +337,8 @@ struct ReferenceTimeDelta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_REF_TIME_DELTA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ReferenceTimeDelta"; + static constexpr const char* DOC_NAME = "ReferenceTimeDelta"; auto as_tuple() const @@ -373,7 +387,7 @@ struct ExternalTimestamp ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -390,6 +404,8 @@ struct ExternalTimestamp static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_EXTERNAL_TIME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalTimestamp"; + static constexpr const char* DOC_NAME = "ExternalTimestamp"; auto as_tuple() const @@ -442,7 +458,7 @@ struct ExternalTimeDelta ValidFlags(int val) : value((uint16_t)val) {} operator uint16_t() const { return value; } ValidFlags& operator=(uint16_t val) { value = val; return *this; } - ValidFlags& operator=(int val) { value = val; return *this; } + ValidFlags& operator=(int val) { value = uint16_t(val); return *this; } ValidFlags& operator|=(uint16_t val) { return *this = value | val; } ValidFlags& operator&=(uint16_t val) { return *this = value & val; } @@ -459,6 +475,8 @@ struct ExternalTimeDelta static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_shared::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_shared::DATA_SYS_TIME_DELTA; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "ExternalTimeDelta"; + static constexpr const char* DOC_NAME = "ExternalTimeDelta"; auto as_tuple() const diff --git a/src/mip/definitions/data_system.hpp b/src/mip/definitions/data_system.hpp index 80b457773..17170999f 100644 --- a/src/mip/definitions/data_system.hpp +++ b/src/mip/definitions/data_system.hpp @@ -79,6 +79,8 @@ struct BuiltInTest static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_BUILT_IN_TEST; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "BuiltInTest"; + static constexpr const char* DOC_NAME = "BuiltInTest"; auto as_tuple() const @@ -111,6 +113,8 @@ struct TimeSyncStatus static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_TIME_SYNC_STATUS; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "TimeSyncStatus"; + static constexpr const char* DOC_NAME = "TimeSyncStatus"; auto as_tuple() const @@ -160,6 +164,8 @@ struct GpioState static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_STATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioState"; + static constexpr const char* DOC_NAME = "GpioState"; auto as_tuple() const @@ -193,6 +199,8 @@ struct GpioAnalogValue static constexpr const uint8_t DESCRIPTOR_SET = ::mip::data_system::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::data_system::DATA_GPIO_ANALOG_VALUE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "GpioAnalogValue"; + static constexpr const char* DOC_NAME = "GpioAnalogValue"; auto as_tuple() const From 0eee5645dd2ecb8caa15419ccd7b17744cae72ff Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Tue, 20 Feb 2024 16:41:58 +0000 Subject: [PATCH 206/252] Generate MIP definitions from branches/dev@56028. --- src/mip/definitions/commands_aiding.c | 193 +++++------------------- src/mip/definitions/commands_aiding.cpp | 182 +++++----------------- src/mip/definitions/commands_aiding.h | 72 +++------ src/mip/definitions/commands_aiding.hpp | 151 +++++------------- 4 files changed, 126 insertions(+), 472 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 1f9327b6d..ab0a9c6c8 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -57,127 +57,6 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb // Mip Fields //////////////////////////////////////////////////////////////////////////////// -void insert_mip_aiding_sensor_frame_mapping_command(mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_command* self) -{ - insert_mip_function_selector(serializer, self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - insert_u8(serializer, self->sensor_id); - - insert_u8(serializer, self->frame_id); - - } -} -void extract_mip_aiding_sensor_frame_mapping_command(mip_serializer* serializer, mip_aiding_sensor_frame_mapping_command* self) -{ - extract_mip_function_selector(serializer, &self->function); - - if( self->function == MIP_FUNCTION_WRITE ) - { - extract_u8(serializer, &self->sensor_id); - - extract_u8(serializer, &self->frame_id); - - } -} - -void insert_mip_aiding_sensor_frame_mapping_response(mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_response* self) -{ - insert_u8(serializer, self->sensor_id); - - insert_u8(serializer, self->frame_id); - -} -void extract_mip_aiding_sensor_frame_mapping_response(mip_serializer* serializer, mip_aiding_sensor_frame_mapping_response* self) -{ - extract_u8(serializer, &self->sensor_id); - - extract_u8(serializer, &self->frame_id); - -} - -mip_cmd_result mip_aiding_write_sensor_frame_mapping(struct mip_interface* device, uint8_t sensor_id, uint8_t frame_id) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - - insert_u8(&serializer, sensor_id); - - insert_u8(&serializer, frame_id); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_aiding_read_sensor_frame_mapping(struct mip_interface* device, uint8_t* sensor_id_out, uint8_t* frame_id_out) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_READ); - - assert(mip_serializer_is_ok(&serializer)); - - uint8_t responseLength = sizeof(buffer); - mip_cmd_result result = mip_interface_run_command_with_response(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), MIP_REPLY_DESC_AIDING_SENSOR_FRAME_MAP, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - mip_serializer deserializer; - mip_serializer_init_insertion(&deserializer, buffer, responseLength); - - assert(sensor_id_out); - extract_u8(&deserializer, sensor_id_out); - - assert(frame_id_out); - extract_u8(&deserializer, frame_id_out); - - if( mip_serializer_remaining(&deserializer) != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -mip_cmd_result mip_aiding_save_sensor_frame_mapping(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_SAVE); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_aiding_load_sensor_frame_mapping(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_LOAD); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -mip_cmd_result mip_aiding_default_sensor_frame_mapping(struct mip_interface* device) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - insert_mip_function_selector(&serializer, MIP_FUNCTION_RESET); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const mip_aiding_reference_frame_command* self) { insert_mip_function_selector(serializer, self->function); @@ -527,7 +406,7 @@ void insert_mip_aiding_ecef_pos_command(mip_serializer* serializer, const mip_ai { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); for(unsigned int i=0; i < 3; i++) insert_double(serializer, self->position[i]); @@ -542,7 +421,7 @@ void extract_mip_aiding_ecef_pos_command(mip_serializer* serializer, mip_aiding_ { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); for(unsigned int i=0; i < 3; i++) extract_double(serializer, &self->position[i]); @@ -565,7 +444,7 @@ void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* seri *self = tmp; } -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -574,7 +453,7 @@ mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); assert(position || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -594,7 +473,7 @@ void insert_mip_aiding_llh_pos_command(mip_serializer* serializer, const mip_aid { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); insert_double(serializer, self->latitude); @@ -612,7 +491,7 @@ void extract_mip_aiding_llh_pos_command(mip_serializer* serializer, mip_aiding_l { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); extract_double(serializer, &self->latitude); @@ -638,7 +517,7 @@ void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -647,7 +526,7 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); insert_double(&serializer, latitude); @@ -669,7 +548,7 @@ void insert_mip_aiding_height_command(mip_serializer* serializer, const mip_aidi { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); insert_float(serializer, self->height); @@ -682,7 +561,7 @@ void extract_mip_aiding_height_command(mip_serializer* serializer, mip_aiding_he { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); extract_float(serializer, &self->height); @@ -692,7 +571,7 @@ void extract_mip_aiding_height_command(mip_serializer* serializer, mip_aiding_he } -mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float height, float uncertainty, uint16_t valid_flags) +mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float height, float uncertainty, uint16_t valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -701,7 +580,7 @@ mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* t assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); insert_float(&serializer, height); @@ -717,7 +596,7 @@ void insert_mip_aiding_pressure_command(mip_serializer* serializer, const mip_ai { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); insert_float(serializer, self->pressure); @@ -730,7 +609,7 @@ void extract_mip_aiding_pressure_command(mip_serializer* serializer, mip_aiding_ { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); extract_float(serializer, &self->pressure); @@ -740,7 +619,7 @@ void extract_mip_aiding_pressure_command(mip_serializer* serializer, mip_aiding_ } -mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float pressure, float uncertainty, uint16_t valid_flags) +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float pressure, float uncertainty, uint16_t valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -749,7 +628,7 @@ mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); insert_float(&serializer, pressure); @@ -765,7 +644,7 @@ void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_ai { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->velocity[i]); @@ -780,7 +659,7 @@ void extract_mip_aiding_ecef_vel_command(mip_serializer* serializer, mip_aiding_ { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->velocity[i]); @@ -803,7 +682,7 @@ void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* seri *self = tmp; } -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -812,7 +691,7 @@ mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -832,7 +711,7 @@ void insert_mip_aiding_ned_vel_command(mip_serializer* serializer, const mip_aid { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->velocity[i]); @@ -847,7 +726,7 @@ void extract_mip_aiding_ned_vel_command(mip_serializer* serializer, mip_aiding_n { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->velocity[i]); @@ -870,7 +749,7 @@ void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -879,7 +758,7 @@ mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -899,7 +778,7 @@ void insert_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* seri { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->velocity[i]); @@ -914,7 +793,7 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* ser { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->velocity[i]); @@ -937,7 +816,7 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct *self = tmp; } -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -946,7 +825,7 @@ mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* dev assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -966,7 +845,7 @@ void insert_mip_aiding_true_heading_command(mip_serializer* serializer, const mi { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); insert_float(serializer, self->heading); @@ -979,7 +858,7 @@ void extract_mip_aiding_true_heading_command(mip_serializer* serializer, mip_aid { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); extract_float(serializer, &self->heading); @@ -989,7 +868,7 @@ void extract_mip_aiding_true_heading_command(mip_serializer* serializer, mip_aid } -mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags) +mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float heading, float uncertainty, uint16_t valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -998,7 +877,7 @@ mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_t assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); insert_float(&serializer, heading); @@ -1014,7 +893,7 @@ void insert_mip_aiding_magnetic_field_command(mip_serializer* serializer, const { insert_mip_time(serializer, &self->time); - insert_u8(serializer, self->sensor_id); + insert_u8(serializer, self->frame_id); for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->magnetic_field[i]); @@ -1029,7 +908,7 @@ void extract_mip_aiding_magnetic_field_command(mip_serializer* serializer, mip_a { extract_mip_time(serializer, &self->time); - extract_u8(serializer, &self->sensor_id); + extract_u8(serializer, &self->frame_id); for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->magnetic_field[i]); @@ -1052,7 +931,7 @@ void extract_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer *self = tmp; } -mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -1061,7 +940,7 @@ mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip assert(time); insert_mip_time(&serializer, time); - insert_u8(&serializer, sensor_id); + insert_u8(&serializer, frame_id); assert(magnetic_field || (3 == 0)); for(unsigned int i=0; i < 3; i++) diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 9981eb802..1a5a72720 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -48,116 +48,6 @@ void extract(Serializer& serializer, Time& self) // Mip Fields //////////////////////////////////////////////////////////////////////////////// -void insert(Serializer& serializer, const SensorFrameMapping& self) -{ - insert(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - insert(serializer, self.sensor_id); - - insert(serializer, self.frame_id); - - } -} -void extract(Serializer& serializer, SensorFrameMapping& self) -{ - extract(serializer, self.function); - - if( self.function == FunctionSelector::WRITE ) - { - extract(serializer, self.sensor_id); - - extract(serializer, self.frame_id); - - } -} - -void insert(Serializer& serializer, const SensorFrameMapping::Response& self) -{ - insert(serializer, self.sensor_id); - - insert(serializer, self.frame_id); - -} -void extract(Serializer& serializer, SensorFrameMapping::Response& self) -{ - extract(serializer, self.sensor_id); - - extract(serializer, self.frame_id); - -} - -TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::WRITE); - insert(serializer, sensorId); - - insert(serializer, frameId); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::READ); - assert(serializer.isOk()); - - uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_SENSOR_FRAME_MAP, buffer, &responseLength); - - if( result == MIP_ACK_OK ) - { - Serializer deserializer(buffer, responseLength); - - assert(sensorIdOut); - extract(deserializer, *sensorIdOut); - - assert(frameIdOut); - extract(deserializer, *frameIdOut); - - if( deserializer.remaining() != 0 ) - result = MIP_STATUS_ERROR; - } - return result; -} -TypedResult saveSensorFrameMapping(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::SAVE); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -TypedResult loadSensorFrameMapping(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::LOAD); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} -TypedResult defaultSensorFrameMapping(C::mip_interface& device) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, FunctionSelector::RESET); - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_SENSOR_FRAME_MAP, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert(Serializer& serializer, const ReferenceFrame& self) { insert(serializer, self.function); @@ -463,7 +353,7 @@ void insert(Serializer& serializer, const EcefPos& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) insert(serializer, self.position[i]); @@ -478,7 +368,7 @@ void extract(Serializer& serializer, EcefPos& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) extract(serializer, self.position[i]); @@ -490,14 +380,14 @@ void extract(Serializer& serializer, EcefPos& self) } -TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t frameId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); assert(position || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -517,7 +407,7 @@ void insert(Serializer& serializer, const LlhPos& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); insert(serializer, self.latitude); @@ -535,7 +425,7 @@ void extract(Serializer& serializer, LlhPos& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); extract(serializer, self.latitude); @@ -550,14 +440,14 @@ void extract(Serializer& serializer, LlhPos& self) } -TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t frameId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); insert(serializer, latitude); @@ -579,7 +469,7 @@ void insert(Serializer& serializer, const Height& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); insert(serializer, self.height); @@ -592,7 +482,7 @@ void extract(Serializer& serializer, Height& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); extract(serializer, self.height); @@ -602,14 +492,14 @@ void extract(Serializer& serializer, Height& self) } -TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags) +TypedResult height(C::mip_interface& device, const Time& time, uint8_t frameId, float height, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); insert(serializer, height); @@ -625,7 +515,7 @@ void insert(Serializer& serializer, const Pressure& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); insert(serializer, self.pressure); @@ -638,7 +528,7 @@ void extract(Serializer& serializer, Pressure& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); extract(serializer, self.pressure); @@ -648,14 +538,14 @@ void extract(Serializer& serializer, Pressure& self) } -TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags) +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t frameId, float pressure, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); insert(serializer, pressure); @@ -671,7 +561,7 @@ void insert(Serializer& serializer, const EcefVel& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) insert(serializer, self.velocity[i]); @@ -686,7 +576,7 @@ void extract(Serializer& serializer, EcefVel& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) extract(serializer, self.velocity[i]); @@ -698,14 +588,14 @@ void extract(Serializer& serializer, EcefVel& self) } -TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -725,7 +615,7 @@ void insert(Serializer& serializer, const NedVel& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) insert(serializer, self.velocity[i]); @@ -740,7 +630,7 @@ void extract(Serializer& serializer, NedVel& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) extract(serializer, self.velocity[i]); @@ -752,14 +642,14 @@ void extract(Serializer& serializer, NedVel& self) } -TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -779,7 +669,7 @@ void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) insert(serializer, self.velocity[i]); @@ -794,7 +684,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) extract(serializer, self.velocity[i]); @@ -806,14 +696,14 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); assert(velocity || (3 == 0)); for(unsigned int i=0; i < 3; i++) @@ -833,7 +723,7 @@ void insert(Serializer& serializer, const TrueHeading& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); insert(serializer, self.heading); @@ -846,7 +736,7 @@ void extract(Serializer& serializer, TrueHeading& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); extract(serializer, self.heading); @@ -856,14 +746,14 @@ void extract(Serializer& serializer, TrueHeading& self) } -TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags) +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t frameId, float heading, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); insert(serializer, heading); @@ -879,7 +769,7 @@ void insert(Serializer& serializer, const MagneticField& self) { insert(serializer, self.time); - insert(serializer, self.sensor_id); + insert(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) insert(serializer, self.magnetic_field[i]); @@ -894,7 +784,7 @@ void extract(Serializer& serializer, MagneticField& self) { extract(serializer, self.time); - extract(serializer, self.sensor_id); + extract(serializer, self.frame_id); for(unsigned int i=0; i < 3; i++) extract(serializer, self.magnetic_field[i]); @@ -906,14 +796,14 @@ void extract(Serializer& serializer, MagneticField& self) } -TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t frameId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); insert(serializer, time); - insert(serializer, sensorId); + insert(serializer, frameId); assert(magneticField || (3 == 0)); for(unsigned int i=0; i < 3; i++) diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index f8386599d..cc7bc1894 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -34,7 +34,6 @@ enum MIP_AIDING_CMD_DESC_SET = 0x13, MIP_CMD_DESC_AIDING_FRAME_CONFIG = 0x01, - MIP_CMD_DESC_AIDING_SENSOR_FRAME_MAP = 0x02, MIP_CMD_DESC_AIDING_LOCAL_FRAME = 0x03, MIP_CMD_DESC_AIDING_ECHO_CONTROL = 0x1F, MIP_CMD_DESC_AIDING_POS_LOCAL = 0x20, @@ -53,7 +52,6 @@ enum MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x39, MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x3A, - MIP_REPLY_DESC_AIDING_SENSOR_FRAME_MAP = 0x82, MIP_REPLY_DESC_AIDING_FRAME_CONFIG = 0x81, MIP_REPLY_DESC_AIDING_ECHO_CONTROL = 0x9F, }; @@ -86,40 +84,6 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb // Mip Fields //////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_sensor_frame_mapping (0x13,0x02) Sensor Frame Mapping [C] -/// -///@{ - -struct mip_aiding_sensor_frame_mapping_command -{ - mip_function_selector function; - uint8_t sensor_id; ///< Sensor ID to configure. Cannot be 0. - uint8_t frame_id; ///< Frame ID to assign to the sensor. Defaults to 1. - -}; -typedef struct mip_aiding_sensor_frame_mapping_command mip_aiding_sensor_frame_mapping_command; -void insert_mip_aiding_sensor_frame_mapping_command(struct mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_command* self); -void extract_mip_aiding_sensor_frame_mapping_command(struct mip_serializer* serializer, mip_aiding_sensor_frame_mapping_command* self); - -struct mip_aiding_sensor_frame_mapping_response -{ - uint8_t sensor_id; ///< Sensor ID to configure. Cannot be 0. - uint8_t frame_id; ///< Frame ID to assign to the sensor. Defaults to 1. - -}; -typedef struct mip_aiding_sensor_frame_mapping_response mip_aiding_sensor_frame_mapping_response; -void insert_mip_aiding_sensor_frame_mapping_response(struct mip_serializer* serializer, const mip_aiding_sensor_frame_mapping_response* self); -void extract_mip_aiding_sensor_frame_mapping_response(struct mip_serializer* serializer, mip_aiding_sensor_frame_mapping_response* self); - -mip_cmd_result mip_aiding_write_sensor_frame_mapping(struct mip_interface* device, uint8_t sensor_id, uint8_t frame_id); -mip_cmd_result mip_aiding_read_sensor_frame_mapping(struct mip_interface* device, uint8_t* sensor_id_out, uint8_t* frame_id_out); -mip_cmd_result mip_aiding_save_sensor_frame_mapping(struct mip_interface* device); -mip_cmd_result mip_aiding_load_sensor_frame_mapping(struct mip_interface* device); -mip_cmd_result mip_aiding_default_sensor_frame_mapping(struct mip_interface* device); - -///@} -/// //////////////////////////////////////////////////////////////////////////////// ///@defgroup c_aiding_reference_frame (0x13,0x01) Reference Frame [C] /// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command @@ -251,7 +215,7 @@ static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND struct mip_aiding_ecef_pos_command { mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. + uint8_t frame_id; ///< Sensor ID. mip_vector3d position; ///< ECEF position [m]. mip_vector3f uncertainty; ///< ECEF position uncertainty [m]. mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. @@ -264,7 +228,7 @@ void extract_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self); void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); ///@} /// @@ -285,7 +249,7 @@ static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_V struct mip_aiding_llh_pos_command { mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. + uint8_t frame_id; ///< Sensor ID. double latitude; ///< [deg] double longitude; ///< [deg] double height; ///< [m] @@ -300,7 +264,7 @@ void extract_mip_aiding_llh_pos_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self); void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self); -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); ///@} /// @@ -313,7 +277,7 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* struct mip_aiding_height_command { mip_time time; - uint8_t sensor_id; + uint8_t frame_id; float height; ///< [m] float uncertainty; ///< [m] uint16_t valid_flags; @@ -323,7 +287,7 @@ typedef struct mip_aiding_height_command mip_aiding_height_command; void insert_mip_aiding_height_command(struct mip_serializer* serializer, const mip_aiding_height_command* self); void extract_mip_aiding_height_command(struct mip_serializer* serializer, mip_aiding_height_command* self); -mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float height, float uncertainty, uint16_t valid_flags); +mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float height, float uncertainty, uint16_t valid_flags); ///@} /// @@ -336,7 +300,7 @@ mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* t struct mip_aiding_pressure_command { mip_time time; - uint8_t sensor_id; + uint8_t frame_id; float pressure; ///< [mbar] float uncertainty; ///< [mbar] uint16_t valid_flags; @@ -346,7 +310,7 @@ typedef struct mip_aiding_pressure_command mip_aiding_pressure_command; void insert_mip_aiding_pressure_command(struct mip_serializer* serializer, const mip_aiding_pressure_command* self); void extract_mip_aiding_pressure_command(struct mip_serializer* serializer, mip_aiding_pressure_command* self); -mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float pressure, float uncertainty, uint16_t valid_flags); +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float pressure, float uncertainty, uint16_t valid_flags); ///@} /// @@ -366,7 +330,7 @@ static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND struct mip_aiding_ecef_vel_command { mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. + uint8_t frame_id; ///< Sensor ID. mip_vector3f velocity; ///< ECEF velocity [m/s]. mip_vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. @@ -379,7 +343,7 @@ void extract_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, mip_ void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self); void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); ///@} /// @@ -399,7 +363,7 @@ static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_V struct mip_aiding_ned_vel_command { mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Sensor ID. + uint8_t frame_id; ///< Sensor ID. mip_vector3f velocity; ///< NED velocity [m/s]. mip_vector3f uncertainty; ///< NED velocity uncertainty [m/s]. mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. @@ -412,7 +376,7 @@ void extract_mip_aiding_ned_vel_command(struct mip_serializer* serializer, mip_a void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self); void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self); -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); ///@} /// @@ -433,7 +397,7 @@ static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AID struct mip_aiding_vehicle_fixed_frame_velocity_command { mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) mip_vector3f velocity; ///< [m/s] mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; @@ -446,7 +410,7 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializ void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self); void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self); -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); ///@} /// @@ -458,7 +422,7 @@ mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* dev struct mip_aiding_true_heading_command { mip_time time; - uint8_t sensor_id; + uint8_t frame_id; float heading; ///< Heading in [radians] float uncertainty; uint16_t valid_flags; @@ -468,7 +432,7 @@ typedef struct mip_aiding_true_heading_command mip_aiding_true_heading_command; void insert_mip_aiding_true_heading_command(struct mip_serializer* serializer, const mip_aiding_true_heading_command* self); void extract_mip_aiding_true_heading_command(struct mip_serializer* serializer, mip_aiding_true_heading_command* self); -mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, float heading, float uncertainty, uint16_t valid_flags); +mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float heading, float uncertainty, uint16_t valid_flags); ///@} /// @@ -488,7 +452,7 @@ static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_F struct mip_aiding_magnetic_field_command { mip_time time; ///< Timestamp of the measurement. - uint8_t sensor_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) mip_vector3f magnetic_field; ///< [G] mip_vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) mip_aiding_magnetic_field_command_valid_flags valid_flags; @@ -501,7 +465,7 @@ void extract_mip_aiding_magnetic_field_command(struct mip_serializer* serializer void insert_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_magnetic_field_command_valid_flags self); void extract_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer* serializer, mip_aiding_magnetic_field_command_valid_flags* self); -mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t sensor_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags); ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 28ffd2617..2ed8d408a 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -33,7 +33,6 @@ enum DESCRIPTOR_SET = 0x13, CMD_FRAME_CONFIG = 0x01, - CMD_SENSOR_FRAME_MAP = 0x02, CMD_LOCAL_FRAME = 0x03, CMD_ECHO_CONTROL = 0x1F, CMD_POS_LOCAL = 0x20, @@ -52,7 +51,6 @@ enum CMD_DELTA_ATTITUDE = 0x39, CMD_LOCAL_ANGULAR_RATE = 0x3A, - REPLY_SENSOR_FRAME_MAP = 0x82, REPLY_FRAME_CONFIG = 0x81, REPLY_ECHO_CONTROL = 0x9F, }; @@ -83,83 +81,6 @@ void extract(Serializer& serializer, Time& self); // Mip Fields //////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_sensor_frame_mapping (0x13,0x02) Sensor Frame Mapping [CPP] -/// -///@{ - -struct SensorFrameMapping -{ - FunctionSelector function = static_cast(0); - uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. - uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. - - static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_SENSOR_FRAME_MAP; - static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorFrameMapping"; - static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping"; - - static constexpr const bool HAS_FUNCTION_SELECTOR = true; - static constexpr const uint32_t WRITE_PARAMS = 0x8003; - static constexpr const uint32_t READ_PARAMS = 0x8000; - static constexpr const uint32_t SAVE_PARAMS = 0x8000; - static constexpr const uint32_t LOAD_PARAMS = 0x8000; - static constexpr const uint32_t DEFAULT_PARAMS = 0x8000; - static constexpr const uint32_t ECHOED_PARAMS = 0x0000; - static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; - - auto as_tuple() const - { - return std::make_tuple(sensor_id,frame_id); - } - - auto as_tuple() - { - return std::make_tuple(std::ref(sensor_id),std::ref(frame_id)); - } - - static SensorFrameMapping create_sld_all(::mip::FunctionSelector function) - { - SensorFrameMapping cmd; - cmd.function = function; - return cmd; - } - - struct Response - { - static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_SENSOR_FRAME_MAP; - static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "SensorFrameMapping::Response"; - static constexpr const char* DOC_NAME = "Sensor ID to Frame Mapping Response"; - - static constexpr const uint32_t ECHOED_PARAMS = 0x0000; - static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; - uint8_t sensor_id = 0; ///< Sensor ID to configure. Cannot be 0. - uint8_t frame_id = 0; ///< Frame ID to assign to the sensor. Defaults to 1. - - - auto as_tuple() - { - return std::make_tuple(std::ref(sensor_id),std::ref(frame_id)); - } - }; -}; -void insert(Serializer& serializer, const SensorFrameMapping& self); -void extract(Serializer& serializer, SensorFrameMapping& self); - -void insert(Serializer& serializer, const SensorFrameMapping::Response& self); -void extract(Serializer& serializer, SensorFrameMapping::Response& self); - -TypedResult writeSensorFrameMapping(C::mip_interface& device, uint8_t sensorId, uint8_t frameId); -TypedResult readSensorFrameMapping(C::mip_interface& device, uint8_t* sensorIdOut, uint8_t* frameIdOut); -TypedResult saveSensorFrameMapping(C::mip_interface& device); -TypedResult loadSensorFrameMapping(C::mip_interface& device); -TypedResult defaultSensorFrameMapping(C::mip_interface& device); - -///@} -/// //////////////////////////////////////////////////////////////////////////////// ///@defgroup cpp_aiding_reference_frame (0x13,0x01) Reference Frame [CPP] /// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command @@ -400,7 +321,7 @@ struct EcefPos }; Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Sensor ID. Vector3d position; ///< ECEF position [m]. Vector3f uncertainty; ///< ECEF position uncertainty [m]. ValidFlags valid_flags; ///< Valid flags. @@ -416,19 +337,19 @@ struct EcefPos auto as_tuple() const { - return std::make_tuple(time,sensor_id,position,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,position,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(position),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(position),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const EcefPos& self); void extract(Serializer& serializer, EcefPos& self); -TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t sensorId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); +TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t frameId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); ///@} /// @@ -473,7 +394,7 @@ struct LlhPos }; Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Sensor ID. double latitude = 0; ///< [deg] double longitude = 0; ///< [deg] double height = 0; ///< [m] @@ -491,19 +412,19 @@ struct LlhPos auto as_tuple() const { - return std::make_tuple(time,sensor_id,latitude,longitude,height,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,latitude,longitude,height,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(latitude),std::ref(longitude),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const LlhPos& self); void extract(Serializer& serializer, LlhPos& self); -TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t sensorId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t frameId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); ///@} /// @@ -516,7 +437,7 @@ TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t s struct Height { Time time; - uint8_t sensor_id = 0; + uint8_t frame_id = 0; float height = 0; ///< [m] float uncertainty = 0; ///< [m] uint16_t valid_flags = 0; @@ -532,19 +453,19 @@ struct Height auto as_tuple() const { - return std::make_tuple(time,sensor_id,height,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,height,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(height),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const Height& self); void extract(Serializer& serializer, Height& self); -TypedResult height(C::mip_interface& device, const Time& time, uint8_t sensorId, float height, float uncertainty, uint16_t validFlags); +TypedResult height(C::mip_interface& device, const Time& time, uint8_t frameId, float height, float uncertainty, uint16_t validFlags); ///@} /// @@ -557,7 +478,7 @@ TypedResult height(C::mip_interface& device, const Time& time, uint8_t s struct Pressure { Time time; - uint8_t sensor_id = 0; + uint8_t frame_id = 0; float pressure = 0; ///< [mbar] float uncertainty = 0; ///< [mbar] uint16_t valid_flags = 0; @@ -573,19 +494,19 @@ struct Pressure auto as_tuple() const { - return std::make_tuple(time,sensor_id,pressure,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,pressure,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(pressure),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(pressure),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const Pressure& self); void extract(Serializer& serializer, Pressure& self); -TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t sensorId, float pressure, float uncertainty, uint16_t validFlags); +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t frameId, float pressure, float uncertainty, uint16_t validFlags); ///@} /// @@ -629,7 +550,7 @@ struct EcefVel }; Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Sensor ID. Vector3f velocity; ///< ECEF velocity [m/s]. Vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. @@ -645,19 +566,19 @@ struct EcefVel auto as_tuple() const { - return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,velocity,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const EcefVel& self); void extract(Serializer& serializer, EcefVel& self); -TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); +TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); ///@} /// @@ -701,7 +622,7 @@ struct NedVel }; Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Sensor ID. Vector3f velocity; ///< NED velocity [m/s]. Vector3f uncertainty; ///< NED velocity uncertainty [m/s]. ValidFlags valid_flags; ///< Valid flags. @@ -717,19 +638,19 @@ struct NedVel auto as_tuple() const { - return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,velocity,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const NedVel& self); void extract(Serializer& serializer, NedVel& self); -TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); +TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); ///@} /// @@ -774,7 +695,7 @@ struct VehicleFixedFrameVelocity }; Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) Vector3f velocity; ///< [m/s] Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) ValidFlags valid_flags; @@ -790,19 +711,19 @@ struct VehicleFixedFrameVelocity auto as_tuple() const { - return std::make_tuple(time,sensor_id,velocity,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,velocity,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(velocity),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); -TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); ///@} /// @@ -814,7 +735,7 @@ TypedResult vehicleFixedFrameVelocity(C::mip_interfac struct TrueHeading { Time time; - uint8_t sensor_id = 0; + uint8_t frame_id = 0; float heading = 0; ///< Heading in [radians] float uncertainty = 0; uint16_t valid_flags = 0; @@ -830,19 +751,19 @@ struct TrueHeading auto as_tuple() const { - return std::make_tuple(time,sensor_id,heading,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,heading,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(heading),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(heading),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const TrueHeading& self); void extract(Serializer& serializer, TrueHeading& self); -TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t sensorId, float heading, float uncertainty, uint16_t validFlags); +TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t frameId, float heading, float uncertainty, uint16_t validFlags); ///@} /// @@ -886,7 +807,7 @@ struct MagneticField }; Time time; ///< Timestamp of the measurement. - uint8_t sensor_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) Vector3f magnetic_field; ///< [G] Vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) ValidFlags valid_flags; @@ -902,19 +823,19 @@ struct MagneticField auto as_tuple() const { - return std::make_tuple(time,sensor_id,magnetic_field,uncertainty,valid_flags); + return std::make_tuple(time,frame_id,magnetic_field,uncertainty,valid_flags); } auto as_tuple() { - return std::make_tuple(std::ref(time),std::ref(sensor_id),std::ref(magnetic_field),std::ref(uncertainty),std::ref(valid_flags)); + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(magnetic_field),std::ref(uncertainty),std::ref(valid_flags)); } typedef void Response; }; void insert(Serializer& serializer, const MagneticField& self); void extract(Serializer& serializer, MagneticField& self); -TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t sensorId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); +TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t frameId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); ///@} /// From f89b82d00699d158c5f9a1d3d74b11f42bc4497e Mon Sep 17 00:00:00 2001 From: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Date: Mon, 26 Feb 2024 14:02:54 -0500 Subject: [PATCH 207/252] Updated Linux build server in Jenkinsfile (#93) Removed Linux ARM builds until server is up and running --- Jenkinsfile | 40 ++++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 6cba57559..667aa328b 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -68,7 +68,7 @@ pipeline { } } stage('Ubuntu amd64') { - agent { label 'aws-amd64' } + agent { label 'linux-amd64' } options { skipDefaultCheckout() } steps { checkoutRepo() @@ -77,7 +77,7 @@ pipeline { } } stage('Centos amd64') { - agent { label 'aws-amd64' } + agent { label 'linux-amd64' } options { skipDefaultCheckout() } steps { checkoutRepo() @@ -85,24 +85,24 @@ pipeline { archiveArtifacts artifacts: 'build_centos_amd64/mipsdk_*' } } - stage('Ubuntu arm64') { - agent { label 'aws-arm64' } - options { skipDefaultCheckout() } - steps { - checkoutRepo() - sh "./.devcontainer/docker_build.sh --os ubuntu --arch arm64v8" - archiveArtifacts artifacts: 'build_ubuntu_arm64v8/mipsdk_*' - } - } - stage('Ubuntu arm32') { - agent { label 'aws-arm64' } - options { skipDefaultCheckout() } - steps { - checkoutRepo() - sh "./.devcontainer/docker_build.sh --os ubuntu --arch arm32v7" - archiveArtifacts artifacts: 'build_ubuntu_arm32v7/mipsdk_*' - } - } + // stage('Ubuntu arm64') { + // agent { label 'linux-arm64' } + // options { skipDefaultCheckout() } + // steps { + // checkoutRepo() + // sh "./.devcontainer/docker_build.sh --os ubuntu --arch arm64v8" + // archiveArtifacts artifacts: 'build_ubuntu_arm64v8/mipsdk_*' + // } + // } + // stage('Ubuntu arm32') { + // agent { label 'linux-arm64' } + // options { skipDefaultCheckout() } + // steps { + // checkoutRepo() + // sh "./.devcontainer/docker_build.sh --os ubuntu --arch arm32v7" + // archiveArtifacts artifacts: 'build_ubuntu_arm32v7/mipsdk_*' + // } + // } } } } From b9fc529211fa75d215507f32a23c19e55a6a4bd3 Mon Sep 17 00:00:00 2001 From: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Date: Mon, 26 Feb 2024 15:00:22 -0500 Subject: [PATCH 208/252] Improve Composite Results & Composite Descriptors (#87) * Added to composite result and composite descriptor Added template append function to composite result Added template composite descriptor helper function to create a descriptor from a mip struct Added a comparison helper function to composite descriptor * Added gnss data descriptor utility function * Fixed GNSS data descriptor check * Using new composite descriptor in structs added CL5-10 model Adopted new composite descriptor in all mip structs Removed added composite descriptor template in favor of built-in descriptor to mip structs --- src/mip/definitions/descriptors.c | 12 ++++++++++++ src/mip/definitions/descriptors.h | 2 ++ src/mip/extras/composite_result.hpp | 2 ++ src/mip/extras/device_models.h | 1 + 4 files changed, 17 insertions(+) diff --git a/src/mip/definitions/descriptors.c b/src/mip/definitions/descriptors.c index 1ca874df7..685a3b500 100644 --- a/src/mip/definitions/descriptors.c +++ b/src/mip/definitions/descriptors.c @@ -57,6 +57,18 @@ bool mip_is_reserved_descriptor_set(uint8_t descriptor_set) return (descriptor_set >= MIP_RESERVED_DESCRIPTOR_SET_START); } +//////////////////////////////////////////////////////////////////////////////// +///@brief Determines if the descriptor set represents some kind of GNSS data. +/// +///@param descriptor_set +/// +///@returns true if the descriptor set represents GNSS data. +/// +bool mip_is_gnss_data_descriptor_set(uint8_t descriptor_set) +{ + return ((descriptor_set == 0x81) || ((descriptor_set >= 0x91) && (descriptor_set <= 0x95))); +} + //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/definitions/descriptors.h b/src/mip/definitions/descriptors.h index 4369bdf00..d2a01b23d 100644 --- a/src/mip/definitions/descriptors.h +++ b/src/mip/definitions/descriptors.h @@ -34,6 +34,7 @@ bool mip_is_valid_descriptor_set(uint8_t descriptor_set); bool mip_is_data_descriptor_set(uint8_t descriptor_set); bool mip_is_cmd_descriptor_set(uint8_t descriptor_set); bool mip_is_reserved_descriptor_set(uint8_t descriptor_set); +bool mip_is_gnss_data_descriptor_set(uint8_t descriptor_set); bool mip_is_valid_field_descriptor(uint8_t field_descriptor); bool mip_is_cmd_field_descriptor(uint8_t field_descriptor); @@ -108,6 +109,7 @@ inline bool isValidDescriptorSet (uint8_t descriptorSet) { return C::mip_is_va inline bool isDataDescriptorSet (uint8_t descriptorSet) { return C::mip_is_data_descriptor_set(descriptorSet); } inline bool isCommandDescriptorSet (uint8_t descriptorSet) { return C::mip_is_cmd_descriptor_set(descriptorSet); } inline bool isReservedDescriptorSet(uint8_t descriptorSet) { return C::mip_is_reserved_descriptor_set(descriptorSet); } +inline bool isGnssDataDescriptorSet(uint8_t descriptorSet) { return C::mip_is_gnss_data_descriptor_set(descriptorSet); } inline bool isValidFieldDescriptor (uint8_t fieldDescriptor) { return C::mip_is_valid_field_descriptor(fieldDescriptor); } inline bool isCommandFieldDescriptor (uint8_t fieldDescriptor) { return C::mip_is_cmd_field_descriptor(fieldDescriptor); } diff --git a/src/mip/extras/composite_result.hpp b/src/mip/extras/composite_result.hpp index 82367e4f5..89729a049 100644 --- a/src/mip/extras/composite_result.hpp +++ b/src/mip/extras/composite_result.hpp @@ -73,6 +73,8 @@ namespace mip //void append(bool success, uint8_t id=0) { m_results.push_back({success ? CmdResult::ACK_OK : CmdResult::STATUS_ERROR, 0x0000}); } void append(CmdResult result, CompositeDescriptor desc=0x0000) { m_results.push_back({result, desc}); } void append(Entry result) { m_results.push_back(result); } + template + void append(CmdResult result, uint16_t index=0) { append({result, DescriptorId(MipType::DESCRIPTOR, index)}); } // Append multiple results. void extend(const CompositeResult& other) { m_results.insert(m_results.end(), other.m_results.begin(), other.m_results.end()); } diff --git a/src/mip/extras/device_models.h b/src/mip/extras/device_models.h index da22e542e..5b47ce67b 100644 --- a/src/mip/extras/device_models.h +++ b/src/mip/extras/device_models.h @@ -35,6 +35,7 @@ enum mip_model_number MODEL_3DM_CX5_25 = 6273,// 3DM-CX5-25 MODEL_3DM_CX5_15 = 6274,// 3DM-CX5-15 MODEL_3DM_CX5_10 = 6275,// 3DM-CX5-10 + MODEL_3DM_CL5_10 = 6279,// 3DM-CL5-10 MODEL_3DM_CL5_15 = 6280,// 3DM-CL5-15 MODEL_3DM_CL5_25 = 6281,// 3DM-CL5-25 MODEL_3DM_GQ7 = 6284,// 3DM-GQ7 From 8744de9d68f00b3f5265fb29cb19e2b0e5be80c8 Mon Sep 17 00:00:00 2001 From: nathanmillermicrostrain <69481927+nathanmillermicrostrain@users.noreply.github.com> Date: Wed, 6 Mar 2024 16:25:47 -0500 Subject: [PATCH 209/252] Update CMakeLists.txt --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f23d49a90..ad69bcdb2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,6 +131,7 @@ set(MIP_SOURCES "${MIP_DIR}/definitions/descriptors.h" "${MIP_DIR}/mip.hpp" "${MIP_DIR}/mip_all.h" + "${MIP_DIR}/mip_all.hpp" ) set(MIPDEV_SOURCES From 70ca96cad0908dfac7493034356bffbb5ce303ca Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 11 Mar 2024 19:00:12 +0000 Subject: [PATCH 210/252] Generate MIP definitions from branches/dev@56145. --- src/mip/definitions/commands_aiding.c | 73 +++++++++++++++---------- src/mip/definitions/commands_aiding.cpp | 57 +++++++++++-------- src/mip/definitions/commands_aiding.h | 56 ++++++++++--------- src/mip/definitions/commands_aiding.hpp | 46 ++++++++-------- 4 files changed, 131 insertions(+), 101 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index ab0a9c6c8..9a8f31ccd 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -57,7 +57,7 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb // Mip Fields //////////////////////////////////////////////////////////////////////////////// -void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const mip_aiding_reference_frame_command* self) +void insert_mip_aiding_frame_config_command(mip_serializer* serializer, const mip_aiding_frame_config_command* self) { insert_mip_function_selector(serializer, self->function); @@ -65,7 +65,7 @@ void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const if( self->function == MIP_FUNCTION_WRITE || self->function == MIP_FUNCTION_READ ) { - insert_mip_aiding_reference_frame_command_format(serializer, self->format); + insert_mip_aiding_frame_config_command_format(serializer, self->format); } if( self->function == MIP_FUNCTION_WRITE ) @@ -73,19 +73,21 @@ void insert_mip_aiding_reference_frame_command(mip_serializer* serializer, const for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { insert_mip_vector3f(serializer, &self->rotation.euler); } - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { insert_mip_quatf(serializer, &self->rotation.quaternion); } + insert_bool(serializer, self->tracking_enabled); + } } -void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_aiding_reference_frame_command* self) +void extract_mip_aiding_frame_config_command(mip_serializer* serializer, mip_aiding_frame_config_command* self) { extract_mip_function_selector(serializer, &self->function); @@ -93,7 +95,7 @@ void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_ if( self->function == MIP_FUNCTION_WRITE || self->function == MIP_FUNCTION_READ ) { - extract_mip_aiding_reference_frame_command_format(serializer, &self->format); + extract_mip_aiding_frame_config_command_format(serializer, &self->format); } if( self->function == MIP_FUNCTION_WRITE ) @@ -101,72 +103,78 @@ void extract_mip_aiding_reference_frame_command(mip_serializer* serializer, mip_ for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { extract_mip_vector3f(serializer, &self->rotation.euler); } - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { extract_mip_quatf(serializer, &self->rotation.quaternion); } + extract_bool(serializer, &self->tracking_enabled); + } } -void insert_mip_aiding_reference_frame_response(mip_serializer* serializer, const mip_aiding_reference_frame_response* self) +void insert_mip_aiding_frame_config_response(mip_serializer* serializer, const mip_aiding_frame_config_response* self) { insert_u8(serializer, self->frame_id); - insert_mip_aiding_reference_frame_command_format(serializer, self->format); + insert_mip_aiding_frame_config_command_format(serializer, self->format); for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { insert_mip_vector3f(serializer, &self->rotation.euler); } - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { insert_mip_quatf(serializer, &self->rotation.quaternion); } + insert_bool(serializer, self->tracking_enabled); + } -void extract_mip_aiding_reference_frame_response(mip_serializer* serializer, mip_aiding_reference_frame_response* self) +void extract_mip_aiding_frame_config_response(mip_serializer* serializer, mip_aiding_frame_config_response* self) { extract_u8(serializer, &self->frame_id); - extract_mip_aiding_reference_frame_command_format(serializer, &self->format); + extract_mip_aiding_frame_config_command_format(serializer, &self->format); for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { extract_mip_vector3f(serializer, &self->rotation.euler); } - if( self->format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { extract_mip_quatf(serializer, &self->rotation.quaternion); } + extract_bool(serializer, &self->tracking_enabled); + } -void insert_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, const mip_aiding_reference_frame_command_format self) +void insert_mip_aiding_frame_config_command_format(struct mip_serializer* serializer, const mip_aiding_frame_config_command_format self) { insert_u8(serializer, (uint8_t)(self)); } -void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, mip_aiding_reference_frame_command_format* self) +void extract_mip_aiding_frame_config_command_format(struct mip_serializer* serializer, mip_aiding_frame_config_command_format* self) { uint8_t tmp = 0; extract_u8(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const mip_aiding_reference_frame_command_rotation* rotation) +mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, const float* translation, const mip_aiding_frame_config_command_rotation* rotation, bool tracking_enabled) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -176,27 +184,29 @@ mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, ui insert_u8(&serializer, frame_id); - insert_mip_aiding_reference_frame_command_format(&serializer, format); + insert_mip_aiding_frame_config_command_format(&serializer, format); assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert_float(&serializer, translation[i]); - if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { insert_mip_vector3f(&serializer, &rotation->euler); } - if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { insert_mip_quatf(&serializer, &rotation->quaternion); } + insert_bool(&serializer, tracking_enabled); + assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, mip_aiding_reference_frame_command_rotation* rotation_out) +mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, float* translation_out, mip_aiding_frame_config_command_rotation* rotation_out, bool* tracking_enabled_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -206,7 +216,7 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin insert_u8(&serializer, frame_id); - insert_mip_aiding_reference_frame_command_format(&serializer, format); + insert_mip_aiding_frame_config_command_format(&serializer, format); assert(mip_serializer_is_ok(&serializer)); @@ -220,28 +230,31 @@ mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uin extract_u8(&deserializer, &frame_id); - extract_mip_aiding_reference_frame_command_format(&deserializer, &format); + extract_mip_aiding_frame_config_command_format(&deserializer, &format); assert(translation_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract_float(&deserializer, &translation_out[i]); - if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER ) + if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { extract_mip_vector3f(&deserializer, &rotation_out->euler); } - if( format == MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION ) + if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { extract_mip_quatf(&deserializer, &rotation_out->quaternion); } + assert(tracking_enabled_out); + extract_bool(&deserializer, tracking_enabled_out); + if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id) +mip_cmd_result mip_aiding_save_frame_config(struct mip_interface* device, uint8_t frame_id) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -255,7 +268,7 @@ mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uin return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id) +mip_cmd_result mip_aiding_load_frame_config(struct mip_interface* device, uint8_t frame_id) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -269,7 +282,7 @@ mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uin return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id) +mip_cmd_result mip_aiding_default_frame_config(struct mip_interface* device, uint8_t frame_id) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 1a5a72720..8b528d09a 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -48,7 +48,7 @@ void extract(Serializer& serializer, Time& self) // Mip Fields //////////////////////////////////////////////////////////////////////////////// -void insert(Serializer& serializer, const ReferenceFrame& self) +void insert(Serializer& serializer, const FrameConfig& self) { insert(serializer, self.function); @@ -64,19 +64,21 @@ void insert(Serializer& serializer, const ReferenceFrame& self) for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); - if( self.format == ReferenceFrame::Format::EULER ) + if( self.format == FrameConfig::Format::EULER ) { insert(serializer, self.rotation.euler); } - if( self.format == ReferenceFrame::Format::QUATERNION ) + if( self.format == FrameConfig::Format::QUATERNION ) { insert(serializer, self.rotation.quaternion); } + insert(serializer, self.tracking_enabled); + } } -void extract(Serializer& serializer, ReferenceFrame& self) +void extract(Serializer& serializer, FrameConfig& self) { extract(serializer, self.function); @@ -92,20 +94,22 @@ void extract(Serializer& serializer, ReferenceFrame& self) for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); - if( self.format == ReferenceFrame::Format::EULER ) + if( self.format == FrameConfig::Format::EULER ) { extract(serializer, self.rotation.euler); } - if( self.format == ReferenceFrame::Format::QUATERNION ) + if( self.format == FrameConfig::Format::QUATERNION ) { extract(serializer, self.rotation.quaternion); } + extract(serializer, self.tracking_enabled); + } } -void insert(Serializer& serializer, const ReferenceFrame::Response& self) +void insert(Serializer& serializer, const FrameConfig::Response& self) { insert(serializer, self.frame_id); @@ -114,18 +118,20 @@ void insert(Serializer& serializer, const ReferenceFrame::Response& self) for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); - if( self.format == ReferenceFrame::Format::EULER ) + if( self.format == FrameConfig::Format::EULER ) { insert(serializer, self.rotation.euler); } - if( self.format == ReferenceFrame::Format::QUATERNION ) + if( self.format == FrameConfig::Format::QUATERNION ) { insert(serializer, self.rotation.quaternion); } + insert(serializer, self.tracking_enabled); + } -void extract(Serializer& serializer, ReferenceFrame::Response& self) +void extract(Serializer& serializer, FrameConfig::Response& self) { extract(serializer, self.frame_id); @@ -134,19 +140,21 @@ void extract(Serializer& serializer, ReferenceFrame::Response& self) for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); - if( self.format == ReferenceFrame::Format::EULER ) + if( self.format == FrameConfig::Format::EULER ) { extract(serializer, self.rotation.euler); } - if( self.format == ReferenceFrame::Format::QUATERNION ) + if( self.format == FrameConfig::Format::QUATERNION ) { extract(serializer, self.rotation.quaternion); } + extract(serializer, self.tracking_enabled); + } -TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation) +TypedResult writeFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, const float* translation, const FrameConfig::Rotation& rotation, bool trackingEnabled) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -160,21 +168,23 @@ TypedResult writeReferenceFrame(C::mip_interface& device, uint8_ for(unsigned int i=0; i < 3; i++) insert(serializer, translation[i]); - if( format == ReferenceFrame::Format::EULER ) + if( format == FrameConfig::Format::EULER ) { insert(serializer, rotation.euler); } - if( format == ReferenceFrame::Format::QUATERNION ) + if( format == FrameConfig::Format::QUATERNION ) { insert(serializer, rotation.quaternion); } + insert(serializer, trackingEnabled); + assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut) +TypedResult readFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, float* translationOut, FrameConfig::Rotation* rotationOut, bool* trackingEnabledOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -187,7 +197,7 @@ TypedResult readReferenceFrame(C::mip_interface& device, uint8_t assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_FRAME_CONFIG, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -201,22 +211,25 @@ TypedResult readReferenceFrame(C::mip_interface& device, uint8_t for(unsigned int i=0; i < 3; i++) extract(deserializer, translationOut[i]); - if( format == ReferenceFrame::Format::EULER ) + if( format == FrameConfig::Format::EULER ) { extract(deserializer, rotationOut->euler); } - if( format == ReferenceFrame::Format::QUATERNION ) + if( format == FrameConfig::Format::QUATERNION ) { extract(deserializer, rotationOut->quaternion); } + assert(trackingEnabledOut); + extract(deserializer, *trackingEnabledOut); + if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } return result; } -TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult saveFrameConfig(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -228,7 +241,7 @@ TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult loadFrameConfig(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -240,7 +253,7 @@ TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId) +TypedResult defaultFrameConfig(C::mip_interface& device, uint8_t frameId) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index cc7bc1894..af738e8d0 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -85,8 +85,8 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_reference_frame (0x13,0x01) Reference Frame [C] -/// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command +///@defgroup c_aiding_frame_config (0x13,0x01) Frame Config [C] +/// Defines an aiding frame associated with a specific sensor frame ID. The frame ID used in this command /// should mirror the frame ID used in the aiding command (if that aiding measurement is measured in this reference frame) /// /// This transform satisfies the following relationship: @@ -111,50 +111,52 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb /// ///@{ -typedef uint8_t mip_aiding_reference_frame_command_format; -static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_EULER = 1; ///< Translation vector followed by euler angles (roll, pitch, yaw). -static const mip_aiding_reference_frame_command_format MIP_AIDING_REFERENCE_FRAME_COMMAND_FORMAT_QUATERNION = 2; ///< Translation vector followed by quaternion (w, x, y, z). +typedef uint8_t mip_aiding_frame_config_command_format; +static const mip_aiding_frame_config_command_format MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER = 1; ///< Translation vector followed by euler angles (roll, pitch, yaw). +static const mip_aiding_frame_config_command_format MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION = 2; ///< Translation vector followed by quaternion (w, x, y, z). -union mip_aiding_reference_frame_command_rotation +union mip_aiding_frame_config_command_rotation { mip_vector3f euler; mip_quatf quaternion; }; -typedef union mip_aiding_reference_frame_command_rotation mip_aiding_reference_frame_command_rotation; +typedef union mip_aiding_frame_config_command_rotation mip_aiding_frame_config_command_rotation; -struct mip_aiding_reference_frame_command +struct mip_aiding_frame_config_command { mip_function_selector function; uint8_t frame_id; ///< Reference frame number. Cannot be 0. - mip_aiding_reference_frame_command_format format; ///< Format of the transformation. + mip_aiding_frame_config_command_format format; ///< Format of the transformation. mip_vector3f translation; ///< Translation X, Y, and Z. - mip_aiding_reference_frame_command_rotation rotation; ///< Rotation as specified by format. + mip_aiding_frame_config_command_rotation rotation; ///< Rotation as specified by format. + bool tracking_enabled; ///< If enabled, the Kalman filter will track errors }; -typedef struct mip_aiding_reference_frame_command mip_aiding_reference_frame_command; -void insert_mip_aiding_reference_frame_command(struct mip_serializer* serializer, const mip_aiding_reference_frame_command* self); -void extract_mip_aiding_reference_frame_command(struct mip_serializer* serializer, mip_aiding_reference_frame_command* self); +typedef struct mip_aiding_frame_config_command mip_aiding_frame_config_command; +void insert_mip_aiding_frame_config_command(struct mip_serializer* serializer, const mip_aiding_frame_config_command* self); +void extract_mip_aiding_frame_config_command(struct mip_serializer* serializer, mip_aiding_frame_config_command* self); -void insert_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, const mip_aiding_reference_frame_command_format self); -void extract_mip_aiding_reference_frame_command_format(struct mip_serializer* serializer, mip_aiding_reference_frame_command_format* self); +void insert_mip_aiding_frame_config_command_format(struct mip_serializer* serializer, const mip_aiding_frame_config_command_format self); +void extract_mip_aiding_frame_config_command_format(struct mip_serializer* serializer, mip_aiding_frame_config_command_format* self); -struct mip_aiding_reference_frame_response +struct mip_aiding_frame_config_response { uint8_t frame_id; ///< Reference frame number. Cannot be 0. - mip_aiding_reference_frame_command_format format; ///< Format of the transformation. + mip_aiding_frame_config_command_format format; ///< Format of the transformation. mip_vector3f translation; ///< Translation X, Y, and Z. - mip_aiding_reference_frame_command_rotation rotation; ///< Rotation as specified by format. + mip_aiding_frame_config_command_rotation rotation; ///< Rotation as specified by format. + bool tracking_enabled; ///< If enabled, the Kalman filter will track errors }; -typedef struct mip_aiding_reference_frame_response mip_aiding_reference_frame_response; -void insert_mip_aiding_reference_frame_response(struct mip_serializer* serializer, const mip_aiding_reference_frame_response* self); -void extract_mip_aiding_reference_frame_response(struct mip_serializer* serializer, mip_aiding_reference_frame_response* self); - -mip_cmd_result mip_aiding_write_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, const float* translation, const mip_aiding_reference_frame_command_rotation* rotation); -mip_cmd_result mip_aiding_read_reference_frame(struct mip_interface* device, uint8_t frame_id, mip_aiding_reference_frame_command_format format, float* translation_out, mip_aiding_reference_frame_command_rotation* rotation_out); -mip_cmd_result mip_aiding_save_reference_frame(struct mip_interface* device, uint8_t frame_id); -mip_cmd_result mip_aiding_load_reference_frame(struct mip_interface* device, uint8_t frame_id); -mip_cmd_result mip_aiding_default_reference_frame(struct mip_interface* device, uint8_t frame_id); +typedef struct mip_aiding_frame_config_response mip_aiding_frame_config_response; +void insert_mip_aiding_frame_config_response(struct mip_serializer* serializer, const mip_aiding_frame_config_response* self); +void extract_mip_aiding_frame_config_response(struct mip_serializer* serializer, mip_aiding_frame_config_response* self); + +mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, const float* translation, const mip_aiding_frame_config_command_rotation* rotation, bool tracking_enabled); +mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, float* translation_out, mip_aiding_frame_config_command_rotation* rotation_out, bool* tracking_enabled_out); +mip_cmd_result mip_aiding_save_frame_config(struct mip_interface* device, uint8_t frame_id); +mip_cmd_result mip_aiding_load_frame_config(struct mip_interface* device, uint8_t frame_id); +mip_cmd_result mip_aiding_default_frame_config(struct mip_interface* device, uint8_t frame_id); ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 2ed8d408a..563f51891 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -82,8 +82,8 @@ void extract(Serializer& serializer, Time& self); //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_reference_frame (0x13,0x01) Reference Frame [CPP] -/// Defines a reference frame associated with a specific sensor frame ID. The frame ID used in this command +///@defgroup cpp_aiding_frame_config (0x13,0x01) Frame Config [CPP] +/// Defines an aiding frame associated with a specific sensor frame ID. The frame ID used in this command /// should mirror the frame ID used in the aiding command (if that aiding measurement is measured in this reference frame) /// /// This transform satisfies the following relationship: @@ -108,7 +108,7 @@ void extract(Serializer& serializer, Time& self); /// ///@{ -struct ReferenceFrame +struct FrameConfig { enum class Format : uint8_t { @@ -128,15 +128,16 @@ struct ReferenceFrame Format format = static_cast(0); ///< Format of the transformation. Vector3f translation; ///< Translation X, Y, and Z. Rotation rotation; ///< Rotation as specified by format. + bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferenceFrame"; - static constexpr const char* DOC_NAME = "Reference Frame Configuration"; + static constexpr const char* NAME = "FrameConfig"; + static constexpr const char* DOC_NAME = "Frame Configuration"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; - static constexpr const uint32_t WRITE_PARAMS = 0x800F; + static constexpr const uint32_t WRITE_PARAMS = 0x801F; static constexpr const uint32_t READ_PARAMS = 0x8003; static constexpr const uint32_t SAVE_PARAMS = 0x8001; static constexpr const uint32_t LOAD_PARAMS = 0x8001; @@ -146,17 +147,17 @@ struct ReferenceFrame auto as_tuple() const { - return std::make_tuple(frame_id,format,translation,rotation); + return std::make_tuple(frame_id,format,translation,rotation,tracking_enabled); } auto as_tuple() { - return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation)); + return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation),std::ref(tracking_enabled)); } - static ReferenceFrame create_sld_all(::mip::FunctionSelector function) + static FrameConfig create_sld_all(::mip::FunctionSelector function) { - ReferenceFrame cmd; + FrameConfig cmd; cmd.function = function; cmd.frame_id = 0; return cmd; @@ -167,8 +168,8 @@ struct ReferenceFrame static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_FRAME_CONFIG; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "ReferenceFrame::Response"; - static constexpr const char* DOC_NAME = "Reference Frame Configuration Response"; + static constexpr const char* NAME = "FrameConfig::Response"; + static constexpr const char* DOC_NAME = "Frame Configuration Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -176,25 +177,26 @@ struct ReferenceFrame Format format = static_cast(0); ///< Format of the transformation. Vector3f translation; ///< Translation X, Y, and Z. Rotation rotation; ///< Rotation as specified by format. + bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors auto as_tuple() { - return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation)); + return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation),std::ref(tracking_enabled)); } }; }; -void insert(Serializer& serializer, const ReferenceFrame& self); -void extract(Serializer& serializer, ReferenceFrame& self); +void insert(Serializer& serializer, const FrameConfig& self); +void extract(Serializer& serializer, FrameConfig& self); -void insert(Serializer& serializer, const ReferenceFrame::Response& self); -void extract(Serializer& serializer, ReferenceFrame::Response& self); +void insert(Serializer& serializer, const FrameConfig::Response& self); +void extract(Serializer& serializer, FrameConfig::Response& self); -TypedResult writeReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, const float* translation, const ReferenceFrame::Rotation& rotation); -TypedResult readReferenceFrame(C::mip_interface& device, uint8_t frameId, ReferenceFrame::Format format, float* translationOut, ReferenceFrame::Rotation* rotationOut); -TypedResult saveReferenceFrame(C::mip_interface& device, uint8_t frameId); -TypedResult loadReferenceFrame(C::mip_interface& device, uint8_t frameId); -TypedResult defaultReferenceFrame(C::mip_interface& device, uint8_t frameId); +TypedResult writeFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, const float* translation, const FrameConfig::Rotation& rotation, bool trackingEnabled); +TypedResult readFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, float* translationOut, FrameConfig::Rotation* rotationOut, bool* trackingEnabledOut); +TypedResult saveFrameConfig(C::mip_interface& device, uint8_t frameId); +TypedResult loadFrameConfig(C::mip_interface& device, uint8_t frameId); +TypedResult defaultFrameConfig(C::mip_interface& device, uint8_t frameId); ///@} /// From 8d952d15d6e05c94b9db63df37797bd87df5badf Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Mon, 11 Mar 2024 15:33:45 -0400 Subject: [PATCH 211/252] Fixed CV7 Example to match new MIP Aiding frame definitions --- examples/CV7_INS/CV7_INS_simple_example.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index b4404be8a..aeb2af1f4 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -120,33 +120,33 @@ int main(int argc, const char* argv[]) // //External heading sensor reference frame. // - commands_aiding::ReferenceFrame::Rotation external_heading_sensor_to_vehicle_frame_rotation; + commands_aiding::FrameConfig::Rotation external_heading_sensor_to_vehicle_frame_rotation; external_heading_sensor_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // External heading sensor is aligned with vehicle frame float external_heading_sensor_to_vehicle_frame_translation[3] = {0.0, 0.0, 0.0}; // Heading measurements are agnostic to translation, translation set to zero - if(commands_aiding::writeReferenceFrame(*device, external_heading_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, external_heading_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, + external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external heading sensor frame ID"); // //External GNSS antenna reference frame // - commands_aiding::ReferenceFrame::Rotation external_gnss_antenna_to_vehicle_frame_rotation; + commands_aiding::FrameConfig::Rotation external_gnss_antenna_to_vehicle_frame_rotation; external_gnss_antenna_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero float external_gnss_antenna_to_vehicle_frame_translation[3] = {0.0, 1.0, 0.0}; // Antenna is translated 1 meter in vehicle frame Y direction - if(commands_aiding::writeReferenceFrame(*device, gnss_antenna_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, gnss_antenna_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, + external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); // //External bodyframe velocity reference frame // - commands_aiding::ReferenceFrame::Rotation external_velocity_sensor_to_vehicle_frame_rotation; + commands_aiding::FrameConfig::Rotation external_velocity_sensor_to_vehicle_frame_rotation; external_gnss_antenna_to_vehicle_frame_rotation.euler= mip::Vector3f(0.0f, 0.0f, 1.57f); // Rotated 90 deg around yaw axis float external_velocity_sensor_to_vehicle_frame_translation[3] = {1.0, 0.0, 0.0}; // Sensor is translated 1 meter in X direction - if(commands_aiding::writeReferenceFrame(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, + external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external vehicle frame velocity sensor ID"); From 81dfeaf486bc2a04ef93dd8f77b0ec98afbc1f7e Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Mon, 11 Mar 2024 15:38:03 -0400 Subject: [PATCH 212/252] Update CV7_INS_simple_ublox_example.cpp --- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 9eb21def8..15e2ba50c 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -139,10 +139,10 @@ int main(int argc, const char* argv[]) // //External GNSS antenna reference frame // - commands_aiding::ReferenceFrame::Rotation external_gnss_antenna_to_vehicle_frame_rotation; + commands_aiding::FrameConfig::Rotation external_gnss_antenna_to_vehicle_frame_rotation; external_gnss_antenna_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero - if(commands_aiding::writeReferenceFrame(*device, gnss_antenna_sensor_id, mip::commands_aiding::ReferenceFrame::Format::EULER, - input_arguments.gnss_antenna_lever_arm, external_gnss_antenna_to_vehicle_frame_rotation) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, gnss_antenna_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, + input_arguments.gnss_antenna_lever_arm, external_gnss_antenna_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); From fd57a86f5316e0c5b1fbc60d6ec56580238ebf57 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 25 Mar 2024 18:12:33 +0000 Subject: [PATCH 213/252] Generate MIP definitions from branches/dev@56174. --- src/mip/definitions/commands_aiding.c | 96 ++++++++++++------------- src/mip/definitions/commands_aiding.cpp | 92 ++++++++++++------------ src/mip/definitions/commands_aiding.h | 48 ++++++------- src/mip/definitions/commands_aiding.hpp | 84 +++++++++++----------- 4 files changed, 160 insertions(+), 160 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 9a8f31ccd..125dec38e 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -605,54 +605,6 @@ mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* t return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_pressure_command(mip_serializer* serializer, const mip_aiding_pressure_command* self) -{ - insert_mip_time(serializer, &self->time); - - insert_u8(serializer, self->frame_id); - - insert_float(serializer, self->pressure); - - insert_float(serializer, self->uncertainty); - - insert_u16(serializer, self->valid_flags); - -} -void extract_mip_aiding_pressure_command(mip_serializer* serializer, mip_aiding_pressure_command* self) -{ - extract_mip_time(serializer, &self->time); - - extract_u8(serializer, &self->frame_id); - - extract_float(serializer, &self->pressure); - - extract_float(serializer, &self->uncertainty); - - extract_u16(serializer, &self->valid_flags); - -} - -mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float pressure, float uncertainty, uint16_t valid_flags) -{ - mip_serializer serializer; - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); - - assert(time); - insert_mip_time(&serializer, time); - - insert_u8(&serializer, frame_id); - - insert_float(&serializer, pressure); - - insert_float(&serializer, uncertainty); - - insert_u16(&serializer, valid_flags); - - assert(mip_serializer_is_ok(&serializer)); - - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_aiding_ecef_vel_command* self) { insert_mip_time(serializer, &self->time); @@ -969,6 +921,54 @@ mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_MAGNETIC_FIELD, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert_mip_aiding_pressure_command(mip_serializer* serializer, const mip_aiding_pressure_command* self) +{ + insert_mip_time(serializer, &self->time); + + insert_u8(serializer, self->frame_id); + + insert_float(serializer, self->pressure); + + insert_float(serializer, self->uncertainty); + + insert_u16(serializer, self->valid_flags); + +} +void extract_mip_aiding_pressure_command(mip_serializer* serializer, mip_aiding_pressure_command* self) +{ + extract_mip_time(serializer, &self->time); + + extract_u8(serializer, &self->frame_id); + + extract_float(serializer, &self->pressure); + + extract_float(serializer, &self->uncertainty); + + extract_u16(serializer, &self->valid_flags); + +} + +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float pressure, float uncertainty, uint16_t valid_flags) +{ + mip_serializer serializer; + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + mip_serializer_init_insertion(&serializer, buffer, sizeof(buffer)); + + assert(time); + insert_mip_time(&serializer, time); + + insert_u8(&serializer, frame_id); + + insert_float(&serializer, pressure); + + insert_float(&serializer, uncertainty); + + insert_u16(&serializer, valid_flags); + + assert(mip_serializer_is_ok(&serializer)); + + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} #ifdef __cplusplus } // namespace C diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 8b528d09a..5858496d5 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -524,52 +524,6 @@ TypedResult height(C::mip_interface& device, const Time& time, uint8_t f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const Pressure& self) -{ - insert(serializer, self.time); - - insert(serializer, self.frame_id); - - insert(serializer, self.pressure); - - insert(serializer, self.uncertainty); - - insert(serializer, self.valid_flags); - -} -void extract(Serializer& serializer, Pressure& self) -{ - extract(serializer, self.time); - - extract(serializer, self.frame_id); - - extract(serializer, self.pressure); - - extract(serializer, self.uncertainty); - - extract(serializer, self.valid_flags); - -} - -TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t frameId, float pressure, float uncertainty, uint16_t validFlags) -{ - uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; - Serializer serializer(buffer, sizeof(buffer)); - - insert(serializer, time); - - insert(serializer, frameId); - - insert(serializer, pressure); - - insert(serializer, uncertainty); - - insert(serializer, validFlags); - - assert(serializer.isOk()); - - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); -} void insert(Serializer& serializer, const EcefVel& self) { insert(serializer, self.time); @@ -832,6 +786,52 @@ TypedResult magneticField(C::mip_interface& device, const Time& t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_MAGNETIC_FIELD, buffer, (uint8_t)mip_serializer_length(&serializer)); } +void insert(Serializer& serializer, const Pressure& self) +{ + insert(serializer, self.time); + + insert(serializer, self.frame_id); + + insert(serializer, self.pressure); + + insert(serializer, self.uncertainty); + + insert(serializer, self.valid_flags); + +} +void extract(Serializer& serializer, Pressure& self) +{ + extract(serializer, self.time); + + extract(serializer, self.frame_id); + + extract(serializer, self.pressure); + + extract(serializer, self.uncertainty); + + extract(serializer, self.valid_flags); + +} + +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t frameId, float pressure, float uncertainty, uint16_t validFlags) +{ + uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; + Serializer serializer(buffer, sizeof(buffer)); + + insert(serializer, time); + + insert(serializer, frameId); + + insert(serializer, pressure); + + insert(serializer, uncertainty); + + insert(serializer, validFlags); + + assert(serializer.isOk()); + + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_PRESSURE, buffer, (uint8_t)mip_serializer_length(&serializer)); +} } // namespace commands_aiding } // namespace mip diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index af738e8d0..ba70252ab 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -41,13 +41,13 @@ enum MIP_CMD_DESC_AIDING_POS_LLH = 0x22, MIP_CMD_DESC_AIDING_HEIGHT_ABS = 0x23, MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x24, - MIP_CMD_DESC_AIDING_PRESSURE = 0x25, MIP_CMD_DESC_AIDING_VEL_ECEF = 0x28, MIP_CMD_DESC_AIDING_VEL_NED = 0x29, MIP_CMD_DESC_AIDING_VEL_ODOM = 0x2A, MIP_CMD_DESC_AIDING_WHEELSPEED = 0x2B, MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x31, MIP_CMD_DESC_AIDING_MAGNETIC_FIELD = 0x32, + MIP_CMD_DESC_AIDING_PRESSURE = 0x33, MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x38, MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x39, MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x3A, @@ -291,29 +291,6 @@ void extract_mip_aiding_height_command(struct mip_serializer* serializer, mip_ai mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float height, float uncertainty, uint16_t valid_flags); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_pressure (0x13,0x25) Pressure [C] -/// Estimated value of air pressure. -/// -///@{ - -struct mip_aiding_pressure_command -{ - mip_time time; - uint8_t frame_id; - float pressure; ///< [mbar] - float uncertainty; ///< [mbar] - uint16_t valid_flags; - -}; -typedef struct mip_aiding_pressure_command mip_aiding_pressure_command; -void insert_mip_aiding_pressure_command(struct mip_serializer* serializer, const mip_aiding_pressure_command* self); -void extract_mip_aiding_pressure_command(struct mip_serializer* serializer, mip_aiding_pressure_command* self); - -mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float pressure, float uncertainty, uint16_t valid_flags); - ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -469,6 +446,29 @@ void extract_mip_aiding_magnetic_field_command_valid_flags(struct mip_serializer mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* magnetic_field, const float* uncertainty, mip_aiding_magnetic_field_command_valid_flags valid_flags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup c_aiding_pressure (0x13,0x33) Pressure [C] +/// Estimated value of air pressure. +/// +///@{ + +struct mip_aiding_pressure_command +{ + mip_time time; + uint8_t frame_id; + float pressure; ///< [mbar] + float uncertainty; ///< [mbar] + uint16_t valid_flags; + +}; +typedef struct mip_aiding_pressure_command mip_aiding_pressure_command; +void insert_mip_aiding_pressure_command(struct mip_serializer* serializer, const mip_aiding_pressure_command* self); +void extract_mip_aiding_pressure_command(struct mip_serializer* serializer, mip_aiding_pressure_command* self); + +mip_cmd_result mip_aiding_pressure(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float pressure, float uncertainty, uint16_t valid_flags); + ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 563f51891..95f55e4bd 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -40,13 +40,13 @@ enum CMD_POS_LLH = 0x22, CMD_HEIGHT_ABS = 0x23, CMD_HEIGHT_REL = 0x24, - CMD_PRESSURE = 0x25, CMD_VEL_ECEF = 0x28, CMD_VEL_NED = 0x29, CMD_VEL_ODOM = 0x2A, CMD_WHEELSPEED = 0x2B, CMD_HEADING_TRUE = 0x31, CMD_MAGNETIC_FIELD = 0x32, + CMD_PRESSURE = 0x33, CMD_DELTA_POSITION = 0x38, CMD_DELTA_ATTITUDE = 0x39, CMD_LOCAL_ANGULAR_RATE = 0x3A, @@ -469,47 +469,6 @@ void extract(Serializer& serializer, Height& self); TypedResult height(C::mip_interface& device, const Time& time, uint8_t frameId, float height, float uncertainty, uint16_t validFlags); -///@} -/// -//////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_pressure (0x13,0x25) Pressure [CPP] -/// Estimated value of air pressure. -/// -///@{ - -struct Pressure -{ - Time time; - uint8_t frame_id = 0; - float pressure = 0; ///< [mbar] - float uncertainty = 0; ///< [mbar] - uint16_t valid_flags = 0; - - static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_PRESSURE; - static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Pressure"; - static constexpr const char* DOC_NAME = "Pressure"; - - static constexpr const bool HAS_FUNCTION_SELECTOR = false; - static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; - - auto as_tuple() const - { - return std::make_tuple(time,frame_id,pressure,uncertainty,valid_flags); - } - - auto as_tuple() - { - return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(pressure),std::ref(uncertainty),std::ref(valid_flags)); - } - typedef void Response; -}; -void insert(Serializer& serializer, const Pressure& self); -void extract(Serializer& serializer, Pressure& self); - -TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t frameId, float pressure, float uncertainty, uint16_t validFlags); - ///@} /// //////////////////////////////////////////////////////////////////////////////// @@ -839,6 +798,47 @@ void extract(Serializer& serializer, MagneticField& self); TypedResult magneticField(C::mip_interface& device, const Time& time, uint8_t frameId, const float* magneticField, const float* uncertainty, MagneticField::ValidFlags validFlags); +///@} +/// +//////////////////////////////////////////////////////////////////////////////// +///@defgroup cpp_aiding_pressure (0x13,0x33) Pressure [CPP] +/// Estimated value of air pressure. +/// +///@{ + +struct Pressure +{ + Time time; + uint8_t frame_id = 0; + float pressure = 0; ///< [mbar] + float uncertainty = 0; ///< [mbar] + uint16_t valid_flags = 0; + + static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_PRESSURE; + static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; + static constexpr const char* NAME = "Pressure"; + static constexpr const char* DOC_NAME = "Pressure"; + + static constexpr const bool HAS_FUNCTION_SELECTOR = false; + static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; + + auto as_tuple() const + { + return std::make_tuple(time,frame_id,pressure,uncertainty,valid_flags); + } + + auto as_tuple() + { + return std::make_tuple(std::ref(time),std::ref(frame_id),std::ref(pressure),std::ref(uncertainty),std::ref(valid_flags)); + } + typedef void Response; +}; +void insert(Serializer& serializer, const Pressure& self); +void extract(Serializer& serializer, Pressure& self); + +TypedResult pressure(C::mip_interface& device, const Time& time, uint8_t frameId, float pressure, float uncertainty, uint16_t validFlags); + ///@} /// From f5af4957b8a8c2d9145a139b7060c877e7af0c5b Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 28 Mar 2024 15:23:46 +0000 Subject: [PATCH 214/252] Generate MIP definitions from branches/dev@56233. --- src/mip/definitions/commands_aiding.c | 30 ++++++++++++------------- src/mip/definitions/commands_aiding.cpp | 30 ++++++++++++------------- src/mip/definitions/commands_aiding.h | 8 +++---- src/mip/definitions/commands_aiding.hpp | 14 ++++++------ 4 files changed, 41 insertions(+), 41 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index 125dec38e..af010defb 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -70,6 +70,8 @@ void insert_mip_aiding_frame_config_command(mip_serializer* serializer, const mi } if( self->function == MIP_FUNCTION_WRITE ) { + insert_bool(serializer, self->tracking_enabled); + for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); @@ -83,8 +85,6 @@ void insert_mip_aiding_frame_config_command(mip_serializer* serializer, const mi insert_mip_quatf(serializer, &self->rotation.quaternion); } - insert_bool(serializer, self->tracking_enabled); - } } void extract_mip_aiding_frame_config_command(mip_serializer* serializer, mip_aiding_frame_config_command* self) @@ -100,6 +100,8 @@ void extract_mip_aiding_frame_config_command(mip_serializer* serializer, mip_aid } if( self->function == MIP_FUNCTION_WRITE ) { + extract_bool(serializer, &self->tracking_enabled); + for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); @@ -113,8 +115,6 @@ void extract_mip_aiding_frame_config_command(mip_serializer* serializer, mip_aid extract_mip_quatf(serializer, &self->rotation.quaternion); } - extract_bool(serializer, &self->tracking_enabled); - } } @@ -124,6 +124,8 @@ void insert_mip_aiding_frame_config_response(mip_serializer* serializer, const m insert_mip_aiding_frame_config_command_format(serializer, self->format); + insert_bool(serializer, self->tracking_enabled); + for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->translation[i]); @@ -137,8 +139,6 @@ void insert_mip_aiding_frame_config_response(mip_serializer* serializer, const m insert_mip_quatf(serializer, &self->rotation.quaternion); } - insert_bool(serializer, self->tracking_enabled); - } void extract_mip_aiding_frame_config_response(mip_serializer* serializer, mip_aiding_frame_config_response* self) { @@ -146,6 +146,8 @@ void extract_mip_aiding_frame_config_response(mip_serializer* serializer, mip_ai extract_mip_aiding_frame_config_command_format(serializer, &self->format); + extract_bool(serializer, &self->tracking_enabled); + for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->translation[i]); @@ -159,8 +161,6 @@ void extract_mip_aiding_frame_config_response(mip_serializer* serializer, mip_ai extract_mip_quatf(serializer, &self->rotation.quaternion); } - extract_bool(serializer, &self->tracking_enabled); - } void insert_mip_aiding_frame_config_command_format(struct mip_serializer* serializer, const mip_aiding_frame_config_command_format self) @@ -174,7 +174,7 @@ void extract_mip_aiding_frame_config_command_format(struct mip_serializer* seria *self = tmp; } -mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, const float* translation, const mip_aiding_frame_config_command_rotation* rotation, bool tracking_enabled) +mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, bool tracking_enabled, const float* translation, const mip_aiding_frame_config_command_rotation* rotation) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -186,6 +186,8 @@ mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8 insert_mip_aiding_frame_config_command_format(&serializer, format); + insert_bool(&serializer, tracking_enabled); + assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert_float(&serializer, translation[i]); @@ -200,13 +202,11 @@ mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8 insert_mip_quatf(&serializer, &rotation->quaternion); } - insert_bool(&serializer, tracking_enabled); - assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, float* translation_out, mip_aiding_frame_config_command_rotation* rotation_out, bool* tracking_enabled_out) +mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, bool* tracking_enabled_out, float* translation_out, mip_aiding_frame_config_command_rotation* rotation_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -232,6 +232,9 @@ mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_ extract_mip_aiding_frame_config_command_format(&deserializer, &format); + assert(tracking_enabled_out); + extract_bool(&deserializer, tracking_enabled_out); + assert(translation_out || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract_float(&deserializer, &translation_out[i]); @@ -246,9 +249,6 @@ mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_ extract_mip_quatf(&deserializer, &rotation_out->quaternion); } - assert(tracking_enabled_out); - extract_bool(&deserializer, tracking_enabled_out); - if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 5858496d5..4b9240e49 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -61,6 +61,8 @@ void insert(Serializer& serializer, const FrameConfig& self) } if( self.function == FunctionSelector::WRITE ) { + insert(serializer, self.tracking_enabled); + for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); @@ -74,8 +76,6 @@ void insert(Serializer& serializer, const FrameConfig& self) insert(serializer, self.rotation.quaternion); } - insert(serializer, self.tracking_enabled); - } } void extract(Serializer& serializer, FrameConfig& self) @@ -91,6 +91,8 @@ void extract(Serializer& serializer, FrameConfig& self) } if( self.function == FunctionSelector::WRITE ) { + extract(serializer, self.tracking_enabled); + for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); @@ -104,8 +106,6 @@ void extract(Serializer& serializer, FrameConfig& self) extract(serializer, self.rotation.quaternion); } - extract(serializer, self.tracking_enabled); - } } @@ -115,6 +115,8 @@ void insert(Serializer& serializer, const FrameConfig::Response& self) insert(serializer, self.format); + insert(serializer, self.tracking_enabled); + for(unsigned int i=0; i < 3; i++) insert(serializer, self.translation[i]); @@ -128,8 +130,6 @@ void insert(Serializer& serializer, const FrameConfig::Response& self) insert(serializer, self.rotation.quaternion); } - insert(serializer, self.tracking_enabled); - } void extract(Serializer& serializer, FrameConfig::Response& self) { @@ -137,6 +137,8 @@ void extract(Serializer& serializer, FrameConfig::Response& self) extract(serializer, self.format); + extract(serializer, self.tracking_enabled); + for(unsigned int i=0; i < 3; i++) extract(serializer, self.translation[i]); @@ -150,11 +152,9 @@ void extract(Serializer& serializer, FrameConfig::Response& self) extract(serializer, self.rotation.quaternion); } - extract(serializer, self.tracking_enabled); - } -TypedResult writeFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, const float* translation, const FrameConfig::Rotation& rotation, bool trackingEnabled) +TypedResult writeFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, bool trackingEnabled, const float* translation, const FrameConfig::Rotation& rotation) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -164,6 +164,8 @@ TypedResult writeFrameConfig(C::mip_interface& device, uint8_t fram insert(serializer, format); + insert(serializer, trackingEnabled); + assert(translation || (3 == 0)); for(unsigned int i=0; i < 3; i++) insert(serializer, translation[i]); @@ -178,13 +180,11 @@ TypedResult writeFrameConfig(C::mip_interface& device, uint8_t fram insert(serializer, rotation.quaternion); } - insert(serializer, trackingEnabled); - assert(serializer.isOk()); return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, float* translationOut, FrameConfig::Rotation* rotationOut, bool* trackingEnabledOut) +TypedResult readFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, bool* trackingEnabledOut, float* translationOut, FrameConfig::Rotation* rotationOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -207,6 +207,9 @@ TypedResult readFrameConfig(C::mip_interface& device, uint8_t frame extract(deserializer, format); + assert(trackingEnabledOut); + extract(deserializer, *trackingEnabledOut); + assert(translationOut || (3 == 0)); for(unsigned int i=0; i < 3; i++) extract(deserializer, translationOut[i]); @@ -221,9 +224,6 @@ TypedResult readFrameConfig(C::mip_interface& device, uint8_t frame extract(deserializer, rotationOut->quaternion); } - assert(trackingEnabledOut); - extract(deserializer, *trackingEnabledOut); - if( deserializer.remaining() != 0 ) result = MIP_STATUS_ERROR; } diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index ba70252ab..95de33171 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -127,9 +127,9 @@ struct mip_aiding_frame_config_command mip_function_selector function; uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_frame_config_command_format format; ///< Format of the transformation. + bool tracking_enabled; ///< If enabled, the Kalman filter will track errors mip_vector3f translation; ///< Translation X, Y, and Z. mip_aiding_frame_config_command_rotation rotation; ///< Rotation as specified by format. - bool tracking_enabled; ///< If enabled, the Kalman filter will track errors }; typedef struct mip_aiding_frame_config_command mip_aiding_frame_config_command; @@ -143,17 +143,17 @@ struct mip_aiding_frame_config_response { uint8_t frame_id; ///< Reference frame number. Cannot be 0. mip_aiding_frame_config_command_format format; ///< Format of the transformation. + bool tracking_enabled; ///< If enabled, the Kalman filter will track errors mip_vector3f translation; ///< Translation X, Y, and Z. mip_aiding_frame_config_command_rotation rotation; ///< Rotation as specified by format. - bool tracking_enabled; ///< If enabled, the Kalman filter will track errors }; typedef struct mip_aiding_frame_config_response mip_aiding_frame_config_response; void insert_mip_aiding_frame_config_response(struct mip_serializer* serializer, const mip_aiding_frame_config_response* self); void extract_mip_aiding_frame_config_response(struct mip_serializer* serializer, mip_aiding_frame_config_response* self); -mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, const float* translation, const mip_aiding_frame_config_command_rotation* rotation, bool tracking_enabled); -mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, float* translation_out, mip_aiding_frame_config_command_rotation* rotation_out, bool* tracking_enabled_out); +mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, bool tracking_enabled, const float* translation, const mip_aiding_frame_config_command_rotation* rotation); +mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_t frame_id, mip_aiding_frame_config_command_format format, bool* tracking_enabled_out, float* translation_out, mip_aiding_frame_config_command_rotation* rotation_out); mip_cmd_result mip_aiding_save_frame_config(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_load_frame_config(struct mip_interface* device, uint8_t frame_id); mip_cmd_result mip_aiding_default_frame_config(struct mip_interface* device, uint8_t frame_id); diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 95f55e4bd..10905c398 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -126,9 +126,9 @@ struct FrameConfig FunctionSelector function = static_cast(0); uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. + bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors Vector3f translation; ///< Translation X, Y, and Z. Rotation rotation; ///< Rotation as specified by format. - bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_FRAME_CONFIG; @@ -147,12 +147,12 @@ struct FrameConfig auto as_tuple() const { - return std::make_tuple(frame_id,format,translation,rotation,tracking_enabled); + return std::make_tuple(frame_id,format,tracking_enabled,translation,rotation); } auto as_tuple() { - return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation),std::ref(tracking_enabled)); + return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(tracking_enabled),std::ref(translation),std::ref(rotation)); } static FrameConfig create_sld_all(::mip::FunctionSelector function) @@ -175,14 +175,14 @@ struct FrameConfig static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. Format format = static_cast(0); ///< Format of the transformation. + bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors Vector3f translation; ///< Translation X, Y, and Z. Rotation rotation; ///< Rotation as specified by format. - bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors auto as_tuple() { - return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(translation),std::ref(rotation),std::ref(tracking_enabled)); + return std::make_tuple(std::ref(frame_id),std::ref(format),std::ref(tracking_enabled),std::ref(translation),std::ref(rotation)); } }; }; @@ -192,8 +192,8 @@ void extract(Serializer& serializer, FrameConfig& self); void insert(Serializer& serializer, const FrameConfig::Response& self); void extract(Serializer& serializer, FrameConfig::Response& self); -TypedResult writeFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, const float* translation, const FrameConfig::Rotation& rotation, bool trackingEnabled); -TypedResult readFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, float* translationOut, FrameConfig::Rotation* rotationOut, bool* trackingEnabledOut); +TypedResult writeFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, bool trackingEnabled, const float* translation, const FrameConfig::Rotation& rotation); +TypedResult readFrameConfig(C::mip_interface& device, uint8_t frameId, FrameConfig::Format format, bool* trackingEnabledOut, float* translationOut, FrameConfig::Rotation* rotationOut); TypedResult saveFrameConfig(C::mip_interface& device, uint8_t frameId); TypedResult loadFrameConfig(C::mip_interface& device, uint8_t frameId); TypedResult defaultFrameConfig(C::mip_interface& device, uint8_t frameId); From 6a3822e2f1a7102f53e7e399945e92b68928e874 Mon Sep 17 00:00:00 2001 From: nathanmillerparker <69481927+nathanmillerparker@users.noreply.github.com> Date: Mon, 8 Apr 2024 09:48:01 -0400 Subject: [PATCH 215/252] Fixed the order of parameters in CV7 INS example Frame config --- examples/CV7_INS/CV7_INS_simple_example.cpp | 12 ++++++------ examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index aeb2af1f4..1ab4c70db 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -123,8 +123,8 @@ int main(int argc, const char* argv[]) commands_aiding::FrameConfig::Rotation external_heading_sensor_to_vehicle_frame_rotation; external_heading_sensor_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // External heading sensor is aligned with vehicle frame float external_heading_sensor_to_vehicle_frame_translation[3] = {0.0, 0.0, 0.0}; // Heading measurements are agnostic to translation, translation set to zero - if(commands_aiding::writeFrameConfig(*device, external_heading_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, - external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, external_heading_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, true, + external_heading_sensor_to_vehicle_frame_translation, external_heading_sensor_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external heading sensor frame ID"); @@ -134,8 +134,8 @@ int main(int argc, const char* argv[]) commands_aiding::FrameConfig::Rotation external_gnss_antenna_to_vehicle_frame_rotation; external_gnss_antenna_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero float external_gnss_antenna_to_vehicle_frame_translation[3] = {0.0, 1.0, 0.0}; // Antenna is translated 1 meter in vehicle frame Y direction - if(commands_aiding::writeFrameConfig(*device, gnss_antenna_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, - external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, gnss_antenna_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, true, + external_gnss_antenna_to_vehicle_frame_translation, external_gnss_antenna_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); @@ -145,8 +145,8 @@ int main(int argc, const char* argv[]) commands_aiding::FrameConfig::Rotation external_velocity_sensor_to_vehicle_frame_rotation; external_gnss_antenna_to_vehicle_frame_rotation.euler= mip::Vector3f(0.0f, 0.0f, 1.57f); // Rotated 90 deg around yaw axis float external_velocity_sensor_to_vehicle_frame_translation[3] = {1.0, 0.0, 0.0}; // Sensor is translated 1 meter in X direction - if(commands_aiding::writeFrameConfig(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, - external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, true, + external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external vehicle frame velocity sensor ID"); diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 15e2ba50c..bf46cc57e 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -141,8 +141,8 @@ int main(int argc, const char* argv[]) // commands_aiding::FrameConfig::Rotation external_gnss_antenna_to_vehicle_frame_rotation; external_gnss_antenna_to_vehicle_frame_rotation.euler = mip::Vector3f(0.0f, 0.0f, 0.0f); // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero // GNSS position/velocity measurements are agnostic to rotation, rotation set to zero - if(commands_aiding::writeFrameConfig(*device, gnss_antenna_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, - input_arguments.gnss_antenna_lever_arm, external_gnss_antenna_to_vehicle_frame_rotation, true) != CmdResult::ACK_OK) + if(commands_aiding::writeFrameConfig(*device, gnss_antenna_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, true, + input_arguments.gnss_antenna_lever_arm, external_gnss_antenna_to_vehicle_frame_rotation) != CmdResult::ACK_OK) exit_gracefully("ERROR: Unable to configure external GNSS antenna frame ID"); From a3606963c9e707b511842d3506f6995e96c75f4d Mon Sep 17 00:00:00 2001 From: Nick DaCosta <25206843+dacuster@users.noreply.github.com> Date: Mon, 22 Apr 2024 10:48:40 -0400 Subject: [PATCH 216/252] Fixed serial port read fail for Windows (#97) * Fixed serial port read fail for Windows On read fail (unplug), serial port is closed * Added clear last error for Windows serial ports --- src/mip/utils/serial_port.c | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index bb045b7d4..a36ef6e5f 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -176,14 +176,14 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) } #else //Linux - + #ifdef __APPLE__ port->handle = open(port_str, O_RDWR | O_NOCTTY | O_NDELAY); #else port->handle = open(port_str, O_RDWR | O_NOCTTY | O_SYNC); #endif - - + + if (port->handle < 0) { MIP_LOG_ERROR("Unable to open port (%d): %s\n", errno, strerror(errno)); @@ -237,7 +237,7 @@ bool serial_port_open(serial_port *port, const char *port_str, int baudrate) #ifdef __APPLE__ speed_t speed = baudrate; - if (ioctl(port->handle, IOSSIOSPEED, &speed) < 0) + if (ioctl(port->handle, IOSSIOSPEED, &speed) < 0) { MIP_LOG_ERROR("Unable to set baud rate (%d): %s\n", errno, strerror(errno)); close(port->handle); @@ -318,6 +318,14 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai #ifdef WIN32 //Windows + DWORD last_error = GetLastError(); + if (last_error != 0) + { + MIP_LOG_ERROR("Failed to read serial port. Error: %lx\n", last_error); + serial_port_close(port); + return false; + } + if( wait_time <= 0 ) { if(bytes_available == 0 ) @@ -379,6 +387,11 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai uint32_t serial_port_read_count(serial_port *port) { +#ifdef WIN32 //Windows + // Clear the last error, if any + SetLastError(0); +#endif + //Check for a valid port handle if(!serial_port_is_open(port)) return 0; From 387cbfa4e117dc54290bb5c61547eeedd2ded883 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 22 Apr 2024 13:25:28 -0400 Subject: [PATCH 217/252] Add performance test. --- test/CMakeLists.txt | 1 + test/mip/mip_parser_performance.cpp | 159 ++++++++++++++++++++++++++++ 2 files changed, 160 insertions(+) create mode 100644 test/mip/mip_parser_performance.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 226e10aaa..14d2eb56a 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -20,6 +20,7 @@ add_mip_test(TestMipParsing "${TEST_DIR}/mip/test_mip_parser.c" TestMipPa add_mip_test(TestMipRandom "${TEST_DIR}/mip/test_mip_random.c" TestMipRandom) add_mip_test(TestMipFields "${TEST_DIR}/mip/test_mip_fields.c" TestMipFields) add_mip_test(TestMipCpp "${TEST_DIR}/mip/test_mip.cpp" TestMipCpp) +add_mip_test(TestMipPerf "${TEST_DIR}/mip/mip_parser_performance.cpp" TestMipPerf) if(WITH_SERIAL) add_executable(TestSerial "${TEST_DIR}/test_serial.cpp") diff --git a/test/mip/mip_parser_performance.cpp b/test/mip/mip_parser_performance.cpp new file mode 100644 index 000000000..7a87c24d3 --- /dev/null +++ b/test/mip/mip_parser_performance.cpp @@ -0,0 +1,159 @@ + +#include "mip/mip.hpp" + +#include +#include +#include + +#include + +const uint8_t PING_PACKET[] = {0x75, 0x65, 0x01, 0x02, 0x02, 0x01, 0xE0, 0xC6}; + +const uint8_t DATA_PACKET[] = { + 0x75,0x65,0x82,0xc1,0x0e,0xd3,0x40,0x8c,0x84,0xef,0x9d,0xb2,0x2d,0x0f,0x00,0x00, + 0x00,0x00,0x0a,0xd5,0x00,0x00,0x00,0xd4,0x7c,0x36,0x4c,0x40,0x10,0x05,0x7f,0xff, + 0xff,0xf8,0x7f,0xc0,0x00,0x00,0x7f,0xff,0xff,0xf8,0x00,0x01,0x10,0x06,0x7f,0xc0, + 0x00,0x00,0x7f,0xc0,0x00,0x00,0x7f,0xc0,0x00,0x00,0x00,0x01,0x10,0x07,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x08,0x10,0x00,0x02, + 0x00,0x00,0x00,0x00,0x1c,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x10,0x02,0x7f,0xc0,0x00,0x00,0x7f,0xc0,0x00,0x00,0x7f,0xc0,0x00,0x00,0x00,0x00, + 0x1c,0x42,0x7f,0xf8,0x00,0x00,0x00,0x00,0x00,0x00,0x7f,0xf8,0x00,0x00,0x00,0x00, + 0x00,0x00,0x7f,0xf8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x09,0x46,0x44,0x64, + 0x26,0x87,0x00,0x04,0x01,0x10,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x10,0x09,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x87,0x56, // 199 bytes +}; + +const char NMEA_SENTENCE[] = "$GPGGA,123456.7,1234.5678,N,1234.5678,W,1,5,1.2,12.345,M,12.345,M,*4F"; + + +struct Test +{ + const char* name = nullptr; + unsigned int num_iterations = 1; + size_t num_packets = 0; + std::vector data; +}; + + +Test generate_pings() +{ + Test test; + + test.name = "Pings"; + test.num_packets = 200000; + test.num_iterations = 100; + + test.data.resize(sizeof(PING_PACKET)*test.num_packets); + + for(unsigned int i=0; i(v); + s.num_bytes += p->totalLength(); + s.num_pkts++; + }; + mip::Parser parser(callback, &stats, MIP_PARSER_DEFAULT_TIMEOUT_MS); + + std::vector timing(test.num_iterations, 0); + + assert(test.num_iterations > 0); + for(unsigned int i=0; i duration = stop - start; + + timing[i] = duration.count(); + } + + const float total_time = std::accumulate(timing.begin(), timing.end(), 0.0f); + const float avg_time = total_time / std::size(timing); + const float max_time = *std::max_element(timing.begin(), timing.end()); + const float bytes_per_sec = test.data.size()*test.num_iterations / total_time; + const float packets_per_sec = test.num_packets*test.num_iterations / total_time; + + std::printf( + "\nTest %s: %u iterations\n" + " Bytes/iter: %.3f MB\n" + " Pkts/iter: %.3f kPkt\n" + " Total Packets: %.f / %.f%s MPkt\n" + " Average Time: %f ms\n" + " Maximum Time: %f ms\n" + " Bytes/s: %.3f MB/s\n" + " Pkts/s: %.3f MPkt/s\n", + test.name, test.num_iterations, + float(test.data.size())/1e6f, + float(test.num_packets)/1e3f, + float(stats.num_pkts)/1e6f, float(test.num_packets*test.num_iterations)/1e6f, (stats.num_pkts != test.num_packets*test.num_iterations ? "***" : ""), + avg_time*1e3f, + max_time*1e3f, + bytes_per_sec/1e6f, + packets_per_sec/1e6f + ); +} + + +int main(int argc, const char* argv[]) +{ + run_parser(generate_long_pkts()); + run_parser(generate_pings()); + run_parser(generate_interleaved()); + + return 0; +} From 619135a857b28e645f5a713ee6f273ca40788e07 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 22 Apr 2024 15:25:34 -0400 Subject: [PATCH 218/252] Add performance test. --- test/mip/mip_parser_performance.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/test/mip/mip_parser_performance.cpp b/test/mip/mip_parser_performance.cpp index 7a87c24d3..9d17ba41f 100644 --- a/test/mip/mip_parser_performance.cpp +++ b/test/mip/mip_parser_performance.cpp @@ -89,6 +89,8 @@ Test generate_interleaved() return test; } +uint8_t parse_buffer[1024]; + void run_parser(const Test& test) { struct Stats @@ -103,8 +105,9 @@ void run_parser(const Test& test) Stats& s = *static_cast(v); s.num_bytes += p->totalLength(); s.num_pkts++; + return true; }; - mip::Parser parser(callback, &stats, MIP_PARSER_DEFAULT_TIMEOUT_MS); + mip::Parser parser(parse_buffer, sizeof(parse_buffer), callback, &stats, MIPPARSER_DEFAULT_TIMEOUT_MS); std::vector timing(test.num_iterations, 0); From 67b47ad03c13259b3e29bde48fd81945a2977adf Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 22 Apr 2024 18:17:41 -0400 Subject: [PATCH 219/252] - Improve performance test for more data. --- test/mip/mip_parser_performance.cpp | 184 ++++++++++++++++++++-------- 1 file changed, 134 insertions(+), 50 deletions(-) diff --git a/test/mip/mip_parser_performance.cpp b/test/mip/mip_parser_performance.cpp index 9d17ba41f..cd6f57e2d 100644 --- a/test/mip/mip_parser_performance.cpp +++ b/test/mip/mip_parser_performance.cpp @@ -1,6 +1,7 @@ #include "mip/mip.hpp" +#include #include #include #include @@ -34,6 +35,7 @@ struct Test unsigned int num_iterations = 1; size_t num_packets = 0; std::vector data; + std::vector chunk_sizes; }; @@ -43,7 +45,8 @@ Test generate_pings() test.name = "Pings"; test.num_packets = 200000; - test.num_iterations = 100; + test.num_iterations = 10; + test.chunk_sizes = {5, 8, 19}; test.data.resize(sizeof(PING_PACKET)*test.num_packets); @@ -57,9 +60,9 @@ Test generate_long_pkts() { Test test; - test.name = "Long Pkts"; + test.name = "Long_Pkts"; test.num_packets = 100'000; - test.num_iterations = 100; + test.num_iterations = 10; test.data.resize(sizeof(DATA_PACKET)*test.num_packets); @@ -73,9 +76,9 @@ Test generate_interleaved() { Test test; - test.name = "InterleavedNMEA"; + test.name = "Interleave_NMEA"; test.num_packets = 100'000; - test.num_iterations = 100; + test.num_iterations = 10; const size_t INTERVAL = sizeof(DATA_PACKET) + sizeof(NMEA_SENTENCE); test.data.resize(INTERVAL*test.num_packets); @@ -89,74 +92,155 @@ Test generate_interleaved() return test; } +volatile bool dummy = false; uint8_t parse_buffer[1024]; -void run_parser(const Test& test) +struct ChunkStats { - struct Stats - { - size_t num_bytes = 0; - size_t num_pkts = 0; - }; + size_t chunk_size = 0; // Size of chunks + size_t num_calls = 0; // Calls to parse per test iteration + float total_time = 0; // Average total time per test iteration + float max_time = 0; // Max of any single call + float avg_time = 0; // Average of every call + float med_time = 0; // Median of every call +}; - Stats stats; +//constexpr float kahan_sum(const float* data, size_t count) +//{ +// float result = 0; +// float c = 0; +// for(size_t i=0; i(v); - s.num_bytes += p->totalLength(); - s.num_pkts++; + *static_cast(v) += 1; return true; }; - mip::Parser parser(parse_buffer, sizeof(parse_buffer), callback, &stats, MIPPARSER_DEFAULT_TIMEOUT_MS); + size_t num_pkts = 0; + mip::Parser parser(parse_buffer, sizeof(parse_buffer), callback, &num_pkts, MIPPARSER_DEFAULT_TIMEOUT_MS); + - std::vector timing(test.num_iterations, 0); + const size_t num_full_chunks = (chunk_size == 0) ? 1 : (test.data.size() / chunk_size); + std::vector chunk_times(num_full_chunks * test.num_iterations); assert(test.num_iterations > 0); - for(unsigned int i=0; i 0) ? chunk_size : test.data.size(); + const uint8_t *buffer = test.data.data(); - auto start = std::chrono::steady_clock::now(); - parser.parse(test.data.data(), test.data.size(), 0); - auto stop = std::chrono::steady_clock::now(); + for(size_t c = 0; c < num_full_chunks; c++) + { + auto start = std::chrono::steady_clock::now(); + dummy = true; // Hopefully prevent compiler/cpu from reordering clock and parse calls. + parser.parse(buffer, buffer_size, 0); + dummy = false; + auto stop = std::chrono::steady_clock::now(); - std::chrono::duration duration = stop - start; + std::chrono::duration duration = stop - start; - timing[i] = duration.count(); + chunk_times[i * num_full_chunks + c] = duration.count(); + buffer += chunk_size; + } + if(chunk_size > 0) + parser.parse(test.data.data() + num_full_chunks * chunk_size, test.data.size() - num_full_chunks * chunk_size, 0); + + if(num_pkts != test.num_packets) + fprintf(stderr, "Error: Got %zu packets but expected %zu!\n", num_pkts, test.num_packets); } - const float total_time = std::accumulate(timing.begin(), timing.end(), 0.0f); - const float avg_time = total_time / std::size(timing); - const float max_time = *std::max_element(timing.begin(), timing.end()); - const float bytes_per_sec = test.data.size()*test.num_iterations / total_time; - const float packets_per_sec = test.num_packets*test.num_iterations / total_time; - - std::printf( - "\nTest %s: %u iterations\n" - " Bytes/iter: %.3f MB\n" - " Pkts/iter: %.3f kPkt\n" - " Total Packets: %.f / %.f%s MPkt\n" - " Average Time: %f ms\n" - " Maximum Time: %f ms\n" - " Bytes/s: %.3f MB/s\n" - " Pkts/s: %.3f MPkt/s\n", - test.name, test.num_iterations, - float(test.data.size())/1e6f, - float(test.num_packets)/1e3f, - float(stats.num_pkts)/1e6f, float(test.num_packets*test.num_iterations)/1e6f, (stats.num_pkts != test.num_packets*test.num_iterations ? "***" : ""), - avg_time*1e3f, - max_time*1e3f, - bytes_per_sec/1e6f, - packets_per_sec/1e6f - ); + ChunkStats stats; + stats.chunk_size = chunk_size; + stats.num_calls = num_full_chunks; + stats.total_time = std::accumulate(chunk_times.begin(), chunk_times.end(), 0.0) / test.num_iterations; // Accumulate with double precision! + stats.avg_time = stats.total_time / num_full_chunks; + stats.max_time = *std::max_element(chunk_times.begin(), chunk_times.end()); + + std::sort(chunk_times.begin(), chunk_times.end()); + stats.med_time = chunk_times[chunk_times.size()/2]; + + return stats; } int main(int argc, const char* argv[]) { - run_parser(generate_long_pkts()); - run_parser(generate_pings()); - run_parser(generate_interleaved()); + std::initializer_list tests = { + generate_pings(), + generate_long_pkts(), + generate_interleaved() + }; + + std::initializer_list chunk_sizes = {5, 8, 19, 53, 127, 512, 1024, 8192}; + + std::vector stats(tests.size() * (chunk_sizes.size()+1)); + + size_t t = 0; + for(const Test& test : tests) + { + printf("Test %s...\n", test.name); + + size_t s = 0; + for(size_t chunk_size : chunk_sizes) + { + printf(" Chunk size %zu\n", chunk_size); + stats[t * (chunk_sizes.size()+1) + s] = chunked_test(test, chunk_size); + s++; + } + printf(" Chunk size oo\n"); + stats[t * (chunk_sizes.size()+1) + s] = chunked_test(test, test.data.size()); + + t++; + } + + std::printf("\n\n" + " Test Total Chunk Parse Mean Median Max Though Through \n" + " Name Bytes Size[B] Calls Time[us] Time[us] Time[us] [MB/s] [MPkt/s] \n" + "---------------------------------------------------------------------------------------------------------\n" + ); + + t=0; + for(const Test& test : tests) + { + size_t s=0; + for(size_t chunk_size : chunk_sizes) + { + const ChunkStats& stat = stats[t * (chunk_sizes.size()+1) + s]; + + std::printf("%16s %8zu %8zu %8zu %8.2f %8.2f %8.2f %8.2f %8.2f\n", + test.name, stat.chunk_size * stat.num_calls, chunk_size, stat.num_calls, + stat.avg_time*1e6f, stat.med_time*1e6f, stat.max_time*1e6f, + test.data.size() / stat.total_time / 1e6f, + test.num_packets / stat.total_time / 1e6f + ); + + s++; + } + const ChunkStats& stat = stats[t * (chunk_sizes.size()+1) + s]; + + std::printf("%16s %8zu infinite %8zu %8.2f %8.2f %8.2f %8.2f %8.2f\n", + test.name, stat.chunk_size * stat.num_calls, stat.num_calls, + stat.avg_time*1e6f, stat.med_time*1e6f, stat.max_time*1e6f, + test.data.size() / stat.total_time / 1e6f, + test.num_packets / stat.total_time / 1e6f + ); + + t++; + } return 0; } From f5086b30d3b8f82a685135f056d06241d165bff5 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 24 Apr 2024 21:32:08 +0000 Subject: [PATCH 220/252] Generate MIP definitions from branches/dev@56338. --- src/mip/definitions/commands_base.c | 2 +- src/mip/definitions/commands_base.cpp | 2 +- src/mip/definitions/commands_base.h | 4 +--- src/mip/definitions/commands_base.hpp | 8 +++----- 4 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/mip/definitions/commands_base.c b/src/mip/definitions/commands_base.c index 5d5bbac6a..fd72ea2fa 100644 --- a/src/mip/definitions/commands_base.c +++ b/src/mip/definitions/commands_base.c @@ -381,7 +381,7 @@ mip_cmd_result mip_base_write_gps_time_update(struct mip_interface* device, mip_ assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_BASE_CMD_DESC_SET, MIP_CMD_DESC_BASE_GPS_TIME_BROADCAST_NEW, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_BASE_CMD_DESC_SET, MIP_CMD_DESC_BASE_GPS_TIME_UPDATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } mip_cmd_result mip_base_soft_reset(struct mip_interface* device) { diff --git a/src/mip/definitions/commands_base.cpp b/src/mip/definitions/commands_base.cpp index bdd9de55d..35965b4cb 100644 --- a/src/mip/definitions/commands_base.cpp +++ b/src/mip/definitions/commands_base.cpp @@ -481,7 +481,7 @@ TypedResult writeGpsTimeUpdate(C::mip_interface& device, GpsTimeU assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPS_TIME_BROADCAST_NEW, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_GPS_TIME_UPDATE, buffer, (uint8_t)mip_serializer_length(&serializer)); } void insert(Serializer& serializer, const SoftReset& self) { diff --git a/src/mip/definitions/commands_base.h b/src/mip/definitions/commands_base.h index 07b1d7e6b..6d30392c6 100644 --- a/src/mip/definitions/commands_base.h +++ b/src/mip/definitions/commands_base.h @@ -42,9 +42,7 @@ enum MIP_CMD_DESC_BASE_GET_EXTENDED_DESCRIPTORS = 0x07, MIP_CMD_DESC_BASE_CONTINUOUS_BIT = 0x08, MIP_CMD_DESC_BASE_COMM_SPEED = 0x09, - MIP_CMD_DESC_BASE_GPS_TIME_BROADCAST = 0x71, - MIP_CMD_DESC_BASE_GPS_TIME_BROADCAST_NEW = 0x72, - MIP_CMD_DESC_BASE_SYSTEM_TIME = 0x73, + MIP_CMD_DESC_BASE_GPS_TIME_UPDATE = 0x72, MIP_CMD_DESC_BASE_SOFT_RESET = 0x7E, MIP_REPLY_DESC_BASE_DEVICE_INFO = 0x81, diff --git a/src/mip/definitions/commands_base.hpp b/src/mip/definitions/commands_base.hpp index e9d1bf0c3..65a17e7e4 100644 --- a/src/mip/definitions/commands_base.hpp +++ b/src/mip/definitions/commands_base.hpp @@ -41,9 +41,7 @@ enum CMD_GET_EXTENDED_DESCRIPTORS = 0x07, CMD_CONTINUOUS_BIT = 0x08, CMD_COMM_SPEED = 0x09, - CMD_GPS_TIME_BROADCAST = 0x71, - CMD_GPS_TIME_BROADCAST_NEW = 0x72, - CMD_SYSTEM_TIME = 0x73, + CMD_GPS_TIME_UPDATE = 0x72, CMD_SOFT_RESET = 0x7E, REPLY_DEVICE_INFO = 0x81, @@ -719,10 +717,10 @@ struct GpsTimeUpdate uint32_t value = 0; ///< Week number or time of week, depending on the field_id. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_base::DESCRIPTOR_SET; - static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_BROADCAST_NEW; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_base::CMD_GPS_TIME_UPDATE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; static constexpr const char* NAME = "GpsTimeUpdate"; - static constexpr const char* DOC_NAME = "Time Broadcast Command"; + static constexpr const char* DOC_NAME = "GPS Time Update Command"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; static constexpr const uint32_t WRITE_PARAMS = 0x8003; From e346aeb409f04045fb4602970c29f9a3a7906c21 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 24 Apr 2024 17:43:46 -0400 Subject: [PATCH 221/252] Rename timestamp_type, timeout_type, and remaining_count (https://lordsensing.atlassian.net/browse/MSCLLITE-102). --- examples/CV7/CV7_example.c | 20 ++++++++++---------- examples/GQ7/GQ7_example.c | 16 ++++++++-------- examples/GX5_45/GX5_45_example.c | 16 ++++++++-------- examples/watch_imu.c | 14 +++++++------- src/mip/mip_cmdqueue.c | 20 ++++++++++---------- src/mip/mip_cmdqueue.h | 26 +++++++++++++------------- src/mip/mip_device.cpp | 2 +- src/mip/mip_device.hpp | 10 +++++----- src/mip/mip_dispatch.c | 6 +++--- src/mip/mip_dispatch.h | 6 +++--- src/mip/mip_interface.c | 18 +++++++++--------- src/mip/mip_interface.h | 18 +++++++++--------- src/mip/mip_packet.c | 16 ++++++++-------- src/mip/mip_packet.h | 8 ++++---- src/mip/mip_parser.c | 20 ++++++++++---------- src/mip/mip_parser.h | 22 +++++++++++----------- src/mip/mip_types.h | 16 ++++++++-------- src/mip/utils/serialization.c | 4 ++-- src/mip/utils/serialization.h | 2 +- test/mip/test_mip_parser.c | 2 +- test/mip/test_mip_random.c | 20 ++++++++++---------- 21 files changed, 141 insertions(+), 141 deletions(-) diff --git a/examples/CV7/CV7_example.c b/examples/CV7/CV7_example.c index a36465a5c..d455ef6a5 100644 --- a/examples/CV7/CV7_example.c +++ b/examples/CV7/CV7_example.c @@ -71,9 +71,9 @@ const uint8_t FILTER_PITCH_EVENT_ACTION_ID = 2; //Required MIP interface user-defined functions -timestamp_type get_current_timestamp(); +mip_timestamp get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -81,7 +81,7 @@ int usage(const char* argv0); void exit_gracefully(const char *message); bool should_exit(); -void handle_filter_event_source(void* user, const mip_field* field, timestamp_type timestamp); +void handle_filter_event_source(void* user, const mip_field* field, mip_timestamp timestamp); //////////////////////////////////////////////////////////////////////////////// @@ -313,8 +313,8 @@ int main(int argc, const char* argv[]) //Main Loop: Update the interface and process data // - bool running = true; - timestamp_type prev_print_timestamp = 0; + bool running = true; + mip_timestamp prev_print_timestamp = 0; printf("Sensor is configured... waiting for filter to enter AHRS mode.\n"); @@ -332,7 +332,7 @@ int main(int argc, const char* argv[]) //Once in full nav, print out data at 10 Hz if(filter_state_ahrs) { - timestamp_type curr_time = get_current_timestamp(); + mip_timestamp curr_time = get_current_timestamp(); if(curr_time - prev_print_timestamp >= 100) { @@ -355,7 +355,7 @@ int main(int argc, const char* argv[]) // Filter Event Source Field Handler //////////////////////////////////////////////////////////////////////////////// -void handle_filter_event_source(void* user, const mip_field* field, timestamp_type timestamp) +void handle_filter_event_source(void* user, const mip_field* field, mip_timestamp timestamp) { mip_shared_event_source_data data; @@ -373,12 +373,12 @@ void handle_filter_event_source(void* user, const mip_field* field, timestamp_ty // MIP Interface Time Access Function //////////////////////////////////////////////////////////////////////////////// -timestamp_type get_current_timestamp() +mip_timestamp get_current_timestamp() { clock_t curr_time; curr_time = clock(); - return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); + return (mip_timestamp)((double)(curr_time - start_time) / (double)CLOCKS_PER_SEC * 1000.0); } @@ -386,7 +386,7 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out) { *timestamp_out = get_current_timestamp(); return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index f56765a5a..cd257d080 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -78,9 +78,9 @@ bool filter_state_full_nav = false; //Required MIP interface user-defined functions -timestamp_type get_current_timestamp(); +mip_timestamp get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -327,8 +327,8 @@ int main(int argc, const char* argv[]) //Main Loop: Update the interface and process data // - bool running = true; - timestamp_type prev_print_timestamp = 0; + bool running = true; + mip_timestamp prev_print_timestamp = 0; printf("Sensor is configured... waiting for filter to enter Full Navigation mode.\n"); @@ -360,7 +360,7 @@ int main(int argc, const char* argv[]) //Once in full nav, print out data at 1 Hz if(filter_state_full_nav) { - timestamp_type curr_time = get_current_timestamp(); + mip_timestamp curr_time = get_current_timestamp(); if(curr_time - prev_print_timestamp >= 1000) { @@ -384,12 +384,12 @@ int main(int argc, const char* argv[]) // MIP Interface Time Access Function //////////////////////////////////////////////////////////////////////////////// -timestamp_type get_current_timestamp() +mip_timestamp get_current_timestamp() { clock_t curr_time; curr_time = clock(); - return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); + return (mip_timestamp)((double)(curr_time - start_time) / (double)CLOCKS_PER_SEC * 1000.0); } @@ -397,7 +397,7 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out) { *timestamp_out = get_current_timestamp(); return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); diff --git a/examples/GX5_45/GX5_45_example.c b/examples/GX5_45/GX5_45_example.c index 745f3b645..b895bc64e 100644 --- a/examples/GX5_45/GX5_45_example.c +++ b/examples/GX5_45/GX5_45_example.c @@ -77,9 +77,9 @@ bool filter_state_running = false; //Required MIP interface user-defined functions -timestamp_type get_current_timestamp(); +mip_timestamp get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -301,8 +301,8 @@ int main(int argc, const char* argv[]) //Main Loop: Update the interface and process data // - bool running = true; - timestamp_type prev_print_timestamp = 0; + bool running = true; + mip_timestamp prev_print_timestamp = 0; printf("Sensor is configured... waiting for filter to enter running mode.\n"); @@ -329,7 +329,7 @@ int main(int argc, const char* argv[]) //Once in running mode, print out data at 1 Hz if(filter_state_running) { - timestamp_type curr_time = get_current_timestamp(); + mip_timestamp curr_time = get_current_timestamp(); if(curr_time - prev_print_timestamp >= 1000) { @@ -353,12 +353,12 @@ int main(int argc, const char* argv[]) // MIP Interface Time Access Function //////////////////////////////////////////////////////////////////////////////// -timestamp_type get_current_timestamp() +mip_timestamp get_current_timestamp() { clock_t curr_time; curr_time = clock(); - return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); + return (mip_timestamp)((double)(curr_time - start_time) / (double)CLOCKS_PER_SEC * 1000.0); } @@ -366,7 +366,7 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out) { *timestamp_out = get_current_timestamp(); return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); diff --git a/examples/watch_imu.c b/examples/watch_imu.c index 048c1ccf5..e2db1e959 100644 --- a/examples/watch_imu.c +++ b/examples/watch_imu.c @@ -50,7 +50,7 @@ void customLog(void* user, mip_log_level level, const char* fmt, va_list args) } } -void handlePacket(void* unused, const mip_packet* packet, timestamp_type timestamp) +void handlePacket(void* unused, const mip_packet* packet, mip_timestamp timestamp) { (void)unused; @@ -65,7 +65,7 @@ void handlePacket(void* unused, const mip_packet* packet, timestamp_type timesta printf("\n"); } -void handleAccel(void* user, const mip_field* field, timestamp_type timestamp) +void handleAccel(void* user, const mip_field* field, mip_timestamp timestamp) { (void)user; mip_sensor_scaled_accel_data data; @@ -82,7 +82,7 @@ void handleAccel(void* user, const mip_field* field, timestamp_type timestamp) } } -void handleGyro(void* user, const mip_field* field, timestamp_type timestamp) +void handleGyro(void* user, const mip_field* field, mip_timestamp timestamp) { (void)user; mip_sensor_scaled_gyro_data data; @@ -91,7 +91,7 @@ void handleGyro(void* user, const mip_field* field, timestamp_type timestamp) printf("Gyro Data: %f, %f, %f\n", data.scaled_gyro[0], data.scaled_gyro[1], data.scaled_gyro[2]); } -void handleMag(void* user, const mip_field* field, timestamp_type timestamp) +void handleMag(void* user, const mip_field* field, mip_timestamp timestamp) { (void)user; mip_sensor_scaled_mag_data data; @@ -103,18 +103,18 @@ void handleMag(void* user, const mip_field* field, timestamp_type timestamp) time_t startTime; -timestamp_type get_current_timestamp() +mip_timestamp get_current_timestamp() { time_t t; time(&t); double delta = difftime(t, startTime); - return (timestamp_type)(delta * 1000); + return (mip_timestamp)(delta * 1000); } -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out) { (void)device; diff --git a/src/mip/mip_cmdqueue.c b/src/mip/mip_cmdqueue.c index 068378262..085793ea2 100644 --- a/src/mip/mip_cmdqueue.c +++ b/src/mip/mip_cmdqueue.c @@ -39,7 +39,7 @@ void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t ///@param additional_time /// Additional time on top of the base reply timeout for this specific command. /// -void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time) +void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, mip_timeout additional_time) { mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time); } @@ -83,7 +83,7 @@ void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor ///@param additional_time /// Additional time on top of the base reply timeout for this specific command. /// -void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time) +void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, mip_timeout additional_time) { cmd->_next = NULL; cmd->_response_buffer = NULL; @@ -157,7 +157,7 @@ uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd) ///@returns The time remaining before the command times out. The value will be /// negative if the timeout time has passed. /// -int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now) +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, mip_timestamp now) { assert(cmd->_status == MIP_STATUS_WAITING); @@ -175,7 +175,7 @@ int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type no ///@returns true if the command should time out. Only possible for MIP_STATUS_WAITING. ///@returns false if the command should not time out. /// -bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now) +bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, mip_timestamp now) { if( cmd->_status == MIP_STATUS_WAITING ) { @@ -199,7 +199,7 @@ bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type no /// used to accommodate long communication latencies, such as when using /// a TCP connection. /// -void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout) +void mip_cmd_queue_init(mip_cmd_queue* queue, mip_timeout base_reply_timeout) { queue->_first_pending_cmd = NULL; queue->_base_timeout = base_reply_timeout; @@ -267,7 +267,7 @@ void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd) /// not updated). The caller should set pending->_status to this value /// after doing any additional processing requiring the pending struct. /// -static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp) +static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, mip_timeout base_timeout, mip_timestamp timestamp) { assert( pending->_status != MIP_STATUS_NONE ); // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here. assert( !mip_cmd_result_is_finished(pending->_status) ); // Command shouldn't be finished yet - make sure the queue is processed properly. @@ -374,7 +374,7 @@ static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pendi ///@param packet The received MIP packet. Assumed to be valid. ///@param timestamp The time the packet was received /// -void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp) +void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, mip_timestamp timestamp) { // Check if the packet is a command descriptor set. const uint8_t descriptor_set = mip_packet_descriptor_set(packet); @@ -439,7 +439,7 @@ void mip_cmd_queue_clear(mip_cmd_queue* queue) ///@param queue ///@param now /// -void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) +void mip_cmd_queue_update(mip_cmd_queue* queue, mip_timestamp now) { if( queue->_first_pending_cmd ) { @@ -476,7 +476,7 @@ void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now) ///@param queue ///@param timeout /// -void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout) +void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, mip_timeout timeout) { queue->_base_timeout = timeout; } @@ -486,7 +486,7 @@ void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type tim /// ///@returns The minimum time to wait for a reply to any command. /// -timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) +mip_timeout mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue) { return queue->_base_timeout; } diff --git a/src/mip/mip_cmdqueue.h b/src/mip/mip_cmdqueue.h index fc0110935..868e735ec 100644 --- a/src/mip/mip_cmdqueue.h +++ b/src/mip/mip_cmdqueue.h @@ -43,9 +43,9 @@ typedef struct mip_pending_cmd struct mip_pending_cmd* _next; ///<@private Next command in the queue. uint8_t* _response_buffer; ///<@private Buffer for response data if response_descriptor != 0x00. union { ///<@private - timeout_type _extra_timeout; ///<@private If MIP_STATUS_PENDING: Duration to wait for reply, excluding base timeout time from the queue object. - timestamp_type _timeout_time; ///<@private If MIP_STATUS_WAITING: timestamp_type after which the command will be timed out. - timestamp_type _reply_time; ///<@private If MIP_STATUS_COMPLETED: timestamp_type from the packet containing the ack/nack. + mip_timeout _extra_timeout; ///<@private If MIP_STATUS_PENDING: Duration to wait for reply, excluding base timeout time from the queue object. + mip_timestamp _timeout_time; ///<@private If MIP_STATUS_WAITING: timestamp_type after which the command will be timed out. + mip_timestamp _reply_time; ///<@private If MIP_STATUS_COMPLETED: timestamp_type from the packet containing the ack/nack. }; uint8_t _descriptor_set; ///<@private Command descriptor set. uint8_t _field_descriptor; ///<@private Command field descriptor. @@ -58,17 +58,17 @@ typedef struct mip_pending_cmd } mip_pending_cmd; void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor); -void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time); +void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, mip_timeout additional_time); void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size); -void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_size, timeout_type additional_time); +void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_size, mip_timeout additional_time); enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd); uint8_t mip_pending_cmd_response_descriptor(const mip_pending_cmd* cmd); const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd); uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd); -int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now); -bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now); +int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, mip_timestamp now); +bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, mip_timestamp now); ///@} //////////////////////////////////////////////////////////////////////////////// @@ -93,7 +93,7 @@ bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type no typedef struct mip_cmd_queue { mip_pending_cmd* _first_pending_cmd; - timeout_type _base_timeout; + mip_timeout _base_timeout; #ifdef MIP_ENABLE_DIAGNOSTICS uint16_t _diag_cmds_queued; ///<@private Number of queued commands. @@ -105,18 +105,18 @@ typedef struct mip_cmd_queue } mip_cmd_queue; -void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout); +void mip_cmd_queue_init(mip_cmd_queue* queue, mip_timeout base_reply_timeout); void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd); void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd); void mip_cmd_queue_clear(mip_cmd_queue* queue); -void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type timestamp); +void mip_cmd_queue_update(mip_cmd_queue* queue, mip_timestamp timestamp); -void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout); -timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue); +void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, mip_timeout timeout); +mip_timeout mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue); -void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp); +void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, mip_timestamp timestamp); #ifdef MIP_ENABLE_DIAGNOSTICS diff --git a/src/mip/mip_device.cpp b/src/mip/mip_device.cpp index 5e68c032d..d1d47968a 100644 --- a/src/mip/mip_device.cpp +++ b/src/mip/mip_device.cpp @@ -37,7 +37,7 @@ void Connection::connect_interface(C::mip_interface* device) { return static_cast(C::mip_interface_user_pointer(device))->sendToDevice(data, length); }; - auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out)->bool + auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::mip_timeout wait_time, size_t* length_out, C::mip_timestamp* timestamp_out)->bool { return static_cast(C::mip_interface_user_pointer(device))->recvFromDevice(buffer, max_length, wait_time, length_out, timestamp_out); }; diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 2034142c3..55eb14afa 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -363,7 +363,7 @@ void DeviceInterface::setSendFunction() template void DeviceInterface::setRecvFunction() { - setRecvFunction([](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out){ + setRecvFunction([](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::mip_timeout wait_time, size_t* length_out, C::mip_timestamp* timestamp_out){ return (*Recv)(*static_cast(device), buffer, max_length, wait_time, length_out, timestamp_out); }); } @@ -376,7 +376,7 @@ void DeviceInterface::setRecvFunction() template void DeviceInterface::setUpdateFunction() { - setUpdateFunction([](C::mip_interface* device, C::timeout_type wait_time){ + setUpdateFunction([](C::mip_interface* device, C::mip_timeout wait_time){ return (*Update)(*static_cast(device), wait_time); }); } @@ -451,7 +451,7 @@ void DeviceInterface::setUpdateFunction() static_assert(std::is_base_of::value, "Derived must be derived from C::mip_interface."); setUpdateFunction( - [](C::mip_interface* device, C::timeout_type wait_time)->bool + [](C::mip_interface* device, C::mip_timeout wait_time)->bool { return (static_cast(device)->*Update)(wait_time); } @@ -508,11 +508,11 @@ void DeviceInterface::setCallbacks(T* object) { return (static_cast(mip_interface_user_pointer(device))->*Send)(data, size); }; - auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::timeout_type wait_time, size_t* length_out, C::timestamp_type* timestamp_out) + auto recv = [](C::mip_interface* device, uint8_t* buffer, size_t max_length, C::mip_timeout wait_time, size_t* length_out, C::mip_timestamp* timestamp_out) { return (static_cast(mip_interface_user_pointer(device))->*Recv)(buffer, max_length, wait_time, length_out, timestamp_out); }; - auto update = [](C::mip_interface* device, C::timeout_type wait_time) + auto update = [](C::mip_interface* device, C::mip_timeout wait_time) { return (static_cast(mip_interface_user_pointer(device))->*Update)(wait_time); }; diff --git a/src/mip/mip_dispatch.c b/src/mip/mip_dispatch.c index 811ba84b2..251e1770e 100644 --- a/src/mip/mip_dispatch.c +++ b/src/mip/mip_dispatch.c @@ -267,7 +267,7 @@ static bool mip_dispatch_is_descriptor_set_match(uint8_t desc_set, uint8_t handl ///@param timestamp Packet parse time. ///@param post If true, this is called after field iteration. Otherwise before. /// -static void mip_dispatcher_call_packet_callbacks(mip_dispatcher* self, const mip_packet* packet, timestamp_type timestamp, bool post) +static void mip_dispatcher_call_packet_callbacks(mip_dispatcher* self, const mip_packet* packet, mip_timestamp timestamp, bool post) { const uint8_t descriptor_set = mip_packet_descriptor_set(packet); @@ -312,7 +312,7 @@ static bool mip_dispatch_is_descriptor_match(uint8_t desc_set, uint8_t field_des ///@param field Valid MIP field. ///@param timestamp Packet parse time. /// -static void mip_dispatcher_call_field_callbacks(mip_dispatcher* self, const mip_field* field, timestamp_type timestamp) +static void mip_dispatcher_call_field_callbacks(mip_dispatcher* self, const mip_field* field, mip_timestamp timestamp) { const uint8_t descriptor_set = mip_field_descriptor_set(field); const uint8_t field_descriptor = mip_field_field_descriptor(field); @@ -348,7 +348,7 @@ static void mip_dispatcher_call_field_callbacks(mip_dispatcher* self, const mip_ ///@param timestamp /// The approximate parse time of the packet. /// -void mip_dispatcher_dispatch_packet(mip_dispatcher* self, const mip_packet* packet, timestamp_type timestamp) +void mip_dispatcher_dispatch_packet(mip_dispatcher* self, const mip_packet* packet, mip_timestamp timestamp) { mip_dispatcher_call_packet_callbacks(self, packet, timestamp, false); diff --git a/src/mip/mip_dispatch.h b/src/mip/mip_dispatch.h index 09357fa88..59b02dbbf 100644 --- a/src/mip/mip_dispatch.h +++ b/src/mip/mip_dispatch.h @@ -35,7 +35,7 @@ namespace C { ///@param packet The MIP packet triggering this callback. ///@param timestamp The approximate parse time of the packet. /// -typedef void (*mip_dispatch_packet_callback)(void* context, const mip_packet* packet, timestamp_type timestamp); +typedef void (*mip_dispatch_packet_callback)(void* context, const mip_packet* packet, mip_timestamp timestamp); //////////////////////////////////////////////////////////////////////////////// ///@brief Signature for field-level callbacks. @@ -44,7 +44,7 @@ typedef void (*mip_dispatch_packet_callback)(void* context, const mip_packet* pa ///@param field The MIP field triggering this callback. ///@param timestamp The approximate parse time of the packet. /// -typedef void (*mip_dispatch_field_callback )(void* context, const mip_field* field, timestamp_type timestamp); +typedef void (*mip_dispatch_field_callback )(void* context, const mip_field* field, mip_timestamp timestamp); //////////////////////////////////////////////////////////////////////////////// ///@brief Signature for extraction callbacks. @@ -129,7 +129,7 @@ void mip_dispatcher_add_handler(mip_dispatcher* self, mip_dispatch_handler* hand void mip_dispatcher_remove_handler(mip_dispatcher* self, mip_dispatch_handler* handler); void mip_dispatcher_remove_all_handlers(mip_dispatcher* self); -void mip_dispatcher_dispatch_packet(mip_dispatcher* self, const mip_packet* packet, timestamp_type timestamp); +void mip_dispatcher_dispatch_packet(mip_dispatcher* self, const mip_packet* packet, mip_timestamp timestamp); ///@} ///@} diff --git a/src/mip/mip_interface.c b/src/mip/mip_interface.c index d0c65735f..533a1fdfc 100644 --- a/src/mip/mip_interface.c +++ b/src/mip/mip_interface.c @@ -90,7 +90,7 @@ /// void mip_interface_init( mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, - timeout_type parse_timeout, timeout_type base_reply_timeout, + mip_timeout parse_timeout, mip_timeout base_reply_timeout, mip_send_callback send, mip_recv_callback recv, mip_update_callback update, void* user_pointer) { @@ -311,7 +311,7 @@ bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, si ///@returns False if the receive callback is NULL. ///@returns False if the receive callback failed (i.e. if it returned false). /// -bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* length_out, timestamp_type* timestamp_out) +bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* length_out, mip_timestamp* timestamp_out) { return device->_recv_callback && device->_recv_callback(device, buffer, max_length, wait_time, length_out, timestamp_out); } @@ -336,7 +336,7 @@ bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size /// updated (e.g. if the serial port is not open). /// -bool mip_interface_update(struct mip_interface* device, timeout_type wait_time) +bool mip_interface_update(struct mip_interface* device, mip_timeout wait_time) { if( !device->_update_callback ) return false; @@ -359,7 +359,7 @@ bool mip_interface_update(struct mip_interface* device, timeout_type wait_time) /// ///@returns The value returned by mip_interface_user_recv_from_device. /// -bool mip_interface_default_update(struct mip_interface* device, timeout_type wait_time) +bool mip_interface_default_update(struct mip_interface* device, mip_timeout wait_time) { if( !device->_recv_callback ) return false; @@ -368,8 +368,8 @@ bool mip_interface_default_update(struct mip_interface* device, timeout_type wai mip_parser* parser = mip_interface_parser(device); size_t max_count = mip_parser_get_write_ptr(parser, &ptr); - size_t count = 0; - timestamp_type timestamp = 0; + size_t count = 0; + mip_timestamp timestamp = 0; if ( !mip_interface_recv_from_device(device, ptr, max_count, wait_time, &count, ×tamp) ) return false; @@ -398,7 +398,7 @@ bool mip_interface_default_update(struct mip_interface* device, timeout_type wai ///@returns The amount of data which couldn't be processed due to the limit on /// number of packets per parse call. Normally the result is 0. /// -remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, timestamp_type timestamp) +mip_remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, mip_timestamp timestamp) { return mip_parser_parse(&device->_parser, data, length, timestamp, device->_max_update_pkts); } @@ -430,7 +430,7 @@ void mip_interface_process_unparsed_packets(mip_interface* device) ///@param timestamp /// timestamp_type of the received MIP packet. /// -void mip_interface_receive_packet(mip_interface* device, const mip_packet* packet, timestamp_type timestamp) +void mip_interface_receive_packet(mip_interface* device, const mip_packet* packet, mip_timestamp timestamp) { mip_cmd_queue_process_packet(&device->_queue, packet, timestamp); mip_dispatcher_dispatch_packet(&device->_dispatcher, packet, timestamp); @@ -445,7 +445,7 @@ void mip_interface_receive_packet(mip_interface* device, const mip_packet* packe /// ///@returns True /// -bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp) +bool mip_interface_parse_callback(void* device, const mip_packet* packet, mip_timestamp timestamp) { mip_interface_receive_packet(device, packet, timestamp); diff --git a/src/mip/mip_interface.h b/src/mip/mip_interface.h index 7784fbcc4..0a939a06a 100644 --- a/src/mip/mip_interface.h +++ b/src/mip/mip_interface.h @@ -34,8 +34,8 @@ struct mip_interface; // Documentation is in source file. typedef bool (*mip_send_callback)(struct mip_interface* device, const uint8_t* data, size_t length); -typedef bool (*mip_recv_callback)(struct mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); -typedef bool (*mip_update_callback)(struct mip_interface* device, timeout_type timeout); +typedef bool (*mip_recv_callback)(struct mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out); +typedef bool (*mip_update_callback)(struct mip_interface* device, mip_timeout timeout); //////////////////////////////////////////////////////////////////////////////// @@ -56,7 +56,7 @@ typedef struct mip_interface void mip_interface_init( mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, - timeout_type parse_timeout, timeout_type base_reply_timeout, + mip_timeout parse_timeout, mip_timeout base_reply_timeout, mip_send_callback send, mip_recv_callback recv, mip_update_callback update, void* user_pointer ); @@ -66,14 +66,14 @@ void mip_interface_init( // bool mip_interface_send_to_device(mip_interface* device, const uint8_t* data, size_t length); -bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type timeout, size_t* length_out, timestamp_type* now); -bool mip_interface_update(mip_interface* device, timeout_type wait_time); +bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout timeout, size_t* length_out, mip_timestamp* now); +bool mip_interface_update(mip_interface* device, mip_timeout wait_time); -bool mip_interface_default_update(mip_interface* device, timeout_type wait_time); -remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, timestamp_type timestamp); +bool mip_interface_default_update(mip_interface* device, mip_timeout wait_time); +mip_remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, mip_timestamp timestamp); void mip_interface_process_unparsed_packets(mip_interface* device); -bool mip_interface_parse_callback(void* device, const mip_packet* packet, timestamp_type timestamp); -void mip_interface_receive_packet(mip_interface* device, const mip_packet* packet, timestamp_type timestamp); +bool mip_interface_parse_callback(void* device, const mip_packet* packet, mip_timestamp timestamp); +void mip_interface_receive_packet(mip_interface* device, const mip_packet* packet, mip_timestamp timestamp); // // Commands diff --git a/src/mip/mip_packet.c b/src/mip/mip_packet.c index d7851e3a9..a8eb55135 100644 --- a/src/mip/mip_packet.c +++ b/src/mip/mip_packet.c @@ -231,7 +231,7 @@ packet_length mip_packet_buffer_size(const mip_packet* packet) /// /// This is equal to the buffer size less the total packet length. /// -remaining_count mip_packet_remaining_space(const mip_packet* packet) +mip_remaining_count mip_packet_remaining_space(const mip_packet* packet) { return mip_packet_buffer_size(packet) - mip_packet_total_length(packet); } @@ -285,7 +285,7 @@ bool mip_packet_is_data(const mip_packet* packet) bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length) { uint8_t* payload_buffer; - remaining_count remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer); + mip_remaining_count remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer); if( remaining < 0 ) return false; @@ -324,12 +324,12 @@ bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const ui /// is negative, the field could not be allocated and the payload must /// not be written. /// -remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out) +mip_remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out) { assert(payload_ptr_out != NULL); // assert( payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); - const remaining_count remaining = mip_packet_remaining_space(packet); + const mip_remaining_count remaining = mip_packet_remaining_space(packet); const packet_length field_length = MIP_FIELD_HEADER_LENGTH + (packet_length)payload_length; @@ -371,7 +371,7 @@ remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descrip ///@returns The space remaining in the packet after changing the field size. /// This will be negative if the new length did not fit. /// -remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length) +mip_remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length) { assert(payload_ptr != NULL); assert( new_payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); @@ -380,9 +380,9 @@ remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* paylo const uint8_t old_field_length = field_ptr[MIP_INDEX_FIELD_LEN]; const uint8_t new_field_length = new_payload_length + MIP_FIELD_HEADER_LENGTH; - const remaining_count delta_length = new_field_length - old_field_length; + const mip_remaining_count delta_length = new_field_length - old_field_length; - const remaining_count remaining = mip_packet_remaining_space(packet) - delta_length; + const mip_remaining_count remaining = mip_packet_remaining_space(packet) - delta_length; if( remaining >= 0 ) { @@ -407,7 +407,7 @@ remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* paylo /// ///@returns The remaining space in the packet after removing the field. /// -remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr) +mip_remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr) { assert(payload_ptr != NULL); diff --git a/src/mip/mip_packet.h b/src/mip/mip_packet.h index 0f22d98e6..907e970f5 100644 --- a/src/mip/mip_packet.h +++ b/src/mip/mip_packet.h @@ -60,9 +60,9 @@ typedef struct mip_packet void mip_packet_create(mip_packet* packet, uint8_t* buffer, size_t buffer_size, uint8_t descriptor_set); bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length); -remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** payload_ptr_out); -remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length); -remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr); +mip_remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** payload_ptr_out); +mip_remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length); +mip_remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr); void mip_packet_finalize(mip_packet* packet); @@ -99,7 +99,7 @@ bool mip_packet_is_valid(const mip_packet* packet); bool mip_packet_is_empty(const mip_packet* packet); packet_length mip_packet_buffer_size(const mip_packet* packet); -remaining_count mip_packet_remaining_space(const mip_packet* packet); +mip_remaining_count mip_packet_remaining_space(const mip_packet* packet); bool mip_packet_is_data(const mip_packet* packet); diff --git a/src/mip/mip_parser.c b/src/mip/mip_parser.c index 6b2f52fef..1964db018 100644 --- a/src/mip/mip_parser.c +++ b/src/mip/mip_parser.c @@ -29,7 +29,7 @@ /// The timeout for receiving one packet. Depends on the serial baud rate /// and is typically 100 milliseconds. /// -void mip_parser_init(mip_parser* parser, uint8_t* buffer, size_t buffer_size, mip_packet_callback callback, void* callback_object, timestamp_type timeout) +void mip_parser_init(mip_parser* parser, uint8_t* buffer, size_t buffer_size, mip_packet_callback callback, void* callback_object, mip_timestamp timeout) { byte_ring_init(&parser->_ring, buffer, buffer_size); @@ -112,7 +112,7 @@ void mip_parser_reset(mip_parser* parser) /// conntains 0x75,0x65, has at least 6 bytes, and has a valid checksum. A /// 16-bit checksum has a 1 in 65,536 chance of appearing to be valid. /// -remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, timestamp_type timestamp, unsigned int max_packets) +mip_remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, mip_timestamp timestamp, unsigned int max_packets) { // Reset the state if the timeout time has elapsed. if( parser->_expected_length != MIPPARSER_RESET_LENGTH && (timestamp - parser->_start_time) > parser->_timeout ) @@ -152,7 +152,7 @@ remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer MIP_DIAG_INC(parser->_diag_bytes_read, count); - return -(remaining_count)input_count; + return -(mip_remaining_count)input_count; } } @@ -162,7 +162,7 @@ remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer } while( input_count ); - return -(remaining_count)input_count; + return -(mip_remaining_count)input_count; } //////////////////////////////////////////////////////////////////////////////// @@ -179,7 +179,7 @@ remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer ///@returns true if a packet was found, false if more data is required. If false, /// the packet is not initialized. /// -bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packet_out, timestamp_type timestamp) +bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packet_out, mip_timestamp timestamp) { // Parse packets while there is sufficient data in the ring buffer. while( byte_ring_count(&parser->_ring) >= parser->_expected_length ) @@ -254,7 +254,7 @@ bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packe ///@brief Returns the packet timeout of the parser. /// /// -timestamp_type mip_parser_timeout(const mip_parser* parser) +mip_timestamp mip_parser_timeout(const mip_parser* parser) { return parser->_timeout; } @@ -266,7 +266,7 @@ timestamp_type mip_parser_timeout(const mip_parser* parser) ///@param parser ///@param timeout /// -void mip_parser_set_timeout(mip_parser* parser, timestamp_type timeout) +void mip_parser_set_timeout(mip_parser* parser, mip_timestamp timeout) { parser->_timeout = timeout; } @@ -327,7 +327,7 @@ void* mip_parser_callback_object(const mip_parser* parser) /// won't matter because an additional call to parse won't produce a new /// packet to be timestamped. /// -timestamp_type mip_parser_last_packet_timestamp(const mip_parser* parser) +mip_timestamp mip_parser_last_packet_timestamp(const mip_parser* parser) { return parser->_start_time; } @@ -378,7 +378,7 @@ size_t mip_parser_get_write_ptr(mip_parser* parser, uint8_t** const ptr_out) ///@param timestamp ///@param max_packets /// -void mip_parser_process_written(mip_parser* parser, size_t count, timestamp_type timestamp, unsigned int max_packets) +void mip_parser_process_written(mip_parser* parser, size_t count, mip_timestamp timestamp, unsigned int max_packets) { MIP_DIAG_INC(parser->_diag_bytes_read, count); @@ -472,7 +472,7 @@ uint32_t mip_parser_diagnostic_timeouts(const mip_parser* parser) /// a single mip packet of maximum size at the given baud rate, plus some /// tolerance. /// -timeout_type mip_timeout_from_baudrate(uint32_t baudrate) +mip_timeout mip_timeout_from_baudrate(uint32_t baudrate) { // num_symbols [b] = (packet_length [B]) * (10 [b/B]) unsigned int num_symbols = MIP_PACKET_LENGTH_MAX * 10; diff --git a/src/mip/mip_parser.h b/src/mip/mip_parser.h index 746c5baa5..f938ac403 100644 --- a/src/mip/mip_parser.h +++ b/src/mip/mip_parser.h @@ -42,7 +42,7 @@ extern "C" { ///@param user A user-specified pointer which will be given the callback_object parameter which was previously passed to mip_parser_init. ///@param Packet A pointer to the MIP packet. Do not store this pointer as it will be invalidated after the callback returns. ///@param timestamp The approximate time the packet was parsed. -typedef bool (*mip_packet_callback)(void* user, const mip_packet* packet, timestamp_type timestamp); +typedef bool (*mip_packet_callback)(void* user, const mip_packet* packet, mip_timestamp timestamp); //////////////////////////////////////////////////////////////////////////////// @@ -54,8 +54,8 @@ typedef bool (*mip_packet_callback)(void* user, const mip_packet* packet, timest /// typedef struct mip_parser { - timestamp_type _start_time; ///<@private The timestamp when the first byte was observed by the parser. - timestamp_type _timeout; ///<@private Duration to wait for the rest of the data in a packet. + mip_timestamp _start_time; ///<@private The timestamp when the first byte was observed by the parser. + mip_timestamp _timeout; ///<@private Duration to wait for the rest of the data in a packet. uint8_t _result_buffer[MIP_PACKET_LENGTH_MAX]; ///<@private Buffer used to output MIP packets to the callback. packet_length _expected_length; ///<@private Expected length of the packet currently being parsed. Keeps track of parser state. Always 1, MIP_HEADER_LENGTH, or at least MIP_PACKET_LENGTH_MAX. byte_ring_state _ring; ///<@private Ring buffer which holds data being parsed. User-specified backing buffer and size. @@ -79,27 +79,27 @@ typedef struct mip_parser #define MIPPARSER_DEFAULT_TIMEOUT_MS 100 ///< Specifies the default timeout for a MIP parser, assuming timestamps are in milliseconds. -void mip_parser_init(mip_parser* parser, uint8_t* buffer, size_t buffer_size, mip_packet_callback callback, void* callback_object, timestamp_type timeout); -bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packet_out, timestamp_type timestamp); -remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, timestamp_type timestamp, unsigned int max_packets); +void mip_parser_init(mip_parser* parser, uint8_t* buffer, size_t buffer_size, mip_packet_callback callback, void* callback_object, mip_timestamp timeout); +bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packet_out, mip_timestamp timestamp); +mip_remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, mip_timestamp timestamp, unsigned int max_packets); void mip_parser_reset(mip_parser* parser); size_t mip_parser_get_write_ptr(mip_parser* parser, uint8_t** ptr_out); -void mip_parser_process_written(mip_parser* parser, size_t count, timestamp_type timestamp, unsigned int max_packets); +void mip_parser_process_written(mip_parser* parser, size_t count, mip_timestamp timestamp, unsigned int max_packets); // // Accessors // -timeout_type mip_parser_timeout(const mip_parser* parser); -void mip_parser_set_timeout(mip_parser* parser, timeout_type timeout); +mip_timeout mip_parser_timeout(const mip_parser* parser); +void mip_parser_set_timeout(mip_parser* parser, mip_timeout timeout); void mip_parser_set_callback(mip_parser* parser, mip_packet_callback callback, void* callback_object); mip_packet_callback mip_parser_callback(const mip_parser* parser); void* mip_parser_callback_object(const mip_parser* parser); -timestamp_type mip_parser_last_packet_timestamp(const mip_parser* parser); +mip_timestamp mip_parser_last_packet_timestamp(const mip_parser* parser); // @@ -123,7 +123,7 @@ uint32_t mip_parser_diagnostic_timeouts(const mip_parser* parser); // Misc // -timeout_type mip_timeout_from_baudrate(uint32_t baudrate); +mip_timeout mip_timeout_from_baudrate(uint32_t baudrate); ///@} ///@} diff --git a/src/mip/mip_types.h b/src/mip/mip_types.h index 4bcc4528c..792a1f5dd 100644 --- a/src/mip/mip_types.h +++ b/src/mip/mip_types.h @@ -13,7 +13,7 @@ extern "C" { // Used like a signed version of size_t -typedef int_least16_t remaining_count; +typedef int_least16_t mip_remaining_count; ///@brief Type used for packet timestamps and timeouts. @@ -26,13 +26,13 @@ typedef int_least16_t remaining_count; /// this requirement may result in false timeouts or delays in getting parsed packets. /// #ifdef MIP_TIMESTAMP_TYPE - typedef MIP_TIMESTAMP_TYPE timestamp_type; - static_assert( sizeof(timestamp_type) >= 8 || timestamp_type(-1) > 0, "MIP_TIMESTAMP_TYPE must be unsigned unless 64 bits."); + typedef MIP_TIMESTAMP_TYPE mip_timestamp; + static_assert( sizeof(mip_timestamp) >= 8 || mip_timestamp(-1) > 0, "MIP_TIMESTAMP_TYPE must be unsigned unless 64 bits."); #else - typedef uint64_t timestamp_type; + typedef uint64_t mip_timestamp; #endif -typedef timestamp_type timeout_type; +typedef mip_timestamp mip_timeout; #ifdef MIP_ENABLE_DIAGNOSTICS @@ -57,9 +57,9 @@ typedef timestamp_type timeout_type; } // extern "C" } // namespace C -using RemainingCount = C::remaining_count; -using Timestamp = C::timestamp_type; -using Timeout = C::timeout_type; +using RemainingCount = C::mip_remaining_count; +using Timestamp = C::mip_timestamp; +using Timeout = C::mip_timeout; } // namespace mip diff --git a/src/mip/utils/serialization.c b/src/mip/utils/serialization.c index b6026b3e9..1fa390a6a 100644 --- a/src/mip/utils/serialization.c +++ b/src/mip/utils/serialization.c @@ -66,7 +66,7 @@ void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet* packe serializer->_buffer_size = 0; serializer->_offset = 0; - const remaining_count length = mip_packet_alloc_field(packet, field_descriptor, 0, &serializer->_buffer); + const mip_remaining_count length = mip_packet_alloc_field(packet, field_descriptor, 0, &serializer->_buffer); if( length >= 0 ) serializer->_buffer_size = length; @@ -142,7 +142,7 @@ size_t mip_serializer_length(const mip_serializer* serializer) /// or read more data than contained in the buffer. This is not a bug and /// it can be detected with the mip_serializer_is_ok() function. /// -remaining_count mip_serializer_remaining(const mip_serializer* serializer) +mip_remaining_count mip_serializer_remaining(const mip_serializer* serializer) { return mip_serializer_capacity(serializer) - mip_serializer_length(serializer); } diff --git a/src/mip/utils/serialization.h b/src/mip/utils/serialization.h index ae460ddb6..0970ee4f0 100644 --- a/src/mip/utils/serialization.h +++ b/src/mip/utils/serialization.h @@ -57,7 +57,7 @@ void mip_serializer_init_from_field(mip_serializer* serializer, const mip_field* size_t mip_serializer_capacity(const mip_serializer* serializer); size_t mip_serializer_length(const mip_serializer* serializer); -remaining_count mip_serializer_remaining(const mip_serializer* serializer); +mip_remaining_count mip_serializer_remaining(const mip_serializer* serializer); bool mip_serializer_is_ok(const mip_serializer* serializer); bool mip_serializer_is_complete(const mip_serializer* serializer); diff --git a/test/mip/test_mip_parser.c b/test/mip/test_mip_parser.c index 1dfd37382..7636f5482 100644 --- a/test/mip/test_mip_parser.c +++ b/test/mip/test_mip_parser.c @@ -16,7 +16,7 @@ unsigned int num_errors = 0; size_t bytesRead = 0; size_t bytes_parsed = 0; -bool handle_packet(void* p, const struct mip_packet* packet, timestamp_type t) +bool handle_packet(void* p, const struct mip_packet* packet, mip_timestamp t) { (void)t; diff --git a/test/mip/test_mip_random.c b/test/mip/test_mip_random.c index 492659b71..9494edd2c 100644 --- a/test/mip/test_mip_random.c +++ b/test/mip/test_mip_random.c @@ -12,8 +12,8 @@ struct mip_parser parser; unsigned int num_errors = 0; unsigned int num_packets_parsed = 0; -size_t parsed_packet_length = 0; -timestamp_type parsed_packet_timestamp = 0; +size_t parsed_packet_length = 0; +mip_timestamp parsed_packet_timestamp = 0; void print_packet(FILE* out, const struct mip_packet* packet) @@ -29,7 +29,7 @@ void print_packet(FILE* out, const struct mip_packet* packet) } -bool handle_packet(void* p, const struct mip_packet* packet, timestamp_type timestamp) +bool handle_packet(void* p, const struct mip_packet* packet, mip_timestamp timestamp) { (void)p; @@ -71,7 +71,7 @@ int main(int argc, const char* argv[]) const uint8_t field_desc = (rand() % 255) + 1; // Random field descriptor. uint8_t* payload; - remaining_count available = mip_packet_alloc_field(&packet, field_desc, paylen, &payload); + mip_remaining_count available = mip_packet_alloc_field(&packet, field_desc, paylen, &payload); if( available < 0 ) { @@ -102,13 +102,13 @@ int main(int argc, const char* argv[]) // Keep track of offsets and timestamps for debug purposes. - size_t offsets[MIP_PACKET_PAYLOAD_LENGTH_MAX / MIP_FIELD_HEADER_LENGTH] = {0}; - timestamp_type timestamps[MIP_PACKET_PAYLOAD_LENGTH_MAX / MIP_FIELD_HEADER_LENGTH] = {0}; - unsigned int c = 0; + size_t offsets[MIP_PACKET_PAYLOAD_LENGTH_MAX / MIP_FIELD_HEADER_LENGTH] = {0}; + mip_timestamp timestamps[MIP_PACKET_PAYLOAD_LENGTH_MAX / MIP_FIELD_HEADER_LENGTH] = {0}; + unsigned int c = 0; - const timestamp_type start_time = rand(); - timestamp_type timestamp = start_time; - size_t sent = 0; + const mip_timestamp start_time = rand(); + mip_timestamp timestamp = start_time; + size_t sent = 0; // Send all but the last chunk. while( sent < (packet_size-MIP_PACKET_LENGTH_MIN) ) From 37dc1859b950703c42de1f63f937fb0840873f62 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 24 Apr 2024 18:02:40 -0400 Subject: [PATCH 222/252] Fix some warnings: - use add_compile_options("-Wextra" "-Werror" "-Wno-missing-field-initializers") for gcc - See https://lordsensing.atlassian.net/browse/MSCLLITE-101 --- examples/CV7_INS/simple_ublox_parser.hpp | 6 +++--- examples/watch_imu.c | 1 - src/mip/utils/serial_port.c | 4 ++-- test/mip/test_mip_random.c | 2 -- 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/examples/CV7_INS/simple_ublox_parser.hpp b/examples/CV7_INS/simple_ublox_parser.hpp index 9e99bddbc..5c54b9b85 100644 --- a/examples/CV7_INS/simple_ublox_parser.hpp +++ b/examples/CV7_INS/simple_ublox_parser.hpp @@ -92,7 +92,7 @@ namespace mip uint16_t payload_length; memcpy(&payload_length, payload_length_bytes, sizeof(uint16_t)); - int total_message_length = HEADER_SIZE + payload_length + CHECKSUM_SIZE; + unsigned int total_message_length = HEADER_SIZE + payload_length + CHECKSUM_SIZE; // Check if buffer contains full packet size if (_buffer.size() < total_message_length) @@ -100,7 +100,7 @@ namespace mip // Extract packet std::vector packet(total_message_length); - for (int i = 0; i < total_message_length; i++) + for (unsigned int i = 0; i < total_message_length; i++) packet[i] = _buffer[i]; // Validate checksum @@ -109,7 +109,7 @@ namespace mip _packet_callback(packet); // Clear packet from buffer - for (int i = 0; i < total_message_length; i++) + for (unsigned int i = 0; i < total_message_length; i++) _buffer.pop_front(); } else _buffer.pop_front(); diff --git a/examples/watch_imu.c b/examples/watch_imu.c index e2db1e959..7fd76467d 100644 --- a/examples/watch_imu.c +++ b/examples/watch_imu.c @@ -176,7 +176,6 @@ int main(int argc, const char* argv[]) enum mip_cmd_result result; // Get the base rate. - volatile uint32_t now = clock(); uint16_t base_rate; result = mip_3dm_get_base_rate(&device, MIP_SENSOR_DATA_DESC_SET, &base_rate); diff --git a/src/mip/utils/serial_port.c b/src/mip/utils/serial_port.c index a36ef6e5f..bffa9a829 100644 --- a/src/mip/utils/serial_port.c +++ b/src/mip/utils/serial_port.c @@ -314,10 +314,10 @@ bool serial_port_read(serial_port *port, void *buffer, size_t num_bytes, int wai if(!serial_port_is_open(port)) return false; - uint32_t bytes_available = serial_port_read_count(port); - #ifdef WIN32 //Windows + uint32_t bytes_available = serial_port_read_count(port); + DWORD last_error = GetLastError(); if (last_error != 0) { diff --git a/test/mip/test_mip_random.c b/test/mip/test_mip_random.c index 9494edd2c..8c2a05cb7 100644 --- a/test/mip/test_mip_random.c +++ b/test/mip/test_mip_random.c @@ -96,8 +96,6 @@ int main(int argc, const char* argv[]) // Send this packet to the parser in small chunks. // - const unsigned int MAX_CHUNKS = 5; - const size_t packet_size = mip_packet_total_length(&packet); From 674e85eeff655ff7baba69214ba334ee1f1834cc Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 25 Apr 2024 13:04:35 -0400 Subject: [PATCH 223/252] - Fix a bunch of int conversion warnings. - Change mip_remaining_count to just int. --- CMakeLists.txt | 2 +- src/mip/extras/version.hpp | 2 +- src/mip/mip.hpp | 22 +++++++-------- src/mip/mip_device.hpp | 2 +- src/mip/mip_interface.c | 2 +- src/mip/mip_interface.h | 2 +- src/mip/mip_packet.c | 42 ++++++++++++++++------------- src/mip/mip_packet.h | 18 ++++++------- src/mip/mip_parser.c | 8 +++--- src/mip/mip_parser.h | 4 +-- src/mip/mip_types.h | 2 +- src/mip/utils/serialization.c | 13 +++++---- src/mip/utils/serialization.h | 8 +++--- test/mip/mip_parser_performance.cpp | 2 +- test/mip/test_mip.cpp | 4 +-- test/mip/test_mip_random.c | 2 +- 16 files changed, 69 insertions(+), 66 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ad69bcdb2..c4ccb3f05 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_C_STANDARD 99) +set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) project( diff --git a/src/mip/extras/version.hpp b/src/mip/extras/version.hpp index 23467e2b2..3ff7a6628 100644 --- a/src/mip/extras/version.hpp +++ b/src/mip/extras/version.hpp @@ -44,7 +44,7 @@ class FirmwareVersion void fromParts(uint8_t major, uint8_t minor, uint8_t patch) { m_version = major * 1000 + minor * 100 + patch; } - uint8_t major() const { return m_version / 1000; } + uint8_t major() const { return uint8_t(m_version / 1000); } uint8_t minor() const { return (m_version / 100) % 10; } uint8_t patch() const { return m_version % 100; } diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index ab6c18711..35dc0cb09 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -31,8 +31,6 @@ namespace mip { -using PacketLength = C::packet_length; - template struct MipFieldInfo; //////////////////////////////////////////////////////////////////////////////// @@ -154,9 +152,9 @@ class PacketRef : public C::mip_packet // C function wrappers // - uint8_t descriptorSet() const { return C::mip_packet_descriptor_set(this); } ///<@copydoc mip::C::mip_packet_descriptor_set - PacketLength totalLength() const { return C::mip_packet_total_length(this); } ///<@copydoc mip::C::mip_packet_total_length - uint8_t payloadLength() const { return C::mip_packet_payload_length(this); } ///<@copydoc mip::C::mip_packet_payload_length + uint8_t descriptorSet() const { return C::mip_packet_descriptor_set(this); } ///<@copydoc mip::C::mip_packet_descriptor_set + uint_least16_t totalLength() const { return C::mip_packet_total_length(this); } ///<@copydoc mip::C::mip_packet_total_length + uint8_t payloadLength() const { return C::mip_packet_payload_length(this); } ///<@copydoc mip::C::mip_packet_payload_length bool isData() const { return C::mip_packet_is_data(this); } @@ -170,13 +168,13 @@ class PacketRef : public C::mip_packet bool isValid() const { return C::mip_packet_is_valid(this); } ///<@copydoc mip::C::mip_packet_is_valid bool isEmpty() const { return C::mip_packet_is_empty(this); } ///<@copydoc mip::C::mip_packet_is_empty - PacketLength bufferSize() const { return C::mip_packet_buffer_size(this); } ///<@copydoc mip::C::mip_packet_buffer_size - RemainingCount remainingSpace() const { return C::mip_packet_remaining_space(this); } ///<@copydoc mip::C::mip_packet_remaining_space + uint_least16_t bufferSize() const { return C::mip_packet_buffer_size(this); } ///<@copydoc mip::C::mip_packet_buffer_size + int remainingSpace() const { return C::mip_packet_remaining_space(this); } ///<@copydoc mip::C::mip_packet_remaining_space - bool addField(uint8_t fieldDescriptor, const uint8_t* payload, size_t payloadLength) { return C::mip_packet_add_field(this, fieldDescriptor, payload, payloadLength); } ///<@copydoc mip::C::mip_packet_add_field - RemainingCount allocField(uint8_t fieldDescriptor, uint8_t payloadLength, uint8_t** payloadPtr_out) { return C::mip_packet_alloc_field(this, fieldDescriptor, payloadLength, payloadPtr_out); } ///<@copydoc mip::C::mip_packet_alloc_field - RemainingCount reallocLastField(uint8_t* payloadPtr, uint8_t newPayloadLength) { return C::mip_packet_realloc_last_field(this, payloadPtr, newPayloadLength); } ///<@copydoc mip::C::mip_packet_realloc_last_field - RemainingCount cancelLastField(uint8_t* payloadPtr) { return C::mip_packet_cancel_last_field(this, payloadPtr); } ///<@copydoc mip::C::mip_packet_cancel_last_field + bool addField(uint8_t fieldDescriptor, const uint8_t* payload, uint8_t payloadLength) { return C::mip_packet_add_field(this, fieldDescriptor, payload, payloadLength); } ///<@copydoc mip::C::mip_packet_add_field + int allocField(uint8_t fieldDescriptor, uint8_t payloadLength, uint8_t** payloadPtr_out) { return C::mip_packet_alloc_field(this, fieldDescriptor, payloadLength, payloadPtr_out); } ///<@copydoc mip::C::mip_packet_alloc_field + int reallocLastField(uint8_t* payloadPtr, uint8_t newPayloadLength) { return C::mip_packet_realloc_last_field(this, payloadPtr, newPayloadLength); } ///<@copydoc mip::C::mip_packet_realloc_last_field + int cancelLastField(uint8_t* payloadPtr) { return C::mip_packet_cancel_last_field(this, payloadPtr); } ///<@copydoc mip::C::mip_packet_cancel_last_field void finalize() { C::mip_packet_finalize(this); } ///<@copydoc mip::C::mip_packet_finalize @@ -417,7 +415,7 @@ class Parser : public C::mip_parser void reset() { C::mip_parser_reset(this); } ///@copydoc mip::C::mip_parser_parse - RemainingCount parse(const uint8_t* inputBuffer, size_t inputCount, Timestamp timestamp, unsigned int maxPackets=0) { return C::mip_parser_parse(this, inputBuffer, inputCount, timestamp, maxPackets); } + size_t parse(const uint8_t* inputBuffer, size_t inputCount, Timestamp timestamp, unsigned int maxPackets=0) { return C::mip_parser_parse(this, inputBuffer, inputCount, timestamp, maxPackets); } ///@copydoc mip::C::mip_parser_timeout Timeout timeout() const { return C::mip_parser_timeout(this); } diff --git a/src/mip/mip_device.hpp b/src/mip/mip_device.hpp index 55eb14afa..743186dd3 100644 --- a/src/mip/mip_device.hpp +++ b/src/mip/mip_device.hpp @@ -275,7 +275,7 @@ class DeviceInterface : public C::mip_interface bool update(Timeout wait_time=0) { return C::mip_interface_update(this, wait_time); } bool defaultUpdate(Timeout wait_time=0) { return C::mip_interface_default_update(this, wait_time); } - RemainingCount receiveBytes(const uint8_t* data, size_t length, Timestamp timestamp) { return C::mip_interface_receive_bytes(this, data, length, timestamp); } + size_t receiveBytes(const uint8_t* data, size_t length, Timestamp timestamp) { return C::mip_interface_receive_bytes(this, data, length, timestamp); } void receivePacket(const C::mip_packet& packet, Timestamp timestamp) { C::mip_interface_receive_packet(this, &packet, timestamp); } void processUnparsedPackets() { C::mip_interface_process_unparsed_packets(this); } diff --git a/src/mip/mip_interface.c b/src/mip/mip_interface.c index 533a1fdfc..9870ba06d 100644 --- a/src/mip/mip_interface.c +++ b/src/mip/mip_interface.c @@ -398,7 +398,7 @@ bool mip_interface_default_update(struct mip_interface* device, mip_timeout wait ///@returns The amount of data which couldn't be processed due to the limit on /// number of packets per parse call. Normally the result is 0. /// -mip_remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, mip_timestamp timestamp) +size_t mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, mip_timestamp timestamp) { return mip_parser_parse(&device->_parser, data, length, timestamp, device->_max_update_pkts); } diff --git a/src/mip/mip_interface.h b/src/mip/mip_interface.h index 0a939a06a..bc14b0f29 100644 --- a/src/mip/mip_interface.h +++ b/src/mip/mip_interface.h @@ -70,7 +70,7 @@ bool mip_interface_recv_from_device(mip_interface* device, uint8_t* buffer, size bool mip_interface_update(mip_interface* device, mip_timeout wait_time); bool mip_interface_default_update(mip_interface* device, mip_timeout wait_time); -mip_remaining_count mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, mip_timestamp timestamp); +size_t mip_interface_receive_bytes(mip_interface* device, const uint8_t* data, size_t length, mip_timestamp timestamp); void mip_interface_process_unparsed_packets(mip_interface* device); bool mip_interface_parse_callback(void* device, const mip_packet* packet, mip_timestamp timestamp); void mip_interface_receive_packet(mip_interface* device, const mip_packet* packet, mip_timestamp timestamp); diff --git a/src/mip/mip_packet.c b/src/mip/mip_packet.c index a8eb55135..b4c70a8cb 100644 --- a/src/mip/mip_packet.c +++ b/src/mip/mip_packet.c @@ -42,7 +42,7 @@ void mip_packet_from_buffer(mip_packet* packet, uint8_t* buffer, size_t length) length = MIP_PACKET_LENGTH_MAX; packet->_buffer = buffer; - packet->_buffer_length = length; + packet->_buffer_length = (uint_least16_t)length; } //////////////////////////////////////////////////////////////////////////////// @@ -102,7 +102,7 @@ uint8_t mip_packet_payload_length(const mip_packet* packet) /// ///@returns The length of the packet. Always at least MIP_PACKET_LENGTH_MIN. /// -packet_length mip_packet_total_length(const mip_packet* packet) +uint_least16_t mip_packet_total_length(const mip_packet* packet) { return mip_packet_payload_length(packet) + MIP_PACKET_LENGTH_MIN; } @@ -139,7 +139,7 @@ const uint8_t* mip_packet_payload(const mip_packet* packet) /// uint16_t mip_packet_checksum_value(const mip_packet* packet) { - const packet_length index = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; + const uint_least16_t index = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; return ((uint16_t)(packet->_buffer[index+0]) << 8) | (uint16_t)(packet->_buffer[index+1]); } @@ -156,9 +156,9 @@ uint16_t mip_packet_compute_checksum(const mip_packet* packet) // mip_packet_total_length always returns at least MIP_PACKET_LENGTH_MIN so this // subtraction is guaranteed to be safe. - const packet_length length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; + const uint_least16_t length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; - for(packet_length i=0; i_buffer[i]; b += a; @@ -221,7 +221,7 @@ bool mip_packet_is_empty(const mip_packet* packet) /// ///@note This is the BUFFER SIZE and not the packet length. /// -packet_length mip_packet_buffer_size(const mip_packet* packet) +uint_least16_t mip_packet_buffer_size(const mip_packet* packet) { return packet->_buffer_length; } @@ -231,7 +231,11 @@ packet_length mip_packet_buffer_size(const mip_packet* packet) /// /// This is equal to the buffer size less the total packet length. /// -mip_remaining_count mip_packet_remaining_space(const mip_packet* packet) +///@caution The result may be negative if the packet length exceeds the actual +/// buffer capacity. Such packets are not 'sane' (mip_packet_is_sane) +/// and can only be produced by manipulating the buffered data directly. +/// +int mip_packet_remaining_space(const mip_packet* packet) { return mip_packet_buffer_size(packet) - mip_packet_total_length(packet); } @@ -285,7 +289,7 @@ bool mip_packet_is_data(const mip_packet* packet) bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length) { uint8_t* payload_buffer; - mip_remaining_count remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer); + int remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer); if( remaining < 0 ) return false; @@ -324,20 +328,20 @@ bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const ui /// is negative, the field could not be allocated and the payload must /// not be written. /// -mip_remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out) +int mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out) { assert(payload_ptr_out != NULL); - // assert( payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); + assert( payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); - const mip_remaining_count remaining = mip_packet_remaining_space(packet); + const int remaining = mip_packet_remaining_space(packet); - const packet_length field_length = MIP_FIELD_HEADER_LENGTH + (packet_length)payload_length; + const uint8_t field_length = MIP_FIELD_HEADER_LENGTH + payload_length; *payload_ptr_out = NULL; if( field_length <= remaining ) { - packet_length field_index = MIP_HEADER_LENGTH + mip_packet_payload_length(packet); + const uint_least16_t field_index = MIP_HEADER_LENGTH + mip_packet_payload_length(packet); packet->_buffer[MIP_INDEX_LENGTH] += field_length; @@ -371,7 +375,7 @@ mip_remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_des ///@returns The space remaining in the packet after changing the field size. /// This will be negative if the new length did not fit. /// -mip_remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length) +int mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length) { assert(payload_ptr != NULL); assert( new_payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); @@ -380,14 +384,14 @@ mip_remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* p const uint8_t old_field_length = field_ptr[MIP_INDEX_FIELD_LEN]; const uint8_t new_field_length = new_payload_length + MIP_FIELD_HEADER_LENGTH; - const mip_remaining_count delta_length = new_field_length - old_field_length; + const int delta_length = new_field_length - old_field_length; - const mip_remaining_count remaining = mip_packet_remaining_space(packet) - delta_length; + const int remaining = mip_packet_remaining_space(packet) - delta_length; if( remaining >= 0 ) { field_ptr[MIP_INDEX_FIELD_LEN] = new_field_length; - packet->_buffer[MIP_INDEX_LENGTH] += delta_length; + packet->_buffer[MIP_INDEX_LENGTH] += (int8_t)delta_length; } return remaining; @@ -407,7 +411,7 @@ mip_remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* p /// ///@returns The remaining space in the packet after removing the field. /// -mip_remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr) +int mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr) { assert(payload_ptr != NULL); @@ -439,7 +443,7 @@ mip_remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* pa void mip_packet_finalize(mip_packet* packet) { uint16_t checksum = mip_packet_compute_checksum(packet); - packet_length length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; + uint_least16_t length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH; packet->_buffer[length+0] = checksum >> 8; packet->_buffer[length+1] = checksum & 0xFF; diff --git a/src/mip/mip_packet.h b/src/mip/mip_packet.h index 907e970f5..e98287fcb 100644 --- a/src/mip/mip_packet.h +++ b/src/mip/mip_packet.h @@ -29,8 +29,6 @@ extern "C" { ///@{ -typedef uint_least16_t packet_length; ///< Type used for the length of a MIP packet. - //////////////////////////////////////////////////////////////////////////////// ///@brief Structure representing a MIP Packet. /// @@ -42,7 +40,7 @@ typedef uint_least16_t packet_length; ///< Type used for the length of a MIP pa /// typedef struct mip_packet { - uint8_t* _buffer; ///<@private Pointer to the packet data. + uint8_t* _buffer; ///<@private Pointer to the packet data. uint_least16_t _buffer_length; ///<@private Length of the buffer (NOT the packet length!). } mip_packet; @@ -59,10 +57,10 @@ typedef struct mip_packet void mip_packet_create(mip_packet* packet, uint8_t* buffer, size_t buffer_size, uint8_t descriptor_set); -bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length); -mip_remaining_count mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** payload_ptr_out); -mip_remaining_count mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length); -mip_remaining_count mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr); +bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length); +int mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** payload_ptr_out); +int mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length); +int mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr); void mip_packet_finalize(mip_packet* packet); @@ -85,7 +83,7 @@ void mip_packet_reset(mip_packet* packet, uint8_t descriptor_set); void mip_packet_from_buffer(mip_packet* packet, uint8_t* buffer, size_t length); uint8_t mip_packet_descriptor_set(const mip_packet* packet); -packet_length mip_packet_total_length(const mip_packet* packet); +uint_least16_t mip_packet_total_length(const mip_packet* packet); uint8_t mip_packet_payload_length(const mip_packet* packet); uint8_t* mip_packet_buffer(mip_packet* packet); const uint8_t* mip_packet_pointer(const mip_packet* packet); @@ -98,8 +96,8 @@ bool mip_packet_is_sane(const mip_packet* packet); bool mip_packet_is_valid(const mip_packet* packet); bool mip_packet_is_empty(const mip_packet* packet); -packet_length mip_packet_buffer_size(const mip_packet* packet); -mip_remaining_count mip_packet_remaining_space(const mip_packet* packet); +uint_least16_t mip_packet_buffer_size(const mip_packet* packet); +int mip_packet_remaining_space(const mip_packet* packet); bool mip_packet_is_data(const mip_packet* packet); diff --git a/src/mip/mip_parser.c b/src/mip/mip_parser.c index 1964db018..d6192347e 100644 --- a/src/mip/mip_parser.c +++ b/src/mip/mip_parser.c @@ -112,7 +112,7 @@ void mip_parser_reset(mip_parser* parser) /// conntains 0x75,0x65, has at least 6 bytes, and has a valid checksum. A /// 16-bit checksum has a 1 in 65,536 chance of appearing to be valid. /// -mip_remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, mip_timestamp timestamp, unsigned int max_packets) +size_t mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, mip_timestamp timestamp, unsigned int max_packets) { // Reset the state if the timeout time has elapsed. if( parser->_expected_length != MIPPARSER_RESET_LENGTH && (timestamp - parser->_start_time) > parser->_timeout ) @@ -148,11 +148,11 @@ mip_remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_bu if( stop ) { // Pull more data from the input buffer if possible. - size_t count = byte_ring_copy_from_and_update(&parser->_ring, &input_buffer, &input_count); + count = byte_ring_copy_from_and_update(&parser->_ring, &input_buffer, &input_count); MIP_DIAG_INC(parser->_diag_bytes_read, count); - return -(mip_remaining_count)input_count; + return input_count; } } @@ -162,7 +162,7 @@ mip_remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_bu } while( input_count ); - return -(mip_remaining_count)input_count; + return input_count; } //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/mip_parser.h b/src/mip/mip_parser.h index f938ac403..30e5cb386 100644 --- a/src/mip/mip_parser.h +++ b/src/mip/mip_parser.h @@ -57,7 +57,7 @@ typedef struct mip_parser mip_timestamp _start_time; ///<@private The timestamp when the first byte was observed by the parser. mip_timestamp _timeout; ///<@private Duration to wait for the rest of the data in a packet. uint8_t _result_buffer[MIP_PACKET_LENGTH_MAX]; ///<@private Buffer used to output MIP packets to the callback. - packet_length _expected_length; ///<@private Expected length of the packet currently being parsed. Keeps track of parser state. Always 1, MIP_HEADER_LENGTH, or at least MIP_PACKET_LENGTH_MAX. + uint16_t _expected_length; ///<@private Expected length of the packet currently being parsed. Keeps track of parser state. Always 1, MIP_HEADER_LENGTH, or at least MIP_PACKET_LENGTH_MAX. byte_ring_state _ring; ///<@private Ring buffer which holds data being parsed. User-specified backing buffer and size. mip_packet_callback _callback; ///<@private Callback called when a valid packet is parsed. Can be NULL. void* _callback_object; ///<@private User-specified pointer passed to the callback function. @@ -81,7 +81,7 @@ typedef struct mip_parser void mip_parser_init(mip_parser* parser, uint8_t* buffer, size_t buffer_size, mip_packet_callback callback, void* callback_object, mip_timestamp timeout); bool mip_parser_parse_one_packet_from_ring(mip_parser* parser, mip_packet* packet_out, mip_timestamp timestamp); -mip_remaining_count mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, mip_timestamp timestamp, unsigned int max_packets); +size_t mip_parser_parse(mip_parser* parser, const uint8_t* input_buffer, size_t input_count, mip_timestamp timestamp, unsigned int max_packets); void mip_parser_reset(mip_parser* parser); diff --git a/src/mip/mip_types.h b/src/mip/mip_types.h index 792a1f5dd..7ccb3e98a 100644 --- a/src/mip/mip_types.h +++ b/src/mip/mip_types.h @@ -13,7 +13,7 @@ extern "C" { // Used like a signed version of size_t -typedef int_least16_t mip_remaining_count; +typedef int mip_remaining_count; ///@brief Type used for packet timestamps and timeouts. diff --git a/src/mip/utils/serialization.c b/src/mip/utils/serialization.c index 1fa390a6a..96cef9bce 100644 --- a/src/mip/utils/serialization.c +++ b/src/mip/utils/serialization.c @@ -66,7 +66,7 @@ void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet* packe serializer->_buffer_size = 0; serializer->_offset = 0; - const mip_remaining_count length = mip_packet_alloc_field(packet, field_descriptor, 0, &serializer->_buffer); + const int length = mip_packet_alloc_field(packet, field_descriptor, 0, &serializer->_buffer); if( length >= 0 ) serializer->_buffer_size = length; @@ -76,7 +76,7 @@ void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet* packe ///@brief Call this after a new field allocated by mip_serializer_init_new_field /// has been written. /// -/// This will either finish the field, or abort it if the serializer overflowed. +/// This will either finish the field, or abort it if the serializer failed. /// ///@param serializer Must be created from mip_serializer_init_new_field. ///@param packet Must be the original packet. @@ -86,7 +86,10 @@ void mip_serializer_finish_new_field(const mip_serializer* serializer, mip_packe assert(packet); if( mip_serializer_is_ok(serializer) ) - mip_packet_realloc_last_field(packet, serializer->_buffer, serializer->_offset); + { + assert(serializer->_offset <= MIP_FIELD_LENGTH_MAX); // Payload too long! + mip_packet_realloc_last_field(packet, serializer->_buffer, (uint8_t) serializer->_offset); + } else if( serializer->_buffer ) mip_packet_cancel_last_field(packet, serializer->_buffer); } @@ -142,9 +145,9 @@ size_t mip_serializer_length(const mip_serializer* serializer) /// or read more data than contained in the buffer. This is not a bug and /// it can be detected with the mip_serializer_is_ok() function. /// -mip_remaining_count mip_serializer_remaining(const mip_serializer* serializer) +int mip_serializer_remaining(const mip_serializer* serializer) { - return mip_serializer_capacity(serializer) - mip_serializer_length(serializer); + return (int)(mip_serializer_capacity(serializer) - mip_serializer_length(serializer)); } //////////////////////////////////////////////////////////////////////////////// diff --git a/src/mip/utils/serialization.h b/src/mip/utils/serialization.h index 0970ee4f0..e0910a80e 100644 --- a/src/mip/utils/serialization.h +++ b/src/mip/utils/serialization.h @@ -55,9 +55,9 @@ void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet* packe void mip_serializer_finish_new_field(const mip_serializer* serializer, mip_packet* packet); void mip_serializer_init_from_field(mip_serializer* serializer, const mip_field* field); -size_t mip_serializer_capacity(const mip_serializer* serializer); -size_t mip_serializer_length(const mip_serializer* serializer); -mip_remaining_count mip_serializer_remaining(const mip_serializer* serializer); +size_t mip_serializer_capacity(const mip_serializer* serializer); +size_t mip_serializer_length(const mip_serializer* serializer); +int mip_serializer_remaining(const mip_serializer* serializer); bool mip_serializer_is_ok(const mip_serializer* serializer); bool mip_serializer_is_complete(const mip_serializer* serializer); @@ -154,7 +154,7 @@ class Serializer : public C::mip_serializer size_t capacity() const { return C::mip_serializer_capacity(this); } size_t length() const { return C::mip_serializer_length(this); } - RemainingCount remaining() const { return C::mip_serializer_remaining(this); } + int remaining() const { return C::mip_serializer_remaining(this); } bool isOk() const { return C::mip_serializer_is_ok(this); } bool isComplete() const { return C::mip_serializer_is_complete(this); } diff --git a/test/mip/mip_parser_performance.cpp b/test/mip/mip_parser_performance.cpp index cd6f57e2d..bb5e80a1a 100644 --- a/test/mip/mip_parser_performance.cpp +++ b/test/mip/mip_parser_performance.cpp @@ -166,7 +166,7 @@ ChunkStats chunked_test(const Test& test, size_t chunk_size) ChunkStats stats; stats.chunk_size = chunk_size; stats.num_calls = num_full_chunks; - stats.total_time = std::accumulate(chunk_times.begin(), chunk_times.end(), 0.0) / test.num_iterations; // Accumulate with double precision! + stats.total_time = (float)std::accumulate(chunk_times.begin(), chunk_times.end(), 0.0) / test.num_iterations; // Accumulate with double precision! stats.avg_time = stats.total_time / num_full_chunks; stats.max_time = *std::max_element(chunk_times.begin(), chunk_times.end()); diff --git a/test/mip/test_mip.cpp b/test/mip/test_mip.cpp index 2c08bcde4..6e9394744 100644 --- a/test/mip/test_mip.cpp +++ b/test/mip/test_mip.cpp @@ -86,7 +86,7 @@ int main(int argc, const char* argv[]) const uint8_t payloadLength = (rand() % MIP_FIELD_PAYLOAD_LENGTH_MAX) + 1; uint8_t* payload; - RemainingCount rem = packet.allocField(fieldDescriptor, payloadLength, &payload); + int rem = packet.allocField(fieldDescriptor, payloadLength, &payload); if( rem < 0 ) break; @@ -99,7 +99,7 @@ int main(int argc, const char* argv[]) packet.finalize(); - RemainingCount rem = parser.parse(packet.pointer(), packet.totalLength(), 0, MIPPARSER_UNLIMITED_PACKETS); + size_t rem = parser.parse(packet.pointer(), packet.totalLength(), 0, MIPPARSER_UNLIMITED_PACKETS); if( rem != 0 ) { diff --git a/test/mip/test_mip_random.c b/test/mip/test_mip_random.c index 8c2a05cb7..26bf944ad 100644 --- a/test/mip/test_mip_random.c +++ b/test/mip/test_mip_random.c @@ -71,7 +71,7 @@ int main(int argc, const char* argv[]) const uint8_t field_desc = (rand() % 255) + 1; // Random field descriptor. uint8_t* payload; - mip_remaining_count available = mip_packet_alloc_field(&packet, field_desc, paylen, &payload); + int available = mip_packet_alloc_field(&packet, field_desc, paylen, &payload); if( available < 0 ) { From 0157f1f6c562a419f2ff81fae84eea82319dc92c Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 25 Apr 2024 13:57:42 -0400 Subject: [PATCH 224/252] Fix printf warning. --- test/mip/test_mip.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mip/test_mip.cpp b/test/mip/test_mip.cpp index 6e9394744..84aeb0e40 100644 --- a/test/mip/test_mip.cpp +++ b/test/mip/test_mip.cpp @@ -104,7 +104,7 @@ int main(int argc, const char* argv[]) if( rem != 0 ) { numErrors++; - fprintf(stderr, "Parser reports %d unparsed bytes.\n", rem); + fprintf(stderr, "Parser reports %zu unparsed bytes.\n", rem); } if( numErrors > 10 ) From 026b0fd28707e76f26578b0b20c3156deae17aa2 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 26 Apr 2024 21:03:10 +0000 Subject: [PATCH 225/252] Generate MIP definitions from branches/dev@56343. --- src/mip/definitions/commands_aiding.c | 24 +++++----- src/mip/definitions/commands_aiding.h | 64 +++++++++++++------------ src/mip/definitions/commands_aiding.hpp | 64 +++++++++++++------------ src/mip/definitions/data_gnss.h | 15 +++--- src/mip/definitions/data_gnss.hpp | 15 +++--- 5 files changed, 94 insertions(+), 88 deletions(-) diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index af010defb..b6ff74621 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -77,12 +77,12 @@ void insert_mip_aiding_frame_config_command(mip_serializer* serializer, const mi if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { - insert_mip_vector3f(serializer, &self->rotation.euler); + insert_mip_vector3f(serializer, self->rotation.euler); } if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { - insert_mip_quatf(serializer, &self->rotation.quaternion); + insert_mip_quatf(serializer, self->rotation.quaternion); } } @@ -107,12 +107,12 @@ void extract_mip_aiding_frame_config_command(mip_serializer* serializer, mip_aid if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { - extract_mip_vector3f(serializer, &self->rotation.euler); + extract_mip_vector3f(serializer, self->rotation.euler); } if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { - extract_mip_quatf(serializer, &self->rotation.quaternion); + extract_mip_quatf(serializer, self->rotation.quaternion); } } @@ -131,12 +131,12 @@ void insert_mip_aiding_frame_config_response(mip_serializer* serializer, const m if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { - insert_mip_vector3f(serializer, &self->rotation.euler); + insert_mip_vector3f(serializer, self->rotation.euler); } if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { - insert_mip_quatf(serializer, &self->rotation.quaternion); + insert_mip_quatf(serializer, self->rotation.quaternion); } } @@ -153,12 +153,12 @@ void extract_mip_aiding_frame_config_response(mip_serializer* serializer, mip_ai if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { - extract_mip_vector3f(serializer, &self->rotation.euler); + extract_mip_vector3f(serializer, self->rotation.euler); } if( self->format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { - extract_mip_quatf(serializer, &self->rotation.quaternion); + extract_mip_quatf(serializer, self->rotation.quaternion); } } @@ -194,12 +194,12 @@ mip_cmd_result mip_aiding_write_frame_config(struct mip_interface* device, uint8 if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { - insert_mip_vector3f(&serializer, &rotation->euler); + insert_mip_vector3f(&serializer, rotation->euler); } if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { - insert_mip_quatf(&serializer, &rotation->quaternion); + insert_mip_quatf(&serializer, rotation->quaternion); } assert(mip_serializer_is_ok(&serializer)); @@ -241,12 +241,12 @@ mip_cmd_result mip_aiding_read_frame_config(struct mip_interface* device, uint8_ if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER ) { - extract_mip_vector3f(&deserializer, &rotation_out->euler); + extract_mip_vector3f(&deserializer, rotation_out->euler); } if( format == MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_QUATERNION ) { - extract_mip_quatf(&deserializer, &rotation_out->quaternion); + extract_mip_quatf(&deserializer, rotation_out->quaternion); } if( mip_serializer_remaining(&deserializer) != 0 ) diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index 95de33171..d2ef69567 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -101,6 +101,8 @@ void extract_mip_time_timebase(struct mip_serializer* serializer, mip_time_timeb /// Rotation can be defined using Euler angles OR quaternions. If Format selector is set to Euler Angles, the fourth element /// in the rotation vector is ignored and should be set to 0. /// +/// When the tracking_enabled flag is 1, the Kalman filter will track errors in the provided frame definition; when 0, no errors are tracked. +/// /// Example: GNSS antenna lever arm /// /// Frame ID: 1 @@ -125,9 +127,9 @@ typedef union mip_aiding_frame_config_command_rotation mip_aiding_frame_config_c struct mip_aiding_frame_config_command { mip_function_selector function; - uint8_t frame_id; ///< Reference frame number. Cannot be 0. + uint8_t frame_id; ///< Reference frame number. Limit 4. mip_aiding_frame_config_command_format format; ///< Format of the transformation. - bool tracking_enabled; ///< If enabled, the Kalman filter will track errors + bool tracking_enabled; ///< If enabled, the Kalman filter will track errors. mip_vector3f translation; ///< Translation X, Y, and Z. mip_aiding_frame_config_command_rotation rotation; ///< Rotation as specified by format. @@ -141,9 +143,9 @@ void extract_mip_aiding_frame_config_command_format(struct mip_serializer* seria struct mip_aiding_frame_config_response { - uint8_t frame_id; ///< Reference frame number. Cannot be 0. + uint8_t frame_id; ///< Reference frame number. Limit 4. mip_aiding_frame_config_command_format format; ///< Format of the transformation. - bool tracking_enabled; ///< If enabled, the Kalman filter will track errors + bool tracking_enabled; ///< If enabled, the Kalman filter will track errors. mip_vector3f translation; ///< Translation X, Y, and Z. mip_aiding_frame_config_command_rotation rotation; ///< Rotation as specified by format. @@ -217,10 +219,10 @@ static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND struct mip_aiding_ecef_pos_command { mip_time time; ///< Timestamp of the measurement. - uint8_t frame_id; ///< Sensor ID. + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3d position; ///< ECEF position [m]. - mip_vector3f uncertainty; ///< ECEF position uncertainty [m]. - mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. + mip_vector3f uncertainty; ///< ECEF position uncertainty [m]. Cannot be 0 unless the corresponding valid flags are 0. + mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; typedef struct mip_aiding_ecef_pos_command mip_aiding_ecef_pos_command; @@ -251,12 +253,12 @@ static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_V struct mip_aiding_llh_pos_command { mip_time time; ///< Timestamp of the measurement. - uint8_t frame_id; ///< Sensor ID. + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). double latitude; ///< [deg] double longitude; ///< [deg] double height; ///< [m] - mip_vector3f uncertainty; ///< NED position uncertainty. - mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. + mip_vector3f uncertainty; ///< NED position uncertainty. Cannot be 0 unless the corresponding valid flags are 0. + mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; typedef struct mip_aiding_llh_pos_command mip_aiding_llh_pos_command; @@ -278,8 +280,8 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* struct mip_aiding_height_command { - mip_time time; - uint8_t frame_id; + mip_time time; ///< Timestamp of the measurement. + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). float height; ///< [m] float uncertainty; ///< [m] uint16_t valid_flags; @@ -309,10 +311,10 @@ static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND struct mip_aiding_ecef_vel_command { mip_time time; ///< Timestamp of the measurement. - uint8_t frame_id; ///< Sensor ID. + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3f velocity; ///< ECEF velocity [m/s]. - mip_vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. - mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. + mip_vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. Cannot be 0 unless the corresponding valid flags are 0. + mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; typedef struct mip_aiding_ecef_vel_command mip_aiding_ecef_vel_command; @@ -342,10 +344,10 @@ static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_V struct mip_aiding_ned_vel_command { mip_time time; ///< Timestamp of the measurement. - uint8_t frame_id; ///< Sensor ID. + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3f velocity; ///< NED velocity [m/s]. - mip_vector3f uncertainty; ///< NED velocity uncertainty [m/s]. - mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. + mip_vector3f uncertainty; ///< NED velocity uncertainty [m/s]. Cannot be 0 unless the corresponding valid flags are 0. + mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; typedef struct mip_aiding_ned_vel_command mip_aiding_ned_vel_command; @@ -376,10 +378,10 @@ static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AID struct mip_aiding_vehicle_fixed_frame_velocity_command { mip_time time; ///< Timestamp of the measurement. - uint8_t frame_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3f velocity; ///< [m/s] - mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; + mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty. Cannot be 0 unless the corresponding valid flags are 0. + mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; typedef struct mip_aiding_vehicle_fixed_frame_velocity_command mip_aiding_vehicle_fixed_frame_velocity_command; @@ -400,10 +402,10 @@ mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* dev struct mip_aiding_true_heading_command { - mip_time time; - uint8_t frame_id; - float heading; ///< Heading in [radians] - float uncertainty; + mip_time time; ///< Timestamp of the measurement. + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). + float heading; ///< Heading [radians]. Range +/- Pi. + float uncertainty; ///< Cannot be 0 unless the valid flags are 0. uint16_t valid_flags; }; @@ -431,10 +433,10 @@ static const mip_aiding_magnetic_field_command_valid_flags MIP_AIDING_MAGNETIC_F struct mip_aiding_magnetic_field_command { mip_time time; ///< Timestamp of the measurement. - uint8_t frame_id; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3f magnetic_field; ///< [G] - mip_vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) - mip_aiding_magnetic_field_command_valid_flags valid_flags; + mip_vector3f uncertainty; ///< [G] 1-sigma uncertainty. Cannot be 0 unless the corresponding valid flags are 0. + mip_aiding_magnetic_field_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; typedef struct mip_aiding_magnetic_field_command mip_aiding_magnetic_field_command; @@ -456,10 +458,10 @@ mip_cmd_result mip_aiding_magnetic_field(struct mip_interface* device, const mip struct mip_aiding_pressure_command { - mip_time time; - uint8_t frame_id; + mip_time time; ///< Timestamp of the measurement. + uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). float pressure; ///< [mbar] - float uncertainty; ///< [mbar] + float uncertainty; ///< [mbar] 1-sigma uncertainty. Cannot be 0 unless the valid flags are 0. uint16_t valid_flags; }; diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 10905c398..85ef6e274 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -98,6 +98,8 @@ void extract(Serializer& serializer, Time& self); /// Rotation can be defined using Euler angles OR quaternions. If Format selector is set to Euler Angles, the fourth element /// in the rotation vector is ignored and should be set to 0. /// +/// When the tracking_enabled flag is 1, the Kalman filter will track errors in the provided frame definition; when 0, no errors are tracked. +/// /// Example: GNSS antenna lever arm /// /// Frame ID: 1 @@ -124,9 +126,9 @@ struct FrameConfig }; FunctionSelector function = static_cast(0); - uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. + uint8_t frame_id = 0; ///< Reference frame number. Limit 4. Format format = static_cast(0); ///< Format of the transformation. - bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors + bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors. Vector3f translation; ///< Translation X, Y, and Z. Rotation rotation; ///< Rotation as specified by format. @@ -173,9 +175,9 @@ struct FrameConfig static constexpr const uint32_t ECHOED_PARAMS = 0x0003; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; - uint8_t frame_id = 0; ///< Reference frame number. Cannot be 0. + uint8_t frame_id = 0; ///< Reference frame number. Limit 4. Format format = static_cast(0); ///< Format of the transformation. - bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors + bool tracking_enabled = 0; ///< If enabled, the Kalman filter will track errors. Vector3f translation; ///< Translation X, Y, and Z. Rotation rotation; ///< Rotation as specified by format. @@ -323,10 +325,10 @@ struct EcefPos }; Time time; ///< Timestamp of the measurement. - uint8_t frame_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). Vector3d position; ///< ECEF position [m]. - Vector3f uncertainty; ///< ECEF position uncertainty [m]. - ValidFlags valid_flags; ///< Valid flags. + Vector3f uncertainty; ///< ECEF position uncertainty [m]. Cannot be 0 unless the corresponding valid flags are 0. + ValidFlags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; @@ -396,12 +398,12 @@ struct LlhPos }; Time time; ///< Timestamp of the measurement. - uint8_t frame_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). double latitude = 0; ///< [deg] double longitude = 0; ///< [deg] double height = 0; ///< [m] - Vector3f uncertainty; ///< NED position uncertainty. - ValidFlags valid_flags; ///< Valid flags. + Vector3f uncertainty; ///< NED position uncertainty. Cannot be 0 unless the corresponding valid flags are 0. + ValidFlags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; @@ -438,8 +440,8 @@ TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t f struct Height { - Time time; - uint8_t frame_id = 0; + Time time; ///< Timestamp of the measurement. + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). float height = 0; ///< [m] float uncertainty = 0; ///< [m] uint16_t valid_flags = 0; @@ -511,10 +513,10 @@ struct EcefVel }; Time time; ///< Timestamp of the measurement. - uint8_t frame_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). Vector3f velocity; ///< ECEF velocity [m/s]. - Vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. - ValidFlags valid_flags; ///< Valid flags. + Vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. Cannot be 0 unless the corresponding valid flags are 0. + ValidFlags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; @@ -583,10 +585,10 @@ struct NedVel }; Time time; ///< Timestamp of the measurement. - uint8_t frame_id = 0; ///< Sensor ID. + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). Vector3f velocity; ///< NED velocity [m/s]. - Vector3f uncertainty; ///< NED velocity uncertainty [m/s]. - ValidFlags valid_flags; ///< Valid flags. + Vector3f uncertainty; ///< NED velocity uncertainty [m/s]. Cannot be 0 unless the corresponding valid flags are 0. + ValidFlags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; @@ -656,10 +658,10 @@ struct VehicleFixedFrameVelocity }; Time time; ///< Timestamp of the measurement. - uint8_t frame_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). Vector3f velocity; ///< [m/s] - Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty (if uncertainty[i] <= 0, then velocity[i] should be treated as invalid and ingnored) - ValidFlags valid_flags; + Vector3f uncertainty; ///< [m/s] 1-sigma uncertainty. Cannot be 0 unless the corresponding valid flags are 0. + ValidFlags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; @@ -695,10 +697,10 @@ TypedResult vehicleFixedFrameVelocity(C::mip_interfac struct TrueHeading { - Time time; - uint8_t frame_id = 0; - float heading = 0; ///< Heading in [radians] - float uncertainty = 0; + Time time; ///< Timestamp of the measurement. + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). + float heading = 0; ///< Heading [radians]. Range +/- Pi. + float uncertainty = 0; ///< Cannot be 0 unless the valid flags are 0. uint16_t valid_flags = 0; static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; @@ -768,10 +770,10 @@ struct MagneticField }; Time time; ///< Timestamp of the measurement. - uint8_t frame_id = 0; ///< Source ID for this estimate ( source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate ) + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). Vector3f magnetic_field; ///< [G] - Vector3f uncertainty; ///< [G] 1-sigma uncertainty (if uncertainty[i] <= 0, then magnetic_field[i] should be treated as invalid and ingnored) - ValidFlags valid_flags; + Vector3f uncertainty; ///< [G] 1-sigma uncertainty. Cannot be 0 unless the corresponding valid flags are 0. + ValidFlags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_MAGNETIC_FIELD; @@ -808,10 +810,10 @@ TypedResult magneticField(C::mip_interface& device, const Time& t struct Pressure { - Time time; - uint8_t frame_id = 0; + Time time; ///< Timestamp of the measurement. + uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). float pressure = 0; ///< [mbar] - float uncertainty = 0; ///< [mbar] + float uncertainty = 0; ///< [mbar] 1-sigma uncertainty. Cannot be 0 unless the valid flags are 0. uint16_t valid_flags = 0; static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; diff --git a/src/mip/definitions/data_gnss.h b/src/mip/definitions/data_gnss.h index 60b9a6035..c2bb9a258 100644 --- a/src/mip/definitions/data_gnss.h +++ b/src/mip/definitions/data_gnss.h @@ -459,13 +459,14 @@ void extract_mip_gnss_clock_info_data_valid_flags(struct mip_serializer* seriali ///@{ typedef uint8_t mip_gnss_fix_info_data_fix_type; -static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_3D = 0; ///< -static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_2D = 1; ///< -static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_TIME_ONLY = 2; ///< -static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_NONE = 3; ///< -static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_INVALID = 4; ///< -static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_RTK_FLOAT = 5; ///< -static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_RTK_FIXED = 6; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_3D = 0; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_2D = 1; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_TIME_ONLY = 2; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_NONE = 3; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_INVALID = 4; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_RTK_FLOAT = 5; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_RTK_FIXED = 6; ///< +static const mip_gnss_fix_info_data_fix_type MIP_GNSS_FIX_INFO_DATA_FIX_TYPE_FIX_DIFFERENTIAL = 7; ///< typedef uint16_t mip_gnss_fix_info_data_fix_flags; static const mip_gnss_fix_info_data_fix_flags MIP_GNSS_FIX_INFO_DATA_FIX_FLAGS_NONE = 0x0000; diff --git a/src/mip/definitions/data_gnss.hpp b/src/mip/definitions/data_gnss.hpp index 5a59b0704..70b783529 100644 --- a/src/mip/definitions/data_gnss.hpp +++ b/src/mip/definitions/data_gnss.hpp @@ -764,13 +764,14 @@ struct FixInfo { enum class FixType : uint8_t { - FIX_3D = 0, ///< - FIX_2D = 1, ///< - FIX_TIME_ONLY = 2, ///< - FIX_NONE = 3, ///< - FIX_INVALID = 4, ///< - FIX_RTK_FLOAT = 5, ///< - FIX_RTK_FIXED = 6, ///< + FIX_3D = 0, ///< + FIX_2D = 1, ///< + FIX_TIME_ONLY = 2, ///< + FIX_NONE = 3, ///< + FIX_INVALID = 4, ///< + FIX_RTK_FLOAT = 5, ///< + FIX_RTK_FIXED = 6, ///< + FIX_DIFFERENTIAL = 7, ///< }; struct FixFlags : Bitfield From 17862fb075e2c54b1b7f61d58b7c0760b4cbf1ed Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Fri, 26 Apr 2024 17:26:35 -0400 Subject: [PATCH 226/252] Fix int conversion warning. --- src/mip/mip_result.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mip/mip_result.h b/src/mip/mip_result.h index 8ffca0748..429fc6356 100644 --- a/src/mip/mip_result.h +++ b/src/mip/mip_result.h @@ -98,7 +98,7 @@ struct CmdResult CmdResult& operator=(const CmdResult& other) = default; CmdResult& operator=(C::mip_cmd_result other) { value = other; return *this; } - static constexpr CmdResult userResult(uint32_t n) { return CmdResult(static_cast(STATUS_USER - n)); } + static constexpr CmdResult userResult(uint8_t n) { return CmdResult(static_cast(STATUS_USER - int8_t(n))); } static constexpr CmdResult fromAckNack(uint8_t code) { return CmdResult(static_cast(code)); } operator const void*() const { return isAck() ? this : nullptr; } From 4a3b25906fd43253422d78f1920447fb8ac8996c Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 29 Apr 2024 15:54:29 -0400 Subject: [PATCH 227/252] - Update documentation. --- docs/docs.c | 70 ++++++++++++--------- src/mip/definitions/common.h | 2 +- src/mip/mip_cmdqueue.c | 2 - src/mip/mip_interface.c | 112 +++++++++++++++++++++++++++------- src/mip/mip_interface.h | 2 +- src/mip/mip_packet.c | 2 +- src/mip/mip_parser.h | 2 +- src/mip/mip_result.h | 12 +++- src/mip/utils/serialization.h | 5 ++ 9 files changed, 152 insertions(+), 57 deletions(-) diff --git a/docs/docs.c b/docs/docs.c index eb80cd4a4..cef1ea16e 100644 --- a/docs/docs.c +++ b/docs/docs.c @@ -1,8 +1,10 @@ //////////////////////////////////////////////////////////////////////////////// ///@mainpage MIP SDK /// -/// Welcome to the official MIP Software Development Kit. -/// +/// Welcome to the official MIP Software Development Kit. This software package +/// provides everything you need to communicate with any MIP-compatible +/// MicroStrain inertial sensor. +/// See @ref mip_interface for details on how to get started. /// ///@par Main Features /// @@ -15,17 +17,17 @@ ///@li Dual C and C++ API for maximum usability, safety, flexibility, and convenience. ///@li Suitable for bare-metal microcontrollers (Minimal code size and memory footprint, No dynamic memory allocation, No dependence on any RTOS or threading) /// -/// See @ref mip_interface for details on how to get started. -/// ///@section quickref_cpp Quick Reference [C++] /// /// All C++ functions and classes reside within the mip namespace. /// The C functions can be accessed via the mip::C namespace. /// ///@li @ref mip::DeviceInterface Top-level MIP interface class. -///@li @ref mip::Packet A class representing a MIP packet for either transmission or reception. -///@li @ref mip::Field A class representing a MIP field within a packet. +///@li @ref mip::PacketRef An interface to a MIP packet for either transmission or reception. +///@li @ref mip::PacketBuf Similar to PacketRef but includes the data buffer. +///@li @ref mip::Field An interface to a MIP field within a packet. ///@li @ref mip::Parser MIP parser class for converting received bytes into packets. +///@li @ref mip::CmdResult Stores the status or result of a MIP command. /// ///@section quickref_c Quick Reference [C] /// @@ -38,12 +40,16 @@ ///@li @ref mip_packet_c ///@li @ref mip_field_c ///@li @ref mip_parser_c +///@li @ref mip::C::mip_cmd_result /// /// //////////////////////////////////////////////////////////////////////////////// ///@page mip_interface Mip Interface //////////////////////////////////////////////////////////////////////////////// /// +//////////////////////////////////////////////////////////////////////////////// +///@section mip_interface_interface Application Interface +/// /// The MIP interface is a high-level abstraction of a physical device which /// communicates using the MIP protocol. It provides both data callbacks and /// command functions for controlling and configuring the device: @@ -57,17 +63,20 @@ /// will return a status code. /// /// Sending and receiving to or from the device occurs via two functions: -///@li mip_interface_user_send_to_device() for transmission and -///@li mip_interface_user_recv_from_device() for reception. +///@li mip::DeviceInterface::sendToDevice() or +/// mip_interface_send_to_device() for transmission and +///@li mip::DeviceInterface::recvFromDevice() or +/// mip_interface_recv_from_device() for reception. /// -/// The application must define these two C functions, or subclass -/// mip::DeviceInterface and implement the pure virtual functions. -/// This should be straightforward; simply pass the bytes between the MIP -/// interface and the connection. +/// Each of these has a corresponding callback to the application. The +/// application is expected to implement the required behavior for each as +/// described below. Additionally, there is an @ref update "update function", +/// which handles periodic tasks such as command timeouts and streaming data +/// collection. An application may optionally override the update callback. /// -/// Because the device transmits continuously when streaming data, the -/// application must poll the connection for new data frequently. This is -/// done via the @ref update "update function". +///@li @ref mip::C::mip_send_callback "mip_send_callback" +///@li @ref mip::C::mip_recv_callback "mip_recv_callback" +///@li @ref mip::C::mip_update_callback "mip_update_callback" /// /// An application obtains sensor data via the /// @ref mip_dispatch "dispatch subsystem". There are 3 ways to do so: @@ -83,14 +92,19 @@ ///@section mip_commands Sending Commands /// /// Typically an application will configure the device, initialize some -/// settings, and start streaming. To do so, it must send commands. In most -/// cases, applications will call a single function for each needed command. +/// settings, and start streaming. To do so, it must send commands. In many +/// cases, applications will call a single function for each needed command +/// (e.g. @ref MipCommands_c / @ref MipCommands_cpp). /// These functions take the command parameters as arguments, send the packet, -/// wait for the reply, and return a result code. When reading from the device, -/// these commands will also report the device response information, assuming -/// the command was successful. The command functions are blocking, that is, -/// they halt execution until the device responds or the command times out. +/// wait for the reply, and return a result code. Additionally some commands can +/// report back responses from the device. /// +/// The command functions are blocking, that is, they halt execution until the +/// device responds or the command times out. +/// +/// Note that since MIP data is received serially and is not buffered, data may +/// be received and processed while waiting for command responses. It is +/// recommended (but not required) to set the device to idle during configuration. /// //////////////////////////////////////////////////////////////////////////////// ///@section mip_dispatch The Dispatch System @@ -118,7 +132,7 @@ ///@par Packet callbacks /// ///@code{.cpp} -/// void packet_callback(void* userdata, const Packet& packet, Timestamp parseTime) +/// void packet_callback(void* userdata, const mip::PacketRef& packet, Timestamp parseTime) ///@endcode /// /// Packet callbacks are invoked when a packet is received which matches the @@ -135,7 +149,7 @@ ///@par Field callbacks /// ///@code{.cpp} -/// void field_callback(void* userdata, const Field& field, Timestamp parseTime) +/// void field_callback(void* userdata, const mip::Field& field, Timestamp parseTime) ///@endcode /// /// Similar to packet callbacks, field callbacks are invoked when a MIP @@ -153,7 +167,7 @@ ///@par Data callbacks /// ///@code{.cpp} -/// void data_callback(void* userdata, const data_sensor::ScaledAccel& packet, Timestamp parseTime) +/// void data_callback(void* userdata, const mip::data_sensor::ScaledAccel& packet, Timestamp parseTime) ///@endcode /// /// Thanks to the power of templates, one additional dispatch mechanism is @@ -161,6 +175,7 @@ /// callback except that instead of getting the raw MIP field data, the function /// is passed the fully-deserialized data structure. /// +/// /// Typically an application will register a series of data or field callbacks /// and write the data to some kind of data structure. Because the order of /// these callbacks depends on the device configuration, it can be difficult @@ -182,7 +197,7 @@ /// a high enough rate to avoid overflowing the connection buffers. The /// precision of the reception timestamp is dependent on the update rate. /// -/// The command functions in @ref MipCommands_c / @ref MipCommands_cpp (e.g. mip_write_message_format() / mip::writeMessageFormat()) +/// The command functions in @ref MipCommands_c / @ref MipCommands_cpp (e.g. mip::C::mip_write_message_format() / mip::writeMessageFormat()) /// will block execution until the command completes. Either the device will /// respond with an ack/nack code, or the command will time out. During this /// time, the system must be able to receive data from the device in order for @@ -254,10 +269,9 @@ /// ///@par Other thread-safety concerns /// -///@li Data transmission to the device (for sending commands) is thread-safe +///@li Data transmission to the device (but not sending commands) is thread-safe /// within the MIP SDK. If multiple threads will send to the device, the -/// application should ensure that mip_interface_user_send_to_device() is -/// thread-safe (e.g. by using a mutex). +/// application should ensure that the device interface is properly protected. /// ///@li It is up to the application to ensure that sending and receiving from /// separate threads is safe. This is true for the built-in serial and TCP diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index f8a55e958..869557c7c 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -115,7 +115,7 @@ struct Vector /// Construct from individual elements or a braced init list. ///@param u The first value (typically X). ///@param v The value value (typically Y). - ///@param reset Additional optional values (typically none, Z, or Z and W). + ///@param rest Additional optional values (typically none, Z, or Z and W). template Vector(U u, V v, Rest... rest) : m_data{u, v, rest...} {} diff --git a/src/mip/mip_cmdqueue.c b/src/mip/mip_cmdqueue.c index 085793ea2..24874891d 100644 --- a/src/mip/mip_cmdqueue.c +++ b/src/mip/mip_cmdqueue.c @@ -57,8 +57,6 @@ void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_ ///@param response_buffer /// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. ///@param response_buffer_size -/// Optional buffer to hold response data, if any. If NULL, response_buffer_size must be 0. -///@param response_buffer_size /// Size of the response buffer. The response will be limited to this size. /// void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size) diff --git a/src/mip/mip_interface.c b/src/mip/mip_interface.c index 9870ba06d..586b7689a 100644 --- a/src/mip/mip_interface.c +++ b/src/mip/mip_interface.c @@ -10,31 +10,73 @@ #include //////////////////////////////////////////////////////////////////////////////// -///@typedef mip_send_callback +///@typedef mip::C::mip_send_callback /// -///@copydoc mip_interface_send_to_device +///@brief Called from mip_interface_send_to_device() to send data to the device port. +/// The application should forward the data to the device port (e.g. a +/// serial port, TCP connection, etc). /// -///@note There are cases where the data will not be a MIP packet. +/// Applications should avoid introducing significant transmission delays as it +/// may cause excessive command response times or timeouts. +/// +///@param device +/// A pointer to the device interface. Applications can use the user data +/// pointer to access additional information such as the port handle. +///@param data +/// Buffer containing the data to be transmitted to the device. +///@param length +/// Length of data to transmit. +/// +///@return True if all of the data was successfully transmitted. +///@return False if an error occurred and some or all data was definitely unable +/// to be transmitted. +///@return Applications should prefer returning true if success is uncertain +/// since command timeouts will help detect failed transmissions. If this +/// function returns false, the associated command will fail with +/// CmdResult::STATUS_ERROR. +/// +///@note +/// +///@note The data buffer is almost always a MIP packet. However, there are some +/// cases where this is not true and an application should not rely on it. +/// +///@see mip_interface_send_to_device /// //////////////////////////////////////////////////////////////////////////////// -///@typedef mip_recv_callback +///@typedef mip::C::mip_recv_callback /// -///@brief Receives new data from the device. Called repeatedly -/// by mip_interface_update() while waiting for command responses. +///@brief Called from mip_interface_recv_from_device() to receive data from the +/// device port. +/// +/// This is called indirectly through mip_interface_update() to poll for new +/// data and command responses. For single-threaded applications, it will be +/// called while waiting for command replies. +/// +/// +///@param device +/// A pointer to the device interface. Applications can use the user data +/// pointer to access additional information such as the port handle. +/// +///@param buffer +/// Buffer to fill with data. Should be allocated before this function is called. +/// +///@param max_length +/// Max number of bytes that can be read into the buffer. +/// +///@param wait_time +/// Time to wait for data from the device. The actual time waited may +/// be less than wait_time, but it should not significantly exceed this value. /// -///@param device The mip interface object -///@param buffer Buffer to fill with data. Should be allocated before -/// calling this function -///@param max_length Max number of bytes that can be read into the buffer. -///@param wait_time Time to wait for data from the device. The actual time -/// waited may be less than wait_time, but it should not -/// significantly exceed this value. -///@param out_length Number of bytes actually read into the buffer. -///@param timestamp_out Timestamp of the data was received. +///@param[out] length_out +/// Number of bytes actually read into the buffer. +/// +///@param[out] timestamp_out +/// Timestamp the data was received. /// ///@returns True if successful, even if no data is received. -///@returns False if the port cannot be read or some other error occurs. +///@returns False if the port cannot be read or some other error occurs (e.g. +/// if the port is closed). /// ///@note Except in case of error (i.e. returning false), the timestamp must be /// set even if no data is received. This is required to allow commands @@ -48,9 +90,11 @@ /// If the actual wait time exceeds wait_time, command timeouts may take /// longer than intended. /// +///@see mip_interface_recv_from_device +/// //////////////////////////////////////////////////////////////////////////////// -///@typedef mip_update_callback +///@typedef mip::C::mip_update_callback /// ///@brief Callback function typedef for custom update behavior. /// @@ -87,6 +131,15 @@ /// Maximum length of time to wait for the end of a MIP packet. See mip_parser_init(). ///@param base_reply_timeout /// Minimum time for all commands. See mip_cmd_queue_init(). +///@param send +/// A callback which is called to send data to the device. +///@param recv +/// A callback which is called when data needs to be read from the device. +///@param update +/// Optional callback which is called to perform routine tasks such as +/// checking for command timeouts. Defaults to mip_interface_default_update. +///@param user_pointer +/// Optional pointer which is passed to the send, recv, and update callbacks. /// void mip_interface_init( mip_interface* device, uint8_t* parse_buffer, size_t parse_buffer_size, @@ -120,7 +173,7 @@ void mip_interface_init( /// ///@param device /// -///@param function +///@param callback /// Function which sends raw bytes to the device. This can be NULL if no /// commands will be issued (they would fail). /// @@ -147,7 +200,7 @@ mip_send_callback mip_interface_send_function(const mip_interface* device) /// ///@param device /// -///@param function +///@param callback /// Function which gets data from the device connection. /// If this is NULL then commands will fail and no data will be received. /// @@ -179,7 +232,7 @@ mip_recv_callback mip_interface_recv_function(const mip_interface* device) /// ///@param device /// -///@param function +///@param callback /// Update function to call when polling the device for data. /// If this is NULL, then update calls will fail and no data or /// or command replies will be received. @@ -518,11 +571,28 @@ enum mip_cmd_result mip_interface_run_command(mip_interface* device, uint8_t des } //////////////////////////////////////////////////////////////////////////////// -///@copydoc mip_interface_run_command +///@brief Runs a command using a pre-serialized payload. /// +///@param device +///@param descriptor_set +/// Command descriptor set. +///@param cmd_descriptor +/// Command field descriptor. +///@param cmd_data +/// Optional payload data. May be NULL if cmd_length == 0. +///@param cmd_length +/// Length of the command payload (parameters). ///@param response_descriptor /// Descriptor of the response data. May be MIP_INVALID_FIELD_DESCRIPTOR /// if no response is expected. +///@param response_buffer +/// Buffer to hold response data. Can be the same as the command data buffer. +/// Can be NULL if response_descriptor is MIP_INVALID_FIELD_DESCRIPTOR. +///@param[in,out] response_length_inout +/// As input, the size of response buffer and max response length. +/// As output, returns the actual length of the response data. +/// +///@returns mip_cmd_result /// enum mip_cmd_result mip_interface_run_command_with_response(mip_interface* device, uint8_t descriptor_set, uint8_t cmd_descriptor, const uint8_t* cmd_data, uint8_t cmd_length, diff --git a/src/mip/mip_interface.h b/src/mip/mip_interface.h index bc14b0f29..c3d2868ed 100644 --- a/src/mip/mip_interface.h +++ b/src/mip/mip_interface.h @@ -34,7 +34,7 @@ struct mip_interface; // Documentation is in source file. typedef bool (*mip_send_callback)(struct mip_interface* device, const uint8_t* data, size_t length); -typedef bool (*mip_recv_callback)(struct mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out); +typedef bool (*mip_recv_callback)(struct mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* length_out, mip_timestamp* timestamp_out); typedef bool (*mip_update_callback)(struct mip_interface* device, mip_timeout timeout); diff --git a/src/mip/mip_packet.c b/src/mip/mip_packet.c index b4c70a8cb..d5f6496af 100644 --- a/src/mip/mip_packet.c +++ b/src/mip/mip_packet.c @@ -231,7 +231,7 @@ uint_least16_t mip_packet_buffer_size(const mip_packet* packet) /// /// This is equal to the buffer size less the total packet length. /// -///@caution The result may be negative if the packet length exceeds the actual +///@warning The result may be negative if the packet length exceeds the actual /// buffer capacity. Such packets are not 'sane' (mip_packet_is_sane) /// and can only be produced by manipulating the buffered data directly. /// diff --git a/src/mip/mip_parser.h b/src/mip/mip_parser.h index 30e5cb386..059b38043 100644 --- a/src/mip/mip_parser.h +++ b/src/mip/mip_parser.h @@ -40,7 +40,7 @@ extern "C" { ///@brief Callback function which receives parsed MIP packets. ///@param user A user-specified pointer which will be given the callback_object parameter which was previously passed to mip_parser_init. -///@param Packet A pointer to the MIP packet. Do not store this pointer as it will be invalidated after the callback returns. +///@param packet A pointer to the MIP packet. Do not store this pointer as it will be invalidated after the callback returns. ///@param timestamp The approximate time the packet was parsed. typedef bool (*mip_packet_callback)(void* user, const mip_packet* packet, mip_timestamp timestamp); diff --git a/src/mip/mip_result.h b/src/mip/mip_result.h index 429fc6356..590c7bff3 100644 --- a/src/mip/mip_result.h +++ b/src/mip/mip_result.h @@ -19,6 +19,8 @@ extern "C" { /// /// Values that start with MIP_STATUS are status codes from this library. /// Values that start with MIP_(N)ACK represent replies from the device. +/// Values at or below MIP_STATUS_USER_START (negative values) are reserved for +/// status codes from user code. /// typedef enum mip_cmd_result { @@ -58,14 +60,20 @@ bool mip_cmd_result_is_ack(enum mip_cmd_result result); } // extern "C" } // namespace C +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_cpp +///@{ + //////////////////////////////////////////////////////////////////////////////// ///@brief Represents the status of a MIP command. /// -/// This is the same as the mip_cmd_result C enum, but with member functions -/// and some operator overloads. +/// This is the same as the mip_cmd_result enum, but with some convenient +// member functions and some operator overloads. /// /// CmdResult is convertible to bool, allowing code like the following: ///@code{.cpp} +/// if( !mip::commands_base::ping(device) ) +/// fprintf(stderr, "Couldn't ping the device!\n"); ///@endcode /// struct CmdResult diff --git a/src/mip/utils/serialization.h b/src/mip/utils/serialization.h index e0910a80e..d0b03f6b0 100644 --- a/src/mip/utils/serialization.h +++ b/src/mip/utils/serialization.h @@ -105,6 +105,10 @@ void extract_count(mip_serializer* serializer, uint8_t* count_out, uint8_t max_c } // extern "C" } // namespace C +//////////////////////////////////////////////////////////////////////////////// +///@addtogroup mip_cpp +///@{ + //////////////////////////////////////////////////////////////////////////////// ///@addtogroup mip_serialization_cpp /// @@ -199,6 +203,7 @@ typename std::enable_if< std::is_enum::value, void>::type ///@param value Value to insert. ///@param buffer Buffer to udpate with the value. ///@param bufferSize Size of the buffer. +///@param offset Starting offset into the buffer. /// ///@returns true if sufficient space was available, false otherwise. /// From 823c986f36565137e1ddcdb22122ef11aa8feded Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 1 May 2024 14:55:37 -0400 Subject: [PATCH 228/252] Remove packet_length and remaining_count typedefs. --- src/mip/mip_types.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/mip/mip_types.h b/src/mip/mip_types.h index 7ccb3e98a..3fb409544 100644 --- a/src/mip/mip_types.h +++ b/src/mip/mip_types.h @@ -12,10 +12,6 @@ extern "C" { -// Used like a signed version of size_t -typedef int mip_remaining_count; - - ///@brief Type used for packet timestamps and timeouts. /// /// Timestamps must be monotonic except for overflow at the maximum value back to 0. @@ -57,7 +53,6 @@ typedef mip_timestamp mip_timeout; } // extern "C" } // namespace C -using RemainingCount = C::mip_remaining_count; using Timestamp = C::mip_timestamp; using Timeout = C::mip_timeout; From 2cf6f79137247835cfcf9dc25b3deb927aed4f07 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 1 May 2024 14:55:54 -0400 Subject: [PATCH 229/252] Clean up commented out code. --- src/mip/definitions/common.h | 170 ----------------------------------- 1 file changed, 170 deletions(-) diff --git a/src/mip/definitions/common.h b/src/mip/definitions/common.h index 869557c7c..9f287ad9a 100644 --- a/src/mip/definitions/common.h +++ b/src/mip/definitions/common.h @@ -42,30 +42,6 @@ DECLARE_MIP_VECTOR_TYPE(4, float, mip_quatf) #undef DECLARE_MIP_VECTOR_TYPE -//typedef struct mip_vector3f -//{ -// float m_data[3]; -//} mip_vector3f; -// -//void insert_mip_vector3(mip_serializer* serializer, const mip_vector3* self); -//void extract_mip_vector3(mip_serializer* serializer, mip_vector3* self); -// -//typedef struct mip_vector4f -//{ -// float m_data[4]; -//} mip_vector4f; -// -//void insert_mip_vector3(mip_serializer* serializer, const mip_vector3* self); -//void extract_mip_vector3(mip_serializer* serializer, mip_vector3* self); -// -//typedef struct mip_matrix3 -//{ -// float m[9]; -//} mip_matrix3; -// -//void insert_mip_matrix3(mip_serializer* serializer, const mip_matrix3* self); -//void extract_mip_matrix3(mip_serializer* serializer, mip_matrix3* self); - #ifdef __cplusplus @@ -179,152 +155,6 @@ void insert(Serializer& serializer, const Vector& v) { for(size_t i=0; i void extract(Serializer& serializer, Vector& v) { for(size_t i=0; i -//class Vector -//{ -//public: -// Vector() { *this = T(0); } -// Vector(const Vector&) = default; -// Vector(const T (&ptr)[N]) { copyFrom(ptr); } -// -// Vector& operator=(const Vector&) = default; -// Vector& operator=(const T* ptr) { copyFrom(ptr); return this; } -// Vector& operator=(T value) { for(size_t i=0; i -// void copyFrom(const U* ptr) { for(size_t i=0; i -// void copyTo(U* ptr) const { for(size_t i=0; i -//class Vector2 : public Vector -//{ -// using Vector::Vector; -// -// template -// Vector2(U x_, U y_) { x()=x_; y()=y_;} -// -// T& x() { return this->m_data[0]; } -// T& y() { return this->m_data[1]; } -// -// T x() const { return this->m_data[0]; } -// T y() const { return this->m_data[1]; } -//}; -// -/////@brief A 3D vector which provides .x(), .y() and .z() accessors. -///// -//template -//class Vector3 : public Vector -//{ -// //using Vector::Vector; -// template -// Vector3(const U* ptr) { copyFrom(ptr); } -// -// template -// Vector3(U x_, U y_, U z_) { x()=x_; y()=y_; z()=z_; } -// -// T& x() { return this->m_data[0]; } -// T& y() { return this->m_data[1]; } -// T& z() { return this->m_data[2]; } -// -// T x() const { return this->m_data[0]; } -// T y() const { return this->m_data[1]; } -// T z() const { return this->m_data[2]; } -//}; -// -/////@brief A 4D vector which provides .x(), .y(), .z(), and .w() accessors. -///// -//template -//class Vector4 : public Vector -//{ -// using Vector::Vector; -// -// template -// Vector4(U x_, U y_, U z_, U w_) { x()=x_; y()=y_; z()=z_; w()=w_; } -// -// T& x() { return this->m_data[0]; } -// T& y() { return this->m_data[1]; } -// T& z() { return this->m_data[2]; } -// T& w() { return this->m_data[3]; } -// -// T x() const { return this->m_data[0]; } -// T y() const { return this->m_data[1]; } -// T z() const { return this->m_data[2]; } -// T w() const { return this->m_data[3]; } -//}; -// -/////@brief A quaternion which provides .x(), .y(), .z(), and .w() accessors. -///// -//template -//class Quaternion : public Vector4 -//{ -// using Vector4::Vector; -//}; -// -//typedef Vector2 Vector2f; -//typedef Vector3 Vector3f; -//typedef Vector4 Vector4f; -// -//typedef Vector2 Vector2d; -//typedef Vector3 Vector3d; -//typedef Vector4 Vector4d; -// -//typedef Quaternion Quatf; -//typedef Vector Matrix3f; -// -//template -//void insert(Serializer& serializer, const Vector& self) -//{ -// for(size_t i=0; i -//void extract(Serializer& serializer, Vector& self) -//{ -// for(size_t i=0; i -//void insert(Serializer& serializer, const Quaternion& self) -//{ -// insert(serializer, self.w()); // w comes first in mip -// insert(serializer, self.x()); -// insert(serializer, self.y()); -// insert(serializer, self.z()); -//} -// -//template -//void extract(Serializer& serializer, Quaternion& self) -//{ -// extract(serializer, self.w()); // w comes first in mip -// extract(serializer, self.x()); -// extract(serializer, self.y()); -// extract(serializer, self.z()); -//} - } // namespace mip From 99a153e48eb57228189bbaec4e3ee0d5c8546f1b Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 1 May 2024 15:13:47 -0400 Subject: [PATCH 230/252] Fix condition in threading example. --- examples/threading.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/threading.cpp b/examples/threading.cpp index 33eeb93af..7d20371b7 100644 --- a/examples/threading.cpp +++ b/examples/threading.cpp @@ -62,7 +62,8 @@ void device_thread_loop(mip::DeviceInterface* device) bool update_device(mip::DeviceInterface& device, mip::Timeout wait_time) { - if( wait_time > 0 ) + // Thread calls this with wait_time 0, commands have wait_time > 0. + if( wait_time == 0 ) return device.defaultUpdate(wait_time); // Optionally display progress while waiting for command replies. From 8892026537935cc0404356b98bcf3382b13acc15 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 1 May 2024 15:14:13 -0400 Subject: [PATCH 231/252] Move some documentation from README to doxygen. --- README.md | 91 +++++++++-------------------------------------------- docs/docs.c | 74 +++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 86 insertions(+), 79 deletions(-) diff --git a/README.md b/README.md index 48f16c55e..f34c1ba86 100644 --- a/README.md +++ b/README.md @@ -53,6 +53,18 @@ Documentation Documentation for all released versions can be found [here](https://lord-microstrain.github.io/mip_sdk_documentation). +### C and C++ APIs + +The C++ API is implemented on top of the C API to provide additional features: +* Object-oriented interfaces +* Improved type safety and sanity checking +* Better clarity / reduced verbosity (e.g. with `using namespace mip`) + +The C++ API uses `TitleCase` for types and `camelCase` for functions and variables, while the C api uses `snake_case` naming for +everything. This makes it easy to tell which is being used when looking at the examples. + +The C API can be accessed directly from C++ via the `mip::C` namespace. + Communications Interfaces ------------------------- @@ -83,7 +95,7 @@ How to Build * CMake version 3.10 or later * A working C compiler - * C99 or later required + * C11 or later required * A working C++ compiler * For C++ API only. Define `MIP_DISABLE_CPP=ON` if you don't want to use any C++. * C++11 or later required for the mip library @@ -114,82 +126,7 @@ The following options may be specified when configuring the build with CMake (e. 2. In the build directory, run `cmake .. ` * Replace `` with your configuration options, such as `-DMIP_USE_SERIAL=1`. * You can use `cmake-gui ..` instead if you'd prefer to use the GUI tool (and have it installed). - * An alternative generator may be used, such as ninja, code blocks, etc. by specifying `-G ` + * An alternative generator may be used, such as ninja, code blocks, etc. by specifying `-G ` 3. Invoke `cmake --build .` in the build directory 4. (Optional, if BUILD_PACKAGE was enabled) Run `cmake --build . --target package` to build the packages. - -Implementation Notes --------------------- - -### User-Implemented Functions - -There are two C functions which must be implemented to use this library. - -The first, `mip_interface_user_recv_from_device()`, must fetch raw data bytes from the connected MIP device. Typically this means reading from -a serial port or TCP socket. - -The second, `mip_interface_send_to_device()`, must pass the provided data bytes directly to the connected MIP device. - -See [`mip_interface`](https://lord-microstrain.github.io/mip_sdk_documentation/latest/mip_interface.html) for details on how to implement these functions. - -#### C++ -For C++ applications, these functions are implemented by the `DeviceInterface` class, which takes a `Connection` object responsible -for reading and writing to the device. Create a class derived from `Connection` and implement the pure virtual `recvFromDevice` and -`sendToDevice` methods. - -If you do not wish to use the `DeviceInterface` class, do not compile the corresponding source file and create the -C functions yourself. Declare them functions as `extern "C"` to avoid linking problems between the C and C++ code. - -### Command Results (mip_cmd_result / MipCmdResult) - -Command results are divided into two categories: -* Reply codes are returned by the device, e.g.: - * ACK / OK - * Invalid parameter - * Unknown command -* Status codes are set by this library, e.g.: - * General ERROR - * TIMEDOUT - * Other statuses are used while the command is still in process - -### Timestamps and Timeouts - -#### Timestamp type -Timestamps (`timestamp_type` / `Timestamp`) represent the local time when data was received or a packet was parsed. These timestamps -are used to implement command timeouts and provide the user with an approximate timestamp of received data. It is not intended to be -a precise timestamp or used for synchronization, and it generally cannot be used instead of the timestamps from the connected MIP device. -In particular, if you limit the maximum number of packets processed per `update` call, the timestamp of some packets may be delayed. - -Because different applications may keep track of time differently (especially on embedded platforms), it is up to the user to provide -the current time whenever data is received from the device. On a PC, this might come from the poxis `time()` function or from the -`std::chrono` library. On ARM systems, it is often derived from the Systick timer. It should be a monotonically increasing value; -jumps backwards in time will cause problems. - -By default, timestamps are `typedef`'d to `uint64_t`. Typically timestamps are in milliseconds. Embedded systems may wish to use -`uint32_t` or even `uint16_t` instead. The value is allowed to wrap around as long as the time between wraparounds is longer than -twice the longest timeout needed. If higher precision is needed or wraparound can't be tolerated by your application, define it to -`uint64_t`. It must be a standard unsigned integer type. - -#### Command Timeouts - -Timeouts for commands are broken down into two parts. -* A "base reply timeout" applies to all commands. This is useful to compensate for communication latency, such as over a TCP socket. -* "Additional time" which applies per command, because some commands may take longer to complete. - -Currently, only the C++ api offers a way to set the additional time parameter, and only when using the `runCommand` function taking -the command structure and the `additionalTime` parameter. - -The `timeout_type` / `Timeout` typedef is an alias to the timestamp type. - -### C and C++ APIs - -The C++ API is implemented on top of the C API to provide additional features: -* Object-oriented interfaces -* Improved type safety and sanity checking -* Better clarity / reduced verbosity (e.g. with `using namespace mip`) - -The C++ API uses `TitleCase` for types and `camelCase` for functions and variables, while the C api uses `snake_case` naming for -everything. This makes it easy to tell which is being used when looking at the examples. - -The C API can be accessed directly from C++ via the `mip::C` namesace. diff --git a/docs/docs.c b/docs/docs.c index cef1ea16e..320da16a8 100644 --- a/docs/docs.c +++ b/docs/docs.c @@ -379,7 +379,7 @@ /// about right. You can use the mip_timeout_from_baudrate() function to /// compute an appropriate timeout. /// -///@see timestamp_type +///@see mip_timestamp ///@see mip::Timestamp /// //////////////////////////////////////////////////////////////////////////////// @@ -424,4 +424,74 @@ /// a single byte is dropped from the ring buffer and the loop is continued. /// Only a single byte can be dropped, because rogue SYNC1 bytes or truncated /// packets may hide real mip packets in what would have been their payload. -/// \ No newline at end of file +/// +//////////////////////////////////////////////////////////////////////////////// +///@page command_results Command Result +//////////////////////////////////////////////////////////////////////////////// +/// +///@li @ref mip::C::mip_cmd_result "mip_command_result [C]" +///@li @ref mip::CmdResult "CmdResult [C++]" +/// +/// Command results are divided into two categories, reply codes and status +/// codes. Reply codes are returned by the device, e.g.: +///@li ACK_OK +///@li Unknown command (NACK_COMMAND_UNKNOWN) +///@li Invalid parameter (NACK_INVALID_PARAM) +///@li Command failed (NACK_COMMAND_FAILED) +/// The values of these enums match the corresponding values returned by the +/// device. They are non-negative integers. +/// +/// Status codes are set by this library, e.g.: +///@li General error (STATUS_ERROR) +///@li Timeout (STATUS_TIMEDOUT) +///@li Other statuses are used to track commands in progress +///@li User status codes can also be set (STATUS_USER) +/// All of these are negative integers. +/// +/// You can use mip_cmd_result_is_reply_code() / CmdResult::isReplyCode() and +/// mip_cmd_result_is_status_code() / CmdResult::isStatusCode() to distinguish +/// between them. +/// +/// In C++, CmdResult is implicitly convertible to bool. ACK_OK converts to true +/// while everything else converts to false. This allows compact code like +///@code{.cpp} +/// if( !resume(device) ) // resume returns a CmdResult +/// fprintf(stderr, "Failed to resume the sensor\n"); +///@endcode +/// +/// For debugging, the name of command results is available via +/// mip_cmd_result_to_string() / CmdResult::name() +/// +/// In C++, CmdResult defaults to the initial state CmdResult::STATUS_NONE. +/// +//////////////////////////////////////////////////////////////////////////////// +///@page timestamps Timestamps and Timeouts +//////////////////////////////////////////////////////////////////////////////// +/// +///@section Timestamp type +/// Timestamps (`mip_timestamp` / `Timestamp`) represent the local time when data was received or a packet was parsed. These timestamps +/// are used to implement command timeouts and provide the user with an approximate timestamp of received data. It is not intended to be +/// a precise timestamp or used for synchronization, and it generally cannot be used instead of the timestamps from the connected MIP device. +/// In particular, if you limit the maximum number of packets processed per `update` call, the timestamp of some packets may be delayed. +/// +/// Because different applications may keep track of time differently (especially on embedded platforms), it is up to the user to provide +/// the current time whenever data is received from the device. On a PC, this might come from the poxis `time()` function or from the +/// `std::chrono` library. On ARM systems, it is often derived from the Systick timer. It should be a monotonically increasing value; +/// jumps backwards in time will cause problems. +/// +/// By default, timestamps are `typedef`'d to `uint64_t`. Typically timestamps are in milliseconds. Embedded systems may wish to use +/// `uint32_t` or even `uint16_t` instead. The value is allowed to wrap around as long as the time between wraparounds is longer than +/// twice the longest timeout needed. If higher precision is needed or wraparound can't be tolerated by your application, define it to +/// `uint64_t`. It must be a standard unsigned integer type. +/// +///@section Command Timeouts +/// +/// Timeouts for commands are broken down into two parts. +/// * A "base reply timeout" applies to all commands. This is useful to compensate for communication latency, such as over a TCP socket. +/// * "Additional time" which applies per command, because some commands may take longer to complete. +/// +/// Currently, only the C++ api offers a way to set the additional time parameter, and only when using the `runCommand` function taking +/// the command structure and the `additionalTime` parameter. +/// +/// The `mip_timeout` / `Timeout` typedef is an alias to the timestamp type. +/// From 2c4cb6b284cb7c3739c71a1090479e6160310328 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 1 May 2024 16:40:52 -0400 Subject: [PATCH 232/252] Add CompositeDescriptor accessor to mip::Field class (reviewed by Nick). --- src/mip/mip.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mip/mip.hpp b/src/mip/mip.hpp index 35dc0cb09..637fe2168 100644 --- a/src/mip/mip.hpp +++ b/src/mip/mip.hpp @@ -62,6 +62,8 @@ class Field : public C::mip_field uint8_t descriptorSet() const { return C::mip_field_descriptor_set(this); } ///@copydoc mip_field_field_descriptor uint8_t fieldDescriptor() const { return C::mip_field_field_descriptor(this); } + ///@brief Returns the descriptor set and field descriptor. + CompositeDescriptor descriptor() const { return {descriptorSet(), fieldDescriptor()}; } ///@copydoc mip_field_payload_length uint8_t payloadLength() const { return C::mip_field_payload_length(this); } ///@copydoc mip_field_payload From dbd630904ec588adc5ebc0b6a3ef2f966b7ef193 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 8 May 2024 12:05:29 -0400 Subject: [PATCH 233/252] Update README and add doc page for known issues and workarounds (see https://lordsensing.atlassian.net/browse/MSCLLITE-94 and https://lordsensing.atlassian.net/browse/MSCLLITE-77) --- README.md | 39 ++++++++++++++++++++++++++++-------- docs/docs.c | 57 +++++++++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 82 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index f34c1ba86..97bb04334 100644 --- a/README.md +++ b/README.md @@ -15,8 +15,8 @@ Features * Simple to interface with existing projects * FindMip.cmake is included for CMake-based projects * Can be used to parse offline binary files -* C API for those who can't use C++ * C++ API for safety, flexibility, and convenience. +* C API for those who can't use C++ * Advanced Features * MIP packet creation @@ -38,7 +38,7 @@ Examples * **GQ7 setup** [[C](./examples/GQ7/GQ7_example.c), [C++](./examples/GQ7/GQ7_example.cpp)]. * **CV7 setup** [[C](./examples/CV7/CV7_example.c), [C++](./examples/CV7/CV7_example.cpp)]. * **GX5-45 setup** [[C](./examples/GX5_45/GX5_45_example.c), [C++](./examples/GX5_45/GX5_45_example.cpp)]. - * **CV7_INS setup** [[C++](./examples/CV7_INS/CV7_INS_simple_example.cpp)]. + * **CV7_INS setup** [[C++](./examples/CV7_INS/CV7_INS_simple_example.cpp)]. * **CV7_INS with UBlox setup** [[C++](./examples/CV7_INS/CV7_INS_simple_ublox_example.cpp)]. You'll need to enable at least one of the [communications interfaces](#communications-interfaces) in the CMake configuration to use the examples. @@ -73,7 +73,7 @@ Two connection types are provided with the MIP SDK to make it easy to run the ex ### Serial Port -A basic serial port interface is provided in C and C++ for Linux and Windows. These can be modified for other platforms by the user. +A basic serial port interface is provided in C and C++ for Linux, Mac, and Windows. These can be modified for other platforms by the user. The serial port connection will be used in most cases, when the MIP device is connected via a serial or USB cable (the USB connection acts like a virtual serial port). @@ -93,23 +93,23 @@ How to Build ### Prerequisites -* CMake version 3.10 or later * A working C compiler * C11 or later required * A working C++ compiler * For C++ API only. Define `MIP_DISABLE_CPP=ON` if you don't want to use any C++. * C++11 or later required for the mip library * C++14 or later for the examples (currently CMakeLists.txt assumes C++14 is required regardless) +* CMake version 3.10 or later (technically this is optional, see below) * Doxygen, if building documentation -### Build configuration +### CMake Build Configuration The following options may be specified when configuring the build with CMake (e.g. `cmake .. -DOPTION=VALUE`): * MIP_USE_SERIAL - Builds the included serial port library (default enabled). * MIP_USE_TCP - Builds the included socket library (default enabled). * MIP_USE_EXTRAS - Builds some higher level utility classes and functions that may use dynamic memory. * MIP_ENABLE_LOGGING - Builds logging functionality into the library. The user is responsible for configuring a logging callback (default enabled) -* MIP_LOGGING_MAX_LEVEL - Max log level the SDK is allowed to log. If this is defined, any log level logged at a higher level than this will result in a noop regardless of runtime configuration. Useful if you want some logs, but do not want the overhead of the higher level functions compiled into the code +* MIP_LOGGING_MAX_LEVEL - Max log level the SDK is allowed to log. If this is defined, any log level logged at a higher level than this will result in a noop regardless of runtime configuration. Useful if you want some logs, but do not want the overhead compiled into the code. * MIP_ENABLE_DIAGNOSTICS - Adds some counters to various entities which can serve as a debugging aid. * BUILD_EXAMPLES - If enabled (`-DBUILD_EXAMPLES=ON`), the example projects will be built (default disabled). * BUILD_TESTING - If enabled (`-DBUILD_TESTING=ON`), the test programs in the /test directory will be compiled and linked. Run the tests with `ctest`. @@ -118,9 +118,9 @@ The following options may be specified when configuring the build with CMake (e. * BUILD_DOCUMENTATION_QUIET - Suppress standard doxygen output (default enabled). * MIP_DISABLE_CPP - Ignores .hpp/.cpp files during the build and does not add them to the project. * BUILD_PACKAGE - Adds a `package` target to the project that will build a `.deb`, `.rpm`, or `.7z` file containing the library -* MIP_TIMESTAMP_TYPE - Overrides the default timestamp type. See the timestamps section below. +* MIP_TIMESTAMP_TYPE - Overrides the default timestamp type. See the timestamps section in the documentation. -### Compilation with CMake +### Compilation 1. Create the build directory (e.g. `mkdir build`). 2. In the build directory, run `cmake .. ` @@ -130,3 +130,26 @@ The following options may be specified when configuring the build with CMake (e. 3. Invoke `cmake --build .` in the build directory 4. (Optional, if BUILD_PACKAGE was enabled) Run `cmake --build . --target package` to build the packages. +### Building without CMake + +If your target platform doesn't support CMake, you can build the project without it. To do so, +include all the necessary files and define a few options. + +#### Minimum Required Files for building without CMake +* Everything in `src/mip/definitions` (or at least all the descriptor sets you require) +* All the .c, .h, .cpp, and .hpp files in `src/mip` (exclude the c++ files if you're using plain C) +* The `byte_ring` and `serialization` .c/.h files in `src/mip/utils` +* You may optionally include the platform-related connection files (`serial_port.h/.c`) as desired. + +#### Required #defines for building without CMake + +Pass these to your compiler as appropriate, e.g. `arm-none-eabi-gcc -DMIP_TIMESTAMP_TYPE=uint32_t -DMIP_ENABLE_LOGGING=0` + +* MIP_ENABLE_LOGGING (and MIP_LOGGING_MAX_LEVEL) - default is enabled +* MIP_TIMESTAMP_TYPE - defaults to uint64_t if not specified +* MIP_ENABLE_DIAGNOSTICS - Supported on embedded platforms to aid debugging + +These options affect the compiled code interface and sizes of various structs. They +MUST be consistent between compiling the MIP SDK and any other code which includes +headers from the MIP SDK. (If you change them after building, make sure everything gets +rebuilt properly. Normally CMake takes care of this for you). diff --git a/docs/docs.c b/docs/docs.c index 320da16a8..7df721d0a 100644 --- a/docs/docs.c +++ b/docs/docs.c @@ -40,7 +40,7 @@ ///@li @ref mip_packet_c ///@li @ref mip_field_c ///@li @ref mip_parser_c -///@li @ref mip::C::mip_cmd_result +///@li @ref mip::C::mip_cmd_result "mip_cmd_result [C]" /// /// //////////////////////////////////////////////////////////////////////////////// @@ -191,7 +191,7 @@ /// /// The application should call mip_interface_update() periodically to process /// data sent by the device. This update function will call -/// mip_interface_user_recv_from_device() to parse packets. When a data packet is +/// mip_interface_recv_from_device() to parse packets. When a data packet is /// received, the list of packet and data callbacks is checked, and any /// matching callbacks are invoked. The update function should be called at /// a high enough rate to avoid overflowing the connection buffers. The @@ -210,7 +210,7 @@ /// from within the command function. While the command is waiting (status code /// MIP_STATUS_WAITING / CmdResult::STATUS_WAITING), repeated calls to the /// update function will be made. By default, the update function calls -/// mip_interface_user_recv_from_device(). Because the function is called from +/// mip_interface_recv_from_device(). Because the function is called from /// within a loop, it should sleep for a short time to wait for data if none /// has been received yet. Doing so prevents excessive CPU usage and lowers /// power consumption. @@ -283,10 +283,10 @@ /// applications, too: ///@li To update a progress bar while waiting for commands to complete ///@li To process data from other devices -///@li To avoid blocking inside mip_interface_user_recv_from_device() when +///@li To avoid blocking inside mip_interface_recv_from_device() when /// called from a data processing loop. ///@li To push data through the system in a different way (e.g. without using -/// mip_interface_user_recv_from_device()) +/// mip_interface_recv_from_device()) /// /// Data may be pushed into the system by calling any of these functions: ///@li mip_interface_default_update() - this is the default behavior. @@ -373,7 +373,7 @@ /// to time out and make the device appear temporarily unresponsive. Setting a /// reasonable timeout ensures that the bad packet is rejected more quickly. /// The timeout should be set so that a MIP packet of the largest possible -/// size (261 bytes) can be transfered well within the transmission time plus +/// size (261 bytes) can be transferred well within the transmission time plus /// any additional processing delays in the application or operating system. /// As an example, for a 115200 baud serial link a timeout of 30 ms would be /// about right. You can use the mip_timeout_from_baudrate() function to @@ -495,3 +495,48 @@ /// /// The `mip_timeout` / `Timeout` typedef is an alias to the timestamp type. /// +//////////////////////////////////////////////////////////////////////////////// +///@page other Other Considerations +//////////////////////////////////////////////////////////////////////////////// +/// +///@section known_issues Known Issues and Workarounds +/// +///@par suppress_ack parameters are not supported +/// +/// Some commands accept a parameter named `suppress_ack` which acts to prevent +/// the standard ack/nack reply from being returned by the device, e.g. the +/// 3DM Poll Data command. Use of this parameter is not supported in the MIP SDK +/// and will cause the command to appear to time out after a short delay. +/// +/// If you do not wish to wait for a reply (i.e. just send the command and continue) +/// you can build and send the command yourself: +///@code{.cpp} +/// // Create the command with required parameters. +/// mip::commands_3dm::PollData cmd; +/// cmd.desc_set = mip::data_sensor::DESCRIPTOR_SET; +/// cmd.suppress_ack = true; +/// cmd.num_descriptors = 2; +/// cmd.descriptors[0] = mip::data_sensor::ScaledAccel::FIELD_DESCRIPTOR; +/// cmd.descriptors[1] = mip::data_sensor::ScaledGyro::FIELD_DESCRIPTOR; +/// // Build a packet. +/// mip::PacketBuf packet(cmd); +/// // Send it to the device. +/// device.sendCommand(packet); +///@endcode +/// +///@par Some commands take longer and may time out +/// +/// This applies to the following commands: +///@li Save All Settings (`mip::commands_3dm::DeviceSettings` with `mip::FunctionSelector::SAVE`) +///@li Commanded Built-In Test (`mip::commands_base::BuiltInTest`) +///@li Capture Gyro Bias (`mip::commands_3dm::CaptureGyroBias`) +/// +/// The device timeout must be sufficiently long when sending these commands. +/// There are 3 potential ways to avoid erroneous timeouts: +///@li Set a high overall device timeout. This is the easiest solution but may cause excess +/// delays in your application if the device is unplugged, not powered, etc. +///@li Temporarily set the timeout higher, and restore it after running the long command. +///@li If using C++, use the `mip::DeviceInterface::runCommand` function and pass a large enough +/// value for the `additionalTime` parameter. This raises the timeout specifically for that +/// one command instance and is the recommended option. +/// \ No newline at end of file From fdc3b06a024747542b56e16b334927b8a339af4a Mon Sep 17 00:00:00 2001 From: robbiefish Date: Wed, 8 May 2024 21:15:30 +0000 Subject: [PATCH 234/252] Properly releases to github --- Jenkinsfile | 60 ++++++++++++++++++++++++++++------------------------- 1 file changed, 32 insertions(+), 28 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 667aa328b..db84d00c3 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -110,37 +110,41 @@ pipeline { success { script { if (BRANCH_NAME && BRANCH_NAME == 'develop') { - node("master") { - withCredentials([string(credentialsId: 'Github_Token', variable: 'GH_TOKEN')]) { - sh ''' - # Release to github - archive_dir="${WORKSPACE}/../../jobs/LORD-MicroStrain/jobs/mip_sdk/branches/${BRANCH_NAME}/builds/${BUILD_NUMBER}/archive/" - ./scripts/release.sh \ - --artifacts "$(find "${archive_dir}" -type f)" \ - --target "${BRANCH_NAME}" \ - --release "latest" \ - --docs-zip "$(find "${archive_dir}" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq)" \ - --generate-notes - ''' + node("linux-amd64") { + dir("/tmp/mip_sdk_${env.BRANCH_NAME}_${currentBuild.number}") { + copyArtifacts(projectName: "${env.JOB_NAME}", selector: specific("${currentBuild.number}")); + withCredentials([string(credentialsId: 'Github_Token', variable: 'GH_TOKEN')]) { + sh ''' + # Release to github + "${WORKSPACE}/scripts/release.sh" \ + --artifacts "$(find "$(pwd)" -type f)" \ + --target "${BRANCH_NAME}" \ + --release "latest" \ + --docs-zip "$(find "$(pwd)" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq)" \ + --generate-notes + ''' + } } } } else if (BRANCH_NAME && BRANCH_NAME == 'master') { - node("master") { - withCredentials([string(credentialsId: 'MICROSTRAIN_BUILD_GH_TOKEN', variable: 'GH_TOKEN')]) { - sh ''' - # Release to the latest version if the master commit matches up with the commit of that version - archive_dir="${WORKSPACE}/../../jobs/LORD-MicroStrain/jobs/mip_sdk/branches/${BRANCH_NAME}/builds/${BUILD_NUMBER}/archive/" - if git describe --exact-match --tags HEAD &> /dev/null; then - # Publish a release - ./scripts/release.sh \ - --artifacts "$(find "${archive_dir}" -type f)" \ - --target "${BRANCH_NAME}" \ - --release "$(git describe --exact-match --tags HEAD)" \ - --docs-zip "$(find "${archive_dir}" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq)" - else - echo "Not releasing from ${BRANCH_NAME} since the current commit does not match the latest released version commit" - fi - ''' + node("linux-amd64") { + dir("/tmp/mip_sdk_${env.BRANCH_NAME}_${currentBuild.number}") { + copyArtifacts(projectName: "${env.JOB_NAME}", selector: specific("${currentBuild.number}")); + withCredentials([string(credentialsId: 'MICROSTRAIN_BUILD_GH_TOKEN', variable: 'GH_TOKEN')]) { + sh ''' + # Release to the latest version if the master commit matches up with the commit of that version + if (cd "${WORKSPACE}" && git describe --exact-match --tags HEAD &> /dev/null); then + # Publish a release + ./scripts/release.sh \ + --artifacts "$(find "$(pwd)" -type f)" \ + --target "${BRANCH_NAME}" \ + --release "$(cd ${WORKSPACE} && git describe --exact-match --tags HEAD)" \ + --docs-zip "$(find "$(pwd)" -type f -name "mipsdk_*_Documentation.zip" | sort | uniq)" + else + echo "Not releasing from ${BRANCH_NAME} since the current commit does not match the latest released version commit" + fi + ''' + } } } } From 23dc00caa80dd6dbd19b1279cd1baa8b6153db3e Mon Sep 17 00:00:00 2001 From: Nick DaCosta Date: Wed, 8 May 2024 17:37:02 -0400 Subject: [PATCH 235/252] Moved all config options to top of CMakeLists.txt Fixed include directories for install vs build --- CMakeLists.txt | 67 ++++++++++++++++++++++++++------------------------ 1 file changed, 35 insertions(+), 32 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4ccb3f05..de9fdebff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,20 +13,37 @@ project( LANGUAGES C CXX ) +# +# Build options +# + +option(MIP_USE_SERIAL "Build serial connection support into the library and examples" ON) +option(MIP_USE_TCP "Build TCP connection support into the library and exampels" ON) +option(MIP_USE_EXTRAS "Build extra support into the library including some things that might work at a higher level and use dynamically allocated memory" ON) +option(MIP_ENABLE_DIAGNOSTICS "Enable various diagnostic counters in the mip library for debugging." OFF) +option(MIP_ENABLE_LOGGING "Build with logging functions enabled" ON) +option(BUILD_PACKAGE "Whether to build a package from the resulting binaries" OFF) +option(BUILD_EXAMPLES "Builds the example programs." ON) +# CTest defines this option to ON by default, so override it to OFF here. +option(BUILD_TESTING "Build the testing tree." OFF) +option(MIP_DISABLE_CPP "Excludes all C++ files from the project." OFF) +option(BUILD_DOCUMENTATION "Build the documentation." OFF) +option(BUILD_DOCUMENTATION_FULL "Build the full (internal) documentation." OFF) +option(BUILD_DOCUMENTATION_QUIET "Suppress doxygen standard output." ON) + +set(MIP_LOGGING_MAX_LEVEL "" CACHE STRING "Max log level the SDK is allowed to log. If this is defined, any log level logged at a higher level than this will result in a noop regardless of runtime configuration.") +set(MIP_TIMESTAMP_TYPE "uint64_t" CACHE STRING "Override the type used for received data timestamps and timeouts (must be unsigned or at least 64 bits).") + set(SRC_DIR "${CMAKE_CURRENT_LIST_DIR}/src") set(EXT_DIR "${CMAKE_CURRENT_LIST_DIR}/ext") set(MIP_CMAKE_DIR "${CMAKE_CURRENT_LIST_DIR}/cmake") -include_directories( - "${SRC_DIR}" -) - if(WITH_INTERNAL) if(NOT DEFINED MIP_INTERNAL_DIR) - set(MIP_INTERNAL_DIR "int" CACHE PATH "") + set(MIP_INTERNAL_DIR "int" CACHE PATH "") endif() - - add_subdirectory("${MIP_INTERNAL_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/mip-internal") + + add_subdirectory("${MIP_INTERNAL_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/mip-internal") endif() set(MIP_DIR "${SRC_DIR}/mip") @@ -76,22 +93,6 @@ set(VERSION_IN_FILE "${MIP_CMAKE_DIR}/mip_version.h.in") set(VERSION_OUT_FILE "${MIP_DIR}/mip_version.h") configure_file(${VERSION_IN_FILE} ${VERSION_OUT_FILE}) -# -# Build options -# - -option(MIP_USE_SERIAL "Build serial connection support into the library and examples" ON) -option(MIP_USE_TCP "Build TCP connection support into the library and exampels" ON) -option(MIP_USE_EXTRAS "Build extra support into the library including some things that might work at a higher level and use dynamically allocated memory" ON) -option(MIP_ENABLE_DIAGNOSTICS "Enable various diagnostic counters in the mip library for debugging." OFF) - -option(MIP_ENABLE_LOGGING "Build with logging functions enabled" ON) -set(MIP_LOGGING_MAX_LEVEL "" CACHE STRING "Max log level the SDK is allowed to log. If this is defined, any log level logged at a higher level than this will result in a noop regardless of runtime configuration.") - -set(MIP_TIMESTAMP_TYPE "uint64_t" CACHE STRING "Override the type used for received data timestamps and timeouts (must be unsigned or at least 64 bits).") - -option(BUILD_PACKAGE "Whether to build a package from the resulting binaries" OFF) - # # Utils # @@ -228,13 +229,23 @@ set(ALL_MIP_SOURCES ${MIP_EXTRA_SOURCES} ) -option(MIP_DISABLE_CPP "Excludes all C++ files from the project." OFF) if(MIP_DISABLE_CPP) list(FILTER ALL_MIP_SOURCES EXCLUDE REGEX "[c|h]pp$") endif() add_library(mip ${ALL_MIP_SOURCES}) +target_include_directories(mip PUBLIC + "$" # Include directory for build only + "$" # Include directory for installation +) + +if(WITH_INTERNAL) + target_include_directories(mip PUBLIC + "$" + ) +endif() + # # Preprocessor definitions # @@ -276,9 +287,6 @@ endif() # TESTING # -# CTest defines this option to ON by default, so override it to OFF here. -option(BUILD_TESTING "Build the testing tree." OFF) - include(CTest) if(BUILD_TESTING) @@ -289,8 +297,6 @@ endif() # EXAMPLES # -option(BUILD_EXAMPLES "Builds the example programs." ON) - if(BUILD_EXAMPLES) add_subdirectory("examples") endif() @@ -300,9 +306,6 @@ endif() # find_package(Doxygen) -option(BUILD_DOCUMENTATION "Build the documentation." OFF) -option(BUILD_DOCUMENTATION_FULL "Build the full (internal) documentation." OFF) -option(BUILD_DOCUMENTATION_QUIET "Suppress doxygen standard output." ON) if(BUILD_DOCUMENTATION) if(NOT DOXYGEN_FOUND) From d9d5c0176aef8f7689ec756e9fe54e7093fb6e82 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Thu, 9 May 2024 09:46:46 -0400 Subject: [PATCH 236/252] Added examples directory to cmake for examples and removed ../ in utility header includes --- examples/CMakeLists.txt | 12 ++++++++++++ examples/CV7/CV7_example.c | 2 +- examples/CV7/CV7_example.cpp | 4 +++- examples/CV7_INS/CV7_INS_simple_example.cpp | 2 +- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 3 ++- examples/GQ7/GQ7_example.c | 2 +- examples/GQ7/GQ7_example.cpp | 4 +++- examples/GX5_45/GX5_45_example.c | 3 ++- examples/GX5_45/GX5_45_example.cpp | 4 +++- examples/example_utils.c | 7 ++----- examples/example_utils.h | 6 +----- 11 files changed, 31 insertions(+), 18 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index d9f5ffbea..93c85f8d7 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -31,35 +31,43 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) add_executable(DeviceInfo "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/device_info.cpp" ${DEVICE_SOURCES}) target_link_libraries(DeviceInfo mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(DeviceInfo PUBLIC "${MIP_EXAMPLE_DEFS}") + target_include_directories(DeviceInfo PUBLIC "${EXAMPLE_DIR}") add_executable(WatchImu "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/watch_imu.cpp" ${DEVICE_SOURCES}) target_link_libraries(WatchImu mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(WatchImu PUBLIC "${MIP_EXAMPLE_DEFS}") + target_include_directories(WatchImu PUBLIC "${EXAMPLE_DIR}") find_package(Threads REQUIRED) add_executable(ThreadingDemo "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/threading.cpp" ${DEVICE_SOURCES}) target_link_libraries(ThreadingDemo mip "${SERIAL_LIB}" "${SOCKET_LIB}" "${CMAKE_THREAD_LIBS_INIT}") target_compile_definitions(ThreadingDemo PUBLIC "${MIP_EXAMPLE_DEFS}") + target_include_directories(ThreadingDemo PUBLIC "${EXAMPLE_DIR}") add_executable(GQ7_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GQ7/GQ7_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(GQ7_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(GQ7_Example PUBLIC "${MIP_EXAMPLE_DEFS}") + target_include_directories(GQ7_Example PUBLIC "${EXAMPLE_DIR}") add_executable(CV7_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7/CV7_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(CV7_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_Example PUBLIC "${MIP_EXAMPLE_DEFS}") + target_include_directories(CV7_Example PUBLIC "${EXAMPLE_DIR}") add_executable(CV7_INS_Simple_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(CV7_INS_Simple_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_include_directories(CV7_INS_Simple_Example PUBLIC "${EXAMPLE_DIR}") add_executable(CV7_INS_Simple_Ublox_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp" ${DEVICE_SOURCES} CV7_INS/ublox_device.hpp CV7_INS/simple_ublox_parser.hpp) target_link_libraries(CV7_INS_Simple_Ublox_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Ublox_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + target_include_directories(CV7_INS_Simple_Ublox_Example PUBLIC "${EXAMPLE_DIR}") add_executable(GX5_45_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GX5_45/GX5_45_example.cpp" ${DEVICE_SOURCES}) target_link_libraries(GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") + target_include_directories(GX5_45_Example PUBLIC "${EXAMPLE_DIR}") endif() @@ -69,13 +77,17 @@ endif() if(MIP_USE_SERIAL) add_executable(WatchImuC "${EXAMPLE_DIR}/watch_imu.c" ${DEVICE_SOURCES}) target_link_libraries(WatchImuC mip) + target_include_directories(WatchImuC PUBLIC "${EXAMPLE_DIR}") add_executable(GQ7_ExampleC "${EXAMPLE_DIR}/GQ7/GQ7_example.c" ${DEVICE_SOURCES}) target_link_libraries(GQ7_ExampleC mip) + target_include_directories(GQ7_ExampleC PUBLIC "${EXAMPLE_DIR}") add_executable(CV7_ExampleC "${EXAMPLE_DIR}/CV7/CV7_example.c" ${DEVICE_SOURCES}) target_link_libraries(CV7_ExampleC mip) + target_include_directories(CV7_ExampleC PUBLIC "${EXAMPLE_DIR}") add_executable(GX5_45_ExampleC "${EXAMPLE_DIR}/GX5_45/GX5_45_example.c" ${DEVICE_SOURCES}) target_link_libraries(GX5_45_ExampleC mip) + target_include_directories(GX5_45_ExampleC PUBLIC "${EXAMPLE_DIR}") endif() \ No newline at end of file diff --git a/examples/CV7/CV7_example.c b/examples/CV7/CV7_example.c index 2932129e3..637fed1c9 100644 --- a/examples/CV7/CV7_example.c +++ b/examples/CV7/CV7_example.c @@ -34,7 +34,7 @@ #include #include -#include "../example_utils.h" +#include "example_utils.h" //////////////////////////////////////////////////////////////////////////////// // Global Variables diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index c15f25097..0c5795660 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -27,9 +27,11 @@ // Include Files //////////////////////////////////////////////////////////////////////////////// +#include "example_utils.hpp" + #include + #include -#include "../example_utils.hpp" using namespace mip; diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 0779c0e17..8bb55948f 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -28,7 +28,7 @@ #include "mip/mip_all.hpp" #include -#include "../example_utils.hpp" +#include "example_utils.hpp" using namespace mip; diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index bf46cc57e..14640172f 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -24,8 +24,9 @@ //////////////////////////////////////////////////////////////////////////////// #include "mip/mip_all.hpp" -#include "../example_utils.hpp" +#include "example_utils.hpp" #include "ublox_device.hpp" + #include #include #include diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index 1f22adf22..bb9bfe0a4 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -34,7 +34,7 @@ #include #include -#include "../example_utils.h" +#include "example_utils.h" //////////////////////////////////////////////////////////////////////////////// diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index 8319023c4..c78702c63 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -28,8 +28,10 @@ //////////////////////////////////////////////////////////////////////////////// #include + #include -#include "../example_utils.hpp" + +#include "example_utils.hpp" using namespace mip; diff --git a/examples/GX5_45/GX5_45_example.c b/examples/GX5_45/GX5_45_example.c index 8063db00b..fb8bcd256 100644 --- a/examples/GX5_45/GX5_45_example.c +++ b/examples/GX5_45/GX5_45_example.c @@ -29,12 +29,13 @@ #include #include + #include #include #include #include -#include "../example_utils.h" +#include "example_utils.h" //////////////////////////////////////////////////////////////////////////////// // Global Variables diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index 73fbebada..552b23125 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -28,8 +28,10 @@ //////////////////////////////////////////////////////////////////////////////// #include + #include -#include "../example_utils.hpp" + +#include "example_utils.hpp" using namespace mip; diff --git a/examples/example_utils.c b/examples/example_utils.c index b02d388b5..421bcbc2f 100644 --- a/examples/example_utils.c +++ b/examples/example_utils.c @@ -1,11 +1,8 @@ #include "example_utils.h" -void displayFilterState( - const mip_filter_mode filter_state, - char **current_state, - bool isFiveSeries -) { +void displayFilterState(const mip_filter_mode filter_state, char **current_state, bool isFiveSeries) +{ char *read_state = ""; if (filter_state == MIP_FILTER_MODE_INIT) { read_state = false ? "GX5_INIT (1)" : "INIT (1)"; diff --git a/examples/example_utils.h b/examples/example_utils.h index 68841db3c..157d5699e 100644 --- a/examples/example_utils.h +++ b/examples/example_utils.h @@ -2,8 +2,4 @@ // Displays current filter state for the connected device if it has changed. -void displayFilterState( - const mip_filter_mode filter_state, - char **current_state, - bool isFiveSeries -); +void displayFilterState(const mip_filter_mode filter_state, char **current_state, bool isFiveSeries); From e1c0de4786182515f02a9ac45e68c49c16a6d7f6 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 9 May 2024 15:01:17 -0400 Subject: [PATCH 237/252] Assign all fields in external_measurement_time in CV7 INS examples (MSCLLITE-98). --- examples/CV7_INS/CV7_INS_simple_example.cpp | 2 ++ examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 1ab4c70db..eda446d45 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -234,6 +234,8 @@ int main(int argc, const char* argv[]) // Use measurement time of arrival for timestamping method commands_aiding::Time external_measurement_time; external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; + external_measurement_time.reserved = 1; + external_measurement_time.nanoseconds = current_timestamp * uint64_t(1'000'000); // External heading command float external_heading = 0.0; diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index bf46cc57e..8a36b0ad2 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -309,6 +309,7 @@ int main(int argc, const char* argv[]) printf("NED_VELOCITY_GNSS_MEAS = [%f %f %f]\n", pvt_message.ned_velocity[0], pvt_message.ned_velocity[1], pvt_message.ned_velocity[2]); commands_aiding::Time external_measurement_time; + external_measurement_time.reserved = 1; if (input_arguments.enable_pps_sync) { From c6add181bece6ce5d3d9ba75f28d8f695cdd15a4 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 9 May 2024 17:29:30 -0400 Subject: [PATCH 238/252] Updates per PR feedback. --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 97bb04334..41c3682d8 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ Features * No dependence on any RTOS or threading * Simple to interface with existing projects * FindMip.cmake is included for CMake-based projects -* Can be used to parse offline binary files +* It can be used to parse offline binary files * C++ API for safety, flexibility, and convenience. * C API for those who can't use C++ @@ -39,7 +39,7 @@ Examples * **CV7 setup** [[C](./examples/CV7/CV7_example.c), [C++](./examples/CV7/CV7_example.cpp)]. * **GX5-45 setup** [[C](./examples/GX5_45/GX5_45_example.c), [C++](./examples/GX5_45/GX5_45_example.cpp)]. * **CV7_INS setup** [[C++](./examples/CV7_INS/CV7_INS_simple_example.cpp)]. - * **CV7_INS with UBlox setup** [[C++](./examples/CV7_INS/CV7_INS_simple_ublox_example.cpp)]. + * **CV7_INS with UBlox setup** [[C++](./examples/CV7_INS/CV7_INS_simple_ublox_example.cpp)]. You'll need to enable at least one of the [communications interfaces](#communications-interfaces) in the CMake configuration to use the examples. From d90b0b0e6e066f838f5d53b80a2ecd486003dd5a Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 13 May 2024 14:47:17 -0400 Subject: [PATCH 239/252] Rename examples to match product support and fix CMakeLists. --- examples/CMakeLists.txt | 35 ++++++------------- .../CX5_GX5_45_example.c} | 0 .../CX5_GX5_45_example.cpp} | 0 .../CX5_GX5_CV5_15_25_example.c} | 14 ++++---- .../CX5_GX5_CV5_15_25_example.cpp} | 2 +- 5 files changed, 18 insertions(+), 33 deletions(-) rename examples/{GX5_45/GX5_45_example.c => CX5_GX5_45/CX5_GX5_45_example.c} (100%) rename examples/{GX5_45/GX5_45_example.cpp => CX5_GX5_45/CX5_GX5_45_example.cpp} (100%) rename examples/{CX5_15/CX5_15_example.c => CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c} (96%) rename examples/{CX5_15/CX5_15_example.cpp => CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp} (99%) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8f80b023f..04aab909e 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -55,22 +55,13 @@ if(MIP_USE_SERIAL OR MIP_USE_TCP) target_link_libraries(CV7_INS_Simple_Ublox_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") target_compile_definitions(CV7_INS_Simple_Ublox_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") - add_executable(GX5_45_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GX5_45/GX5_45_example.cpp" ${DEVICE_SOURCES}) - target_link_libraries(GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") - - add_executable(GX5_15_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/GX5_15/GX5_15_example.cpp" ${DEVICE_SOURCES}) - target_link_libraries(GX5_15_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(GX5_15_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") - - add_executable(CX5_15_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CX5_15/CX5_15_example.cpp" ${DEVICE_SOURCES}) - target_link_libraries(CX5_15_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(CX5_15_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") - - add_executable(CV5_15_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CV5_15/CV5_15_example.cpp" ${DEVICE_SOURCES}) - target_link_libraries(CV5_15_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") - target_compile_definitions(CV5_15_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") + add_executable(CX5_GX5_45_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CX5_GX5_45/CX5_GX5_45_example.cpp" ${DEVICE_SOURCES}) + target_link_libraries(CX5_GX5_45_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") + target_compile_definitions(CX5_GX5_45_Example PUBLIC "${MIP_EXAMPLE_DEFS}") + add_executable(CX5_GX5_CV5_15_25_Example "${EXAMPLE_SOURCES}" "${EXAMPLE_DIR}/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp" ${DEVICE_SOURCES}) + target_link_libraries(CX5_GX5_CV5_15_25_Example mip "${SERIAL_LIB}" "${SOCKET_LIB}") + target_compile_definitions(CX5_GX5_CV5_15_25_Example PUBLIC "${SERIAL_DEFS}" "${TCP_DEFS}") endif() @@ -87,16 +78,10 @@ if(MIP_USE_SERIAL) add_executable(CV7_ExampleC "${EXAMPLE_DIR}/CV7/CV7_example.c") target_link_libraries(CV7_ExampleC mip) - add_executable(GX5_45_ExampleC "${EXAMPLE_DIR}/GX5_45/GX5_45_example.c") - target_link_libraries(GX5_45_ExampleC mip) - - add_executable(GX5_15_ExampleC "${EXAMPLE_DIR}/GX5_15/GX5_15_example.c") - target_link_libraries(GX5_15_ExampleC mip) - - add_executable(CX5_15_ExampleC "${EXAMPLE_DIR}/CX5_15/CX5_15_example.c") - target_link_libraries(CX5_15_ExampleC mip) + add_executable(CX5_GX5_45_ExampleC "${EXAMPLE_DIR}/CX5_GX5_45/CX5_GX5_45_example.c") + target_link_libraries(CX5_GX5_45_ExampleC mip) - add_executable(CV5_15_ExampleC "${EXAMPLE_DIR}/CV5_15/CV5_15_example.c") - target_link_libraries(CV5_15_ExampleC mip) + add_executable(CX5_GX5_CV5_15_25_ExampleC "${EXAMPLE_DIR}/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c") + target_link_libraries(CX5_GX5_CV5_15_25_ExampleC mip) endif() diff --git a/examples/GX5_45/GX5_45_example.c b/examples/CX5_GX5_45/CX5_GX5_45_example.c similarity index 100% rename from examples/GX5_45/GX5_45_example.c rename to examples/CX5_GX5_45/CX5_GX5_45_example.c diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/CX5_GX5_45/CX5_GX5_45_example.cpp similarity index 100% rename from examples/GX5_45/GX5_45_example.cpp rename to examples/CX5_GX5_45/CX5_GX5_45_example.cpp diff --git a/examples/CX5_15/CX5_15_example.c b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c similarity index 96% rename from examples/CX5_15/CX5_15_example.c rename to examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c index 899e5630f..0a56e71e9 100644 --- a/examples/CX5_15/CX5_15_example.c +++ b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c @@ -69,9 +69,9 @@ bool filter_state_running = false; //Required MIP interface user-defined functions -timestamp_type get_current_timestamp(); +mip_timestamp get_current_timestamp(); -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out); +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out); bool mip_interface_user_send_to_device(mip_interface* device, const uint8_t* data, size_t length); int usage(const char* argv0); @@ -258,7 +258,7 @@ int main(int argc, const char* argv[]) // bool running = true; - timestamp_type prev_print_timestamp = 0; + mip_timestamp prev_print_timestamp = 0; printf("Sensor is configured... waiting for filter to enter running mode.\n"); @@ -277,7 +277,7 @@ int main(int argc, const char* argv[]) if(filter_state_running) { - timestamp_type curr_time = get_current_timestamp(); + mip_timestamp curr_time = get_current_timestamp(); if(curr_time - prev_print_timestamp >= 1000) { @@ -301,12 +301,12 @@ int main(int argc, const char* argv[]) // MIP Interface Time Access Function //////////////////////////////////////////////////////////////////////////////// -timestamp_type get_current_timestamp() +mip_timestamp get_current_timestamp() { clock_t curr_time; curr_time = clock(); - return (timestamp_type)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); + return (mip_timestamp)((double)(curr_time - start_time)/(double)CLOCKS_PER_SEC*1000.0); } @@ -314,7 +314,7 @@ timestamp_type get_current_timestamp() // MIP Interface User Recv Data Function //////////////////////////////////////////////////////////////////////////////// -bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, timeout_type wait_time, size_t* out_length, timestamp_type* timestamp_out) +bool mip_interface_user_recv_from_device(mip_interface* device, uint8_t* buffer, size_t max_length, mip_timeout wait_time, size_t* out_length, mip_timestamp* timestamp_out) { *timestamp_out = get_current_timestamp(); return serial_port_read(&device_port, buffer, max_length, wait_time, out_length); diff --git a/examples/CX5_15/CX5_15_example.cpp b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp similarity index 99% rename from examples/CX5_15/CX5_15_example.cpp rename to examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp index 4d92fcd3e..947007b2e 100644 --- a/examples/CX5_15/CX5_15_example.cpp +++ b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp @@ -125,7 +125,7 @@ int main(int argc, const char* argv[]) // Reset the timeout device->setBaseReplyTimeout(old_mip_sdk_timeout); - printf("Gyro bias captured with sampling time: %d, and gyro bias captured as: %f %f %f.\n", sampling_time, gyro_bias[0], gyro_bias[1], gyro_bias[3]); + printf("Gyro bias captured with sampling time: %d, and gyro bias captured as: %f %f %f.\n", sampling_time, gyro_bias[0], gyro_bias[1], gyro_bias[2]); // From ee8157b0c3624525ffc9a967fa5dd89702a0afc2 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 13 May 2024 14:54:03 -0400 Subject: [PATCH 240/252] Remove digit separators causing compilation errors in some cases. --- examples/CV7_INS/CV7_INS_simple_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index eda446d45..b094e0e75 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -235,7 +235,7 @@ int main(int argc, const char* argv[]) commands_aiding::Time external_measurement_time; external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; external_measurement_time.reserved = 1; - external_measurement_time.nanoseconds = current_timestamp * uint64_t(1'000'000); + external_measurement_time.nanoseconds = current_timestamp * uint64_t(1000000); // External heading command float external_heading = 0.0; From 7dcab0c0f29a251c01941556200ab303d634906d Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 13 May 2024 16:20:36 -0400 Subject: [PATCH 241/252] Resolved most conversations --- examples/CV7/CV7_example.cpp | 2 +- examples/CV7_INS/CV7_INS_simple_example.cpp | 4 ++-- examples/GQ7/GQ7_example.cpp | 4 ++-- examples/GX5_45/GX5_45_example.cpp | 4 ++-- examples/example_utils.c | 25 ++++++++++++--------- examples/example_utils.cpp | 25 +++++++++------------ examples/example_utils.h | 2 +- examples/example_utils.hpp | 5 +---- 8 files changed, 34 insertions(+), 37 deletions(-) diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index 0c5795660..f7d1f47e8 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -351,7 +351,7 @@ void exit_gracefully(const char *message) printf("%s\n", message); #ifdef _WIN32 - std::cout << "Press ENTER to exit..." << std::endl; + printf("Press ENTER to exit...\n"); int dummy = getchar(); #endif diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 8bb55948f..658fe0dcc 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -213,7 +213,7 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to initialize (FULL_NAV)...\n"); - auto current_state = std::string{""}; + std::string current_state = std::string{""}; while(running) { device->update(); @@ -335,7 +335,7 @@ void exit_gracefully(const char *message) printf("%s\n", message); #ifdef _WIN32 - std::cout << "Press ENTER to exit..." << std::endl; + printf("Press ENTER to exit...\n"); int dummy = getchar(); #endif diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index c78702c63..fa4277479 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -298,7 +298,7 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter Full Navigation mode (FULL_NAV).\n"); - auto current_state = std::string{""}; + std::string current_state = std::string{""}; while(running) { device->update(); displayFilterState(filter_status.filter_state, current_state); @@ -367,7 +367,7 @@ void exit_gracefully(const char *message) printf("%s\n", message); #ifdef _WIN32 - std::cout << "Press ENTER to exit..." << std::endl; + printf("Press ENTER to exit...\n"); int dummy = getchar(); #endif diff --git a/examples/GX5_45/GX5_45_example.cpp b/examples/GX5_45/GX5_45_example.cpp index 552b23125..b131e81b6 100644 --- a/examples/GX5_45/GX5_45_example.cpp +++ b/examples/GX5_45/GX5_45_example.cpp @@ -268,7 +268,7 @@ int main(int argc, const char* argv[]) printf("Sensor is configured... waiting for filter to enter running mode (GX5_RUN_SOLUTION_VALID).\n"); - auto current_state = std::string{""}; + std::string current_state = std::string{""}; while(running) { device->update(); @@ -333,7 +333,7 @@ void exit_gracefully(const char *message) printf("%s\n", message); #ifdef _WIN32 - std::cout << "Press ENTER to exit..." << std::endl; + printf("Press ENTER to exit...\n"); int dummy = getchar(); #endif diff --git a/examples/example_utils.c b/examples/example_utils.c index 421bcbc2f..ffe166c4e 100644 --- a/examples/example_utils.c +++ b/examples/example_utils.c @@ -4,19 +4,24 @@ void displayFilterState(const mip_filter_mode filter_state, char **current_state, bool isFiveSeries) { char *read_state = ""; - if (filter_state == MIP_FILTER_MODE_INIT) { - read_state = false ? "GX5_INIT (1)" : "INIT (1)"; - } else if (filter_state == MIP_FILTER_MODE_VERT_GYRO) { - read_state = false ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; - } else if (filter_state == MIP_FILTER_MODE_AHRS) { - read_state = false ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; - } else if (filter_state == MIP_FILTER_MODE_FULL_NAV) { + if (filter_state == MIP_FILTER_MODE_INIT) + read_state = isFiveSeries ? "GX5_INIT (1)" : "INIT (1)"; + else if (filter_state == MIP_FILTER_MODE_VERT_GYRO) + read_state = isFiveSeries ? "GX5_RUN_SOLUTION_VALID (2)" : "VERT_GYRO (2)"; + else if (filter_state == MIP_FILTER_MODE_AHRS) + read_state = isFiveSeries ? "GX5_RUN_SOLUTION_ERROR (3)" : "AHRS (3)"; + else if (filter_state == MIP_FILTER_MODE_FULL_NAV) read_state = "FULL_NAV (4)"; - } else { + else read_state = "STARTUP (0)"; - } + + // switch (filter_state) + // { + // case MIP_FILTER_MODE_INIT: + // } - if (strcmp(read_state, *current_state) != 0) { + if (strcmp(read_state, *current_state) != 0) + { printf("FILTER STATE: %s\n", read_state); *current_state = read_state; } diff --git a/examples/example_utils.cpp b/examples/example_utils.cpp index f0d00df15..0e47a84fe 100644 --- a/examples/example_utils.cpp +++ b/examples/example_utils.cpp @@ -136,24 +136,18 @@ int printCommonUsage(const char* argv[]) return 1; } -void displayFilterState( - const mip::data_filter::FilterMode &filter_state, - std::string ¤t_state, - bool isFiveSeries -) { +void displayFilterState(const mip::data_filter::FilterMode &filterState, std::string ¤tState, bool isFiveSeries) { std::string read_state = ""; - switch (filter_state) { + switch (filterState) + { case mip::data_filter::FilterMode::INIT: - read_state = isFiveSeries ? "GX5_INIT" : "INIT" - + std::string(" (1)"); + read_state = (isFiveSeries ? "GX5_INIT" : "INIT") + std::string(" (1)"); break; case mip::data_filter::FilterMode::VERT_GYRO: - read_state = isFiveSeries ? "GX5_RUN_SOLUTION_VALID" : "VERT_GYRO" - + std::string(" (2)"); + read_state = (isFiveSeries ? "GX5_RUN_SOLUTION_VALID" : "VERT_GYRO") + std::string(" (2)"); break; case mip::data_filter::FilterMode::AHRS: - read_state = isFiveSeries ? "GX5_RUN_SOLUTION_ERROR" : "AHRS" - + std::string(" (3)"); + read_state = (isFiveSeries ? "GX5_RUN_SOLUTION_ERROR" : "AHRS") + std::string(" (3)"); break; case mip::data_filter::FilterMode::FULL_NAV: read_state = "FULL_NAV (4)"; @@ -163,8 +157,9 @@ void displayFilterState( break; } - if (read_state != current_state) { - std::cout << "Filter state: " << read_state << std::endl; - current_state = read_state; + if (read_state != currentState) + { + printf("Filter state: %s\n", read_state.data()); + currentState = read_state; } } diff --git a/examples/example_utils.h b/examples/example_utils.h index 157d5699e..34668af1e 100644 --- a/examples/example_utils.h +++ b/examples/example_utils.h @@ -2,4 +2,4 @@ // Displays current filter state for the connected device if it has changed. -void displayFilterState(const mip_filter_mode filter_state, char **current_state, bool isFiveSeries); +void displayFilterState(const mip_filter_mode filterState, char **currentState, bool isFiveSeries); diff --git a/examples/example_utils.hpp b/examples/example_utils.hpp index 68a670bd3..7ede3b40c 100644 --- a/examples/example_utils.hpp +++ b/examples/example_utils.hpp @@ -37,7 +37,4 @@ std::unique_ptr handleCommonArgs(int argc, const char* argv[], int int printCommonUsage(const char* argv[]); /// Displays current filter state for the connected device if it has changed. -void displayFilterState( - const mip::data_filter::FilterMode &filter_status, - std::string ¤t_state, - bool isFiveSeries = false); +void displayFilterState(const mip::data_filter::FilterMode &filterState, std::string ¤tState, bool isFiveSeries = false); From 65c8ad0da282319aed8b680770d7739f849f5966 Mon Sep 17 00:00:00 2001 From: Alex Farrell Date: Mon, 13 May 2024 16:22:54 -0400 Subject: [PATCH 242/252] Removed switch statement comments --- examples/example_utils.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/examples/example_utils.c b/examples/example_utils.c index ffe166c4e..d8bbae36d 100644 --- a/examples/example_utils.c +++ b/examples/example_utils.c @@ -14,11 +14,6 @@ void displayFilterState(const mip_filter_mode filter_state, char **current_state read_state = "FULL_NAV (4)"; else read_state = "STARTUP (0)"; - - // switch (filter_state) - // { - // case MIP_FILTER_MODE_INIT: - // } if (strcmp(read_state, *current_state) != 0) { From eb7211f5da78f1a8b9143fc95f4c4f78a03962ea Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 13 May 2024 16:38:01 -0400 Subject: [PATCH 243/252] Update changelog. --- CHANGELOG.md | 63 +++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 62 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 01aaf9dd7..dad187bc7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,68 @@ REMOVED - A function/class has been removed. Forthcoming ----------- -* TBD +### New Features +### Interface Changes +### Bug Fixes + + +v2.0.0 +------ + +### New features +* CV7-INS support +* Logging capability +* Diagnostic counters for debugging +* User-defined values in CmdResult +* Additional metadata in C++ command structs +* `mip::PacketBuf` - implements `mip::PacketRef` and includes a data buffer +* Extra helper utilities + * CompositeResult - stores a std::vector of CmdResults and associated command descriptors + * Index - Helps prevent off-by-one errors when using 1-based MIP and 0-based arrays + * RecordingConnection - Intermediate connection which logs sent/received data to files + +### Interface Changes + +#### Renamed +* `timestamp_type` → `mip_timestamp` \[C] +* `timeout_type` → `mip_timeout` \[C] +* `renaming_count` → `int` (typedef removed) \[C] +* `packet_length` → `uint_least16_t` (typedef removed) \[C] +* `MIP_CMD_DESC_BASE_GPS_TIME_BROADCAST_NEW` → `MIP_CMD_DESC_BASE_GPS_TIME_UPDATE` \[C] +* `CMD_GPS_TIME_BROADCAST_NEW` → `CMD_GPS_TIME_UPDATE` \[C++] +* `mip::Packet` → `mip::PacketRef` \[C++] +* The following definitions have been renamed in CMake: + * `WITH_SERIAL` → `MIP_USE_SERIAL` + * `WITH_TCP` → `MIP_USE_TCP` + +#### Changed +* The following 2 extern functions have been changed to callbacks to better support shared libraries: + Supply your callbacks to `mip_interface_init`. + * `mip_interface_user_send_to_device` + * `mip_interface_user_recv_from_device` +* The interface for certain commands from files in `mip/definitions` have been modified: + * Vectors and Quaternions are now explicitly-defined types. + * In C, these are typedef'd to arrays. + * In C++, these are simple structs offering conversion to/from plain arrays and some + other helpful features such as `fill`. + * Command/response structs (e.g. `mip::commands_base::DeviceDescriptors::Response`) now have + arrays embedded rather than pointers. This change simplifies user code and reduces bugs due + to dangling pointers. +* The C standard in CMake has been switched to `C11` from `C99` to reflect actual usage and fix + some warnings. +* `serial_port_init` must now be called before any of the other serial port functions. + +### Bug Fixes +* Use `NULL` payload for `mip_field_from_header_ptr` if input field isn't long enough +* Properly de-queue pending commands in `mip_interface_wait_for_reply` if `mip_interface_update` fails. +* `mip_packet_cancel_last_field` now computes the new header length correctly +* Serial Ports + * Serial ports now close themselves properly if an error occurs while reading from the port. This happens when the device is connected via USB and is unplugged. + * The port is now opened exclusively in Posix (Linux, Mac) systems + * The port is now closed properly if setup fails during `serial_port_open`. + * Removed `handle->is_open` member to avoid it becoming out of date. +* TCP connections are now supported on Windows. + v1.0.0 ------ From b94ec7843e57ea399521cd0c8e71fa25c4488465 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 13 May 2024 16:58:44 -0400 Subject: [PATCH 244/252] Fix missing includes. --- examples/example_utils.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/example_utils.c b/examples/example_utils.c index d8bbae36d..8713585f7 100644 --- a/examples/example_utils.c +++ b/examples/example_utils.c @@ -1,5 +1,7 @@ #include "example_utils.h" +#include +#include void displayFilterState(const mip_filter_mode filter_state, char **current_state, bool isFiveSeries) { From a01e734b99b35335c281f016639ea63f56f58776 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 13 May 2024 17:39:04 -0400 Subject: [PATCH 245/252] Update example headers. --- examples/CV7/CV7_example.c | 3 +- examples/CV7/CV7_example.cpp | 2 +- examples/CV7_INS/CV7_INS_simple_example.cpp | 2 +- examples/CX5_GX5_45/CX5_GX5_45_example.c | 6 +- examples/CX5_GX5_45/CX5_GX5_45_example.cpp | 6 +- .../CX5_GX5_CV5_15_25_example.c | 6 +- .../CX5_GX5_CV5_15_25_example.cpp | 6 +- examples/GQ7/GQ7_example.c | 2 +- examples/GQ7/GQ7_example.cpp | 2 +- examples/device_info.cpp | 18 +- examples/ping.cpp | 286 ------------------ examples/threading.cpp | 17 ++ 12 files changed, 51 insertions(+), 305 deletions(-) delete mode 100644 examples/ping.cpp diff --git a/examples/CV7/CV7_example.c b/examples/CV7/CV7_example.c index 637fed1c9..b84c7167b 100644 --- a/examples/CV7/CV7_example.c +++ b/examples/CV7/CV7_example.c @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. @@ -31,7 +31,6 @@ #include #include #include -#include #include #include "example_utils.h" diff --git a/examples/CV7/CV7_example.cpp b/examples/CV7/CV7_example.cpp index f7d1f47e8..4dbb7a54c 100644 --- a/examples/CV7/CV7_example.cpp +++ b/examples/CV7/CV7_example.cpp @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index c0d6f5652..fd2cc36f1 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -27,8 +27,8 @@ //////////////////////////////////////////////////////////////////////////////// #include "mip/mip_all.hpp" -#include #include "example_utils.hpp" +#include using namespace mip; diff --git a/examples/CX5_GX5_45/CX5_GX5_45_example.c b/examples/CX5_GX5_45/CX5_GX5_45_example.c index e0c3b2cc8..f1c8af86d 100644 --- a/examples/CX5_GX5_45/CX5_GX5_45_example.c +++ b/examples/CX5_GX5_45/CX5_GX5_45_example.c @@ -1,9 +1,9 @@ ///////////////////////////////////////////////////////////////////////////// // -// GX5_45_Example.c +// CX5_GX5_45_Example.c // -// C Example set-up program for the GX5-45 +// C Example set-up program for the CX5-45 and GX5-45 // // This example shows a typical setup for the GX5-45 sensor in a wheeled-vehicle application using using C. // It is not an exhaustive example of all GX5-45 settings. @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/CX5_GX5_45/CX5_GX5_45_example.cpp b/examples/CX5_GX5_45/CX5_GX5_45_example.cpp index b131e81b6..484444522 100644 --- a/examples/CX5_GX5_45/CX5_GX5_45_example.cpp +++ b/examples/CX5_GX5_45/CX5_GX5_45_example.cpp @@ -1,9 +1,9 @@ ///////////////////////////////////////////////////////////////////////////// // -// GX5_45_Example.cpp +// CX5_GX5_45_Example.cpp // -// C++ Example set-up program for the GX5-45 +// C++ Example set-up program for the CX5-45 and GX5-45 // // This example shows a typical setup for the GX5-45 sensor in a wheeled-vehicle application using using C++. // It is not an exhaustive example of all GX5-45 settings. @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c index 9b1cb8317..4721360cc 100644 --- a/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c +++ b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.c @@ -1,9 +1,9 @@ ///////////////////////////////////////////////////////////////////////////// // -// CX5_15_Example.c +// CX5_GX5_CV5_15_25_Example.c // -// C Example set-up program for the CX5-15 +// C Example set-up program for the CX5-15, CX5-25, GX5-15, GX5-25, CV5-15, and CV5-25. // // This example shows a typical setup for the CX5-15 sensor in a wheeled-vehicle application using using C. // It is not an exhaustive example of all CX5-15 settings. @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp index 947007b2e..74bd0b228 100644 --- a/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp +++ b/examples/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp @@ -1,9 +1,9 @@ ///////////////////////////////////////////////////////////////////////////// // -// CX5_15_Example.cpp +// CX5_GX5_CV5_15_25_Example.cpp // -// C++ Example set-up program for the CX5-15 +// C++ Example set-up program for the CX5-15, CX5-25, GX5-15, GX5-25, CV5-15, and CV5-25. // // This example shows a typical setup for the CX5-15 sensor in a wheeled-vehicle application using using C++. // It is not an exhaustive example of all CX5-15 settings. @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/GQ7/GQ7_example.c b/examples/GQ7/GQ7_example.c index bb9bfe0a4..4e575d702 100644 --- a/examples/GQ7/GQ7_example.c +++ b/examples/GQ7/GQ7_example.c @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/GQ7/GQ7_example.cpp b/examples/GQ7/GQ7_example.cpp index fa4277479..0b2f1ba8b 100644 --- a/examples/GQ7/GQ7_example.cpp +++ b/examples/GQ7/GQ7_example.cpp @@ -15,7 +15,7 @@ //! //! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING //! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER -//! FOR THEM TO SAVE TIME. AS A RESULT, PARKER MICROSTRAIN SHALL NOT BE HELD +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD //! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY //! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS //! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. diff --git a/examples/device_info.cpp b/examples/device_info.cpp index 740d5656c..7c9c454a7 100644 --- a/examples/device_info.cpp +++ b/examples/device_info.cpp @@ -1,8 +1,24 @@ +///////////////////////////////////////////////////////////////////////////// +// +// device_info.cpp +// +// C++ example program to print device information from any mip-enabled MicroStrain device. +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + #include "example_utils.hpp" #include -#include #include #include diff --git a/examples/ping.cpp b/examples/ping.cpp deleted file mode 100644 index f987dc735..000000000 --- a/examples/ping.cpp +++ /dev/null @@ -1,286 +0,0 @@ - -#include "serial_mip_device.hpp" - -#include -#include -#include -#include - -#include -#include - -namespace mip -{ - namespace C { - CmdResult mipcmd_base_getDeviceInfo(struct C::MipInterfaceState* device, const struct MipCmd_Base_GetDeviceInfo* cmd, struct MipCmd_Base_GetDeviceInfo_Response* response); - CmdResult mipcmd_base_getDeviceInfo(struct C::MipInterfaceState* device, struct MipBaseDeviceInfo* info); - } -} - -int usage(const char* argv[]) -{ - fprintf(stderr, "Usage: %s \n", argv[0]); - return 1; -} - - -int main(int argc, const char* argv[]) -{ - uint32_t baud = 0; - - if( argc == 1 ) - { - printf("Available serial ports:\n"); - std::vector ports = serial::list_ports(); - - for(const serial::PortInfo& port : ports) - { - printf(" %s %s %s\n", port.port.c_str(), port.description.c_str(), port.hardware_id.c_str()); - } - return 0; - } - else if( argc == 3 ) - { - baud = std::strtoul(argv[2], nullptr, 10); - if( baud == 0 ) - { - fprintf(stderr, "Error: invalid baud rate '%s'\n", argv[2]); - return 1; - } - } - else - { - return usage(argv); - } - - try - { -#define METHOD 8 - -#if METHOD == 1 || METHOD == 2 || METHOD == 3 - serial::Serial port(argv[1], baud, serial::Timeout::simpleTimeout(10)); - - uint8_t buffer[PACKET_LENGTH_MAX]; - - mip::Packet packet(buffer, sizeof(buffer), mip::MIP_BASE_COMMAND_DESC_SET); - - uint8_t* payload; - mip::RemainingCount available = packet.allocField(mip::MIP_CMD_DESC_BASE_PING, 0, &payload); - - #if METHOD == 1 - mip::MipCmd_Base_GetDeviceInfo info; - size_t used = mip::insert_MipCmd_Base_GetDeviceInfo(payload, available, 0, &info); - #elif METHOD == 2 - mip::MipCmd_Base_GetDeviceInfo info; - size_t used = mip::insert(payload, available, 0, info); - #elif METHOD == 3 - size_t used = mip::insert_MipCmd_Base_GetDeviceInfo_args(payload, available, 0); - #endif - - // Skip error checking as this field will always fit in this buffer. - packet.reallocLastField(payload, used); - - packet.finalize(); - - printf("Send bytes ["); - for(size_t i=0; i= 5 - MipDevice device(argv[1], baud); - - mip::MipCmd_Base_GetDeviceInfo cmd; - mip::MipCmd_Base_GetDeviceInfo_Response response; - - #if METHOD == 5 - mip::CmdResult result = exec_MipCmd_Base_GetDeviceInfo(&device, &cmd, &response); - #elif METHOD == 6 - mip::CmdResult result = mipcmd_base_getDeviceInfo(&device, &response.device_info); - #elif METHOD == 7 - mip::CmdResult result = mip::runCommand(&device, cmd, response); - #elif METHOD == 8 - mip::DeviceInterface* device2 = &device; - mip::C::MipInterfaceState* device3 = device2; - mip::MipInterfaceState* device4 = device3; - static_assert(std::is_same::value, "Not the same"); - mip::CmdResult result = mip::get_device_information(device4, &response.device_info); - #endif - - if( result == mip::MIP_ACK_OK ) - { - printf("Success:\n"); - - auto print_info = [](const char* name, const char info[16]) - { - char msg[17] = {0}; - std::strncpy(msg, info, 16); - printf(" %s%s\n", name, msg); - }; - - print_info("Model name: ", response.device_info.model_name); - print_info("Model number: ", response.device_info.model_number); - print_info("Serial Number: ", response.device_info.serial_number); - print_info("Device Options: ", response.device_info.device_options); - print_info("Lot Number: ", response.device_info.lot_number); - - printf( " Firmware version: %d.%d.%d\n\n", - (response.device_info.firmware_version / 1000), - (response.device_info.firmware_version / 100) % 10, - (response.device_info.firmware_version / 1) % 100 - ); - } - else - { - printf("Error: command completed with NACK: %s (%d)\n", mip::MipCmdResult_toString(result), result); - } - -#endif - } - catch(const std::exception& ex) - { - fprintf(stderr, "Could not open serial port: %s", ex.what()); - return 1; - } - - return 0; -} -// -// namespace mip -// { -// namespace C -// { -// -// CmdResult mipcmd_base_getDeviceInfo(struct C::MipInterfaceState* device, struct MipBaseDeviceInfo* info) -// { -// uint8_t payload[MIP_FIELD_LENGTH_MAX]; -// -// size_t payloadLength = insert_MipCmd_Base_GetDeviceInfo(payload, sizeof(payload), 0, NULL); -// assert(payloadLength <= sizeof(payload)); -// -// uint8_t responseLength; -// CmdResult result = C::MipInterface_runCommandWithResponse(device, MIP_BASE_COMMAND_DESC_SET, MIP_CMD_DESC_BASE_GET_DEVICE_INFO, payload, payloadLength, MIP_REPLY_DESC_BASE_DEVICE_INFO, payload, &responseLength); -// if( result == MIP_ACK_OK ) -// { -// size_t used = extract_MipBaseDeviceInfo(payload, responseLength, 0, info); -// if( used != responseLength ) -// result = MIP_STATUS_ERROR; -// } -// -// return result; -// } -// -// CmdResult mipcmd_base_getDeviceInfo(struct C::MipInterfaceState* device, const struct MipCmd_Base_GetDeviceInfo* cmd, struct MipCmd_Base_GetDeviceInfo_Response* response) -// { -// uint8_t buffer[PACKET_LENGTH_MAX]; -// -// struct C::Packet packet; -// MipPacket_create(&packet, buffer, sizeof(buffer), MIP_BASE_COMMAND_DESC_SET); -// -// uint8_t* payload; -// RemainingCount available = MipPacket_allocField(&packet, MIP_CMD_DESC_BASE_GET_DEVICE_INFO, 0, &payload); -// -// size_t used = insert_MipCmd_Base_GetDeviceInfo(payload, available, 0, cmd); -// assert( used <= available ); -// MipPacket_reallocLastField(&packet, payload, used); -// -// MipPacket_finalize(&packet); -// -// -// struct C::MipPendingCmd pending; -// C::MipPendingCmd_initWithResponse(&pending, MIP_BASE_COMMAND_DESC_SET, MIP_CMD_DESC_BASE_GET_DEVICE_INFO, MIP_REPLY_DESC_BASE_DEVICE_INFO, buffer, MIP_FIELD_PAYLOAD_LENGTH_MAX); -// -// C::MipCmdQueue_enqueue( C::MipInterface_cmdQueue(device), &pending ); -// -// if( !C::MipInterface_sendToDevice(device, MipPacket_pointer(&packet), MipPacket_totalLength(&packet)) ) -// { -// C::MipCmdQueue_dequeue(C::MipInterface_cmdQueue(device), &pending); -// return MIP_STATUS_ERROR; -// } -// -// CmdResult result = C::MipInterface_waitForReply(device, &pending); -// -// if( result == MIP_ACK_OK ) -// { -// size_t responseLength = C::MipPendingCmd_responseLength(&pending); -// -// used = extract_MipCmd_Base_GetDeviceInfo_Response( C::MipPendingCmd_response(&pending), responseLength, 0, response); -// -// if( used!= responseLength ) -// return MIP_STATUS_ERROR; -// } -// -// return result; -// } -// -// } -// } diff --git a/examples/threading.cpp b/examples/threading.cpp index 7d20371b7..c12cdf5c4 100644 --- a/examples/threading.cpp +++ b/examples/threading.cpp @@ -1,4 +1,21 @@ +///////////////////////////////////////////////////////////////////////////// +// +// threading.cpp +// +// C++ example program to print device information from any mip-enabled MicroStrain device. +// +//!@section LICENSE +//! +//! THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING +//! CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER +//! FOR THEM TO SAVE TIME. AS A RESULT, HBK MICROSTRAIN SHALL NOT BE HELD +//! LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY +//! CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS +//! OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +// +///////////////////////////////////////////////////////////////////////////// + #include "example_utils.hpp" #include From b68a84a339b6aea64bf98ac94cc59d0b7bc2df5d Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 15 May 2024 18:10:08 -0400 Subject: [PATCH 246/252] Update README and CHANGELOG per PR comments. --- CHANGELOG.md | 31 ++++++++++++++++------------- README.md | 47 ++++++++++++++++++++++++++++++++++---------- docs/docs.c | 17 ++++++++-------- src/mip/mip_result.h | 2 +- 4 files changed, 64 insertions(+), 33 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dad187bc7..836aaa4e8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,32 +30,35 @@ v2.0.0 ### New features * CV7-INS support -* Logging capability -* Diagnostic counters for debugging +* GV7-INS support +* Logging capability (`mip_logging.h`) +* Diagnostic counters in mip parser and mip interface for debugging (define `MIP_ENABLE_DIAGNOSTICS`) * User-defined values in CmdResult * Additional metadata in C++ command structs * `mip::PacketBuf` - implements `mip::PacketRef` and includes a data buffer * Extra helper utilities - * CompositeResult - stores a std::vector of CmdResults and associated command descriptors - * Index - Helps prevent off-by-one errors when using 1-based MIP and 0-based arrays - * RecordingConnection - Intermediate connection which logs sent/received data to files + * `CompositeResult` - stores a std::vector of CmdResults and associated command descriptors + * `Index` - Helps prevent off-by-one errors when using 1-based MIP and 0-based arrays + * `RecordingConnection` - Intermediate connection which logs sent/received data to files ### Interface Changes #### Renamed -* `timestamp_type` → `mip_timestamp` \[C] -* `timeout_type` → `mip_timeout` \[C] -* `renaming_count` → `int` (typedef removed) \[C] -* `packet_length` → `uint_least16_t` (typedef removed) \[C] -* `MIP_CMD_DESC_BASE_GPS_TIME_BROADCAST_NEW` → `MIP_CMD_DESC_BASE_GPS_TIME_UPDATE` \[C] -* `CMD_GPS_TIME_BROADCAST_NEW` → `CMD_GPS_TIME_UPDATE` \[C++] -* `mip::Packet` → `mip::PacketRef` \[C++] -* The following definitions have been renamed in CMake: +* CMake: * `WITH_SERIAL` → `MIP_USE_SERIAL` * `WITH_TCP` → `MIP_USE_TCP` +* C++ + * `mip::Packet` → `mip::PacketRef` + * `CMD_GPS_TIME_BROADCAST_NEW` → `CMD_GPS_TIME_UPDATE` +* C + * `timestamp_type` → `mip_timestamp` + * `timeout_type` → `mip_timeout` + * `renaming_count` → `int` (typedef removed) + * `packet_length` → `uint_least16_t` (typedef removed) + * `MIP_CMD_DESC_BASE_GPS_TIME_BROADCAST_NEW` → `MIP_CMD_DESC_BASE_GPS_TIME_UPDATE` #### Changed -* The following 2 extern functions have been changed to callbacks to better support shared libraries: +* The following 2 extern functions have been changed to callbacks to better support shared libraries. Supply your callbacks to `mip_interface_init`. * `mip_interface_user_send_to_device` * `mip_interface_user_recv_from_device` diff --git a/README.md b/README.md index 41c3682d8..1edc92919 100644 --- a/README.md +++ b/README.md @@ -30,16 +30,16 @@ Examples be better suited to being in the example code files themselves. Feel free to uncomment these again if you want, or remove them completely. --> -* **Get device information** [[C++](./examples/device_info.cpp)]. -* **Watch IMU** [[C](./examples/watch_imu.c) | [C++](./examples/watch_imu.cpp)]. -* **Threading** [[C++](./examples/threading.cpp)]. -* **Ping** [[C++](./examples/ping.cpp)]. -* **Product-specific examples:** - * **GQ7 setup** [[C](./examples/GQ7/GQ7_example.c), [C++](./examples/GQ7/GQ7_example.cpp)]. - * **CV7 setup** [[C](./examples/CV7/CV7_example.c), [C++](./examples/CV7/CV7_example.cpp)]. - * **GX5-45 setup** [[C](./examples/GX5_45/GX5_45_example.c), [C++](./examples/GX5_45/GX5_45_example.cpp)]. - * **CV7_INS setup** [[C++](./examples/CV7_INS/CV7_INS_simple_example.cpp)]. - * **CV7_INS with UBlox setup** [[C++](./examples/CV7_INS/CV7_INS_simple_ublox_example.cpp)]. +* Get device information [[C++](./examples/device_info.cpp)] +* Watch IMU [[C](./examples/watch_imu.c) | [C++](./examples/watch_imu.cpp)] +* Threading [[C++](./examples/threading.cpp)] +* Ping [[C++](./examples/ping.cpp)] +* Product-specific examples: + * GQ7 setup [[C](./examples/GQ7/GQ7_example.c) | [C++](./examples/GQ7/GQ7_example.cpp)] + * CV7 setup [[C](./examples/CV7/CV7_example.c) | [C++](./examples/CV7/CV7_example.cpp)] + * GX5-45 setup [[C](./examples/GX5_45/GX5_45_example.c) | [C++](./examples/GX5_45/GX5_45_example.cpp)] + * CV7_INS setup [[C++](./examples/CV7_INS/CV7_INS_simple_example.cpp)] + * CV7_INS with UBlox setup [[C++](./examples/CV7_INS/CV7_INS_simple_ublox_example.cpp)] You'll need to enable at least one of the [communications interfaces](#communications-interfaces) in the CMake configuration to use the examples. @@ -65,6 +65,25 @@ everything. This makes it easy to tell which is being used when looking at the e The C API can be accessed directly from C++ via the `mip::C` namespace. +### Command Results + +MIP devices return an ack/nack field in response to commands to allow the user to determine if the command was +successfully executed. These fields contain a "reply code" which is defined by the MIP protocol. This library +additionally defines several "status codes" for situations where an ack/nack field is not applicable (i.e. if +the device doesn't respond to the command, if the command couldn't be transmitted, etc). + +See the documentation page for [Command Results](https://lord-microstrain.github.io/mip_sdk_documentation/latest/command_results.html) for details. + +### Timestamps + +In order to implement command timeouts and provide time of arrival information, this library requires applications to +provide the time of received data. The time must be provided as an unsigned integral value with a reasonable precision, +typically milliseconds since program startup. By default the timestamp type is set to `uint64_t`, but some embedded +applications may which to change this to `uint32_t` via the `MIP_TIMESTAMP_TYPE` define. Note that wraparound is +permissible if the wraparound period is longer than twice the longest timeout used by the application. + +See the documentation page for [Timestamps](https://lord-microstrain.github.io/mip_sdk_documentation/latest/timestamps.html). + Communications Interfaces ------------------------- @@ -153,3 +172,11 @@ These options affect the compiled code interface and sizes of various structs. T MUST be consistent between compiling the MIP SDK and any other code which includes headers from the MIP SDK. (If you change them after building, make sure everything gets rebuilt properly. Normally CMake takes care of this for you). + +Known Issues +------------ + +* `suppress_ack=true` is not supported +* The commanded BIT, device settings, and capture gyro bias commands can time out unless the timeout is increased + +See the documentation page for [Known Issues](https://lord-microstrain.github.io/mip_sdk_documentation/latest/other.html#known_issues). diff --git a/docs/docs.c b/docs/docs.c index 7df721d0a..82cba0c54 100644 --- a/docs/docs.c +++ b/docs/docs.c @@ -445,11 +445,12 @@ ///@li General error (STATUS_ERROR) ///@li Timeout (STATUS_TIMEDOUT) ///@li Other statuses are used to track commands in progress -///@li User status codes can also be set (STATUS_USER) +///@li User status codes can also be set (STATUS_USER). +/// /// All of these are negative integers. /// -/// You can use mip_cmd_result_is_reply_code() / CmdResult::isReplyCode() and -/// mip_cmd_result_is_status_code() / CmdResult::isStatusCode() to distinguish +/// You can use `mip_cmd_result_is_reply_code()` / `CmdResult::isReplyCode()` and +/// `mip_cmd_result_is_status_code()` / `CmdResult::isStatusCode()` to distinguish /// between them. /// /// In C++, CmdResult is implicitly convertible to bool. ACK_OK converts to true @@ -468,20 +469,20 @@ ///@page timestamps Timestamps and Timeouts //////////////////////////////////////////////////////////////////////////////// /// -///@section Timestamp type +///@section timestamp_type Timestamp Type /// Timestamps (`mip_timestamp` / `Timestamp`) represent the local time when data was received or a packet was parsed. These timestamps /// are used to implement command timeouts and provide the user with an approximate timestamp of received data. It is not intended to be /// a precise timestamp or used for synchronization, and it generally cannot be used instead of the timestamps from the connected MIP device. /// In particular, if you limit the maximum number of packets processed per `update` call, the timestamp of some packets may be delayed. /// /// Because different applications may keep track of time differently (especially on embedded platforms), it is up to the user to provide -/// the current time whenever data is received from the device. On a PC, this might come from the poxis `time()` function or from the +/// the current time whenever data is received from the device. On a PC, this might come from the posix `clock()` function or from the /// `std::chrono` library. On ARM systems, it is often derived from the Systick timer. It should be a monotonically increasing value; -/// jumps backwards in time will cause problems. +/// jumps backwards in time (other than due to wraparound) will cause problems. /// /// By default, timestamps are `typedef`'d to `uint64_t`. Typically timestamps are in milliseconds. Embedded systems may wish to use /// `uint32_t` or even `uint16_t` instead. The value is allowed to wrap around as long as the time between wraparounds is longer than -/// twice the longest timeout needed. If higher precision is needed or wraparound can't be tolerated by your application, define it to +/// twice the longest timeout needed. If higher precision is needed or wraparound can't be tolerated by your application, define it to /// `uint64_t`. It must be a standard unsigned integer type. /// ///@section Command Timeouts @@ -493,7 +494,7 @@ /// Currently, only the C++ api offers a way to set the additional time parameter, and only when using the `runCommand` function taking /// the command structure and the `additionalTime` parameter. /// -/// The `mip_timeout` / `Timeout` typedef is an alias to the timestamp type. +/// The `mip_timeout` / `mip::Timeout` typedef is an alias to the timestamp type. /// //////////////////////////////////////////////////////////////////////////////// ///@page other Other Considerations diff --git a/src/mip/mip_result.h b/src/mip/mip_result.h index 590c7bff3..c08dce000 100644 --- a/src/mip/mip_result.h +++ b/src/mip/mip_result.h @@ -68,7 +68,7 @@ bool mip_cmd_result_is_ack(enum mip_cmd_result result); ///@brief Represents the status of a MIP command. /// /// This is the same as the mip_cmd_result enum, but with some convenient -// member functions and some operator overloads. +/// member functions and some operator overloads. /// /// CmdResult is convertible to bool, allowing code like the following: ///@code{.cpp} From 3546aeba4b790b669fed8d07774e454d689555a3 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 16 May 2024 13:53:45 -0400 Subject: [PATCH 247/252] Fix CMake error. --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index de9fdebff..c0d38f499 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,7 @@ if(WITH_INTERNAL) if(NOT DEFINED MIP_INTERNAL_DIR) set(MIP_INTERNAL_DIR "int" CACHE PATH "") endif() + file(REAL_PATH "${MIP_INTERNAL_DIR}" MIP_INTERNAL_DIR) add_subdirectory("${MIP_INTERNAL_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/mip-internal") endif() From 89d0cd1ef93743228118c722b6dbe63a7d591861 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 16 May 2024 17:19:33 -0400 Subject: [PATCH 248/252] Fix compilation errors due to renamed enum value. --- examples/CV7_INS/CV7_INS_simple_example.cpp | 8 ++++---- examples/CV7_INS/CV7_INS_simple_ublox_example.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 1ab4c70db..3f839d869 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -238,7 +238,7 @@ int main(int argc, const char* argv[]) // External heading command float external_heading = 0.0; float external_heading_uncertainty = .001; - if(commands_aiding::trueHeading(*device, external_measurement_time, external_heading_sensor_id, external_heading, external_heading_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::headingTrue(*device, external_measurement_time, external_heading_sensor_id, external_heading, external_heading_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external heading to CV7-INS\n"); // External position command @@ -246,19 +246,19 @@ int main(int argc, const char* argv[]) double longitude = -73.10628129871753; double height = 0.0; float llh_uncertainty[3] = {1.0, 1.0, 1.0}; - if(commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, latitude, longitude, height, llh_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::posLlh(*device, external_measurement_time, gnss_antenna_sensor_id, latitude, longitude, height, llh_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command float ned_velocity[3] = {0.0, 0.0, 0.0}; float ned_velocity_uncertainty[3] = {0.1, 0.1, 0.1}; - if(commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id, ned_velocity, ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::velNed(*device, external_measurement_time, gnss_antenna_sensor_id, ned_velocity, ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); // External vehicle frame velocity command float vehicle_frame_velocity[3] = {0.0, 0.0, 0.0}; float vehicle_frame_velocity_uncertainty[3] = {0.1, 0.1, 0.1}; - if(commands_aiding::vehicleFixedFrameVelocity(*device, external_measurement_time, vehicle_frame_velocity_sensor_id, vehicle_frame_velocity, vehicle_frame_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::velBodyFrame(*device, external_measurement_time, vehicle_frame_velocity_sensor_id, vehicle_frame_velocity, vehicle_frame_velocity_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external vehicle frame velocity to CV7-INS\n"); prev_measurement_update_timestamp = current_timestamp; diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index bf46cc57e..385ab429f 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -333,11 +333,11 @@ int main(int argc, const char* argv[]) } // External position command - if (commands_aiding::llhPos(*device, external_measurement_time, gnss_antenna_sensor_id, pvt_message.latitude, pvt_message.longitude, pvt_message.height_above_ellipsoid, pvt_message.llh_position_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::posLlh(*device, external_measurement_time, gnss_antenna_sensor_id, pvt_message.latitude, pvt_message.longitude, pvt_message.height_above_ellipsoid, pvt_message.llh_position_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command - if (commands_aiding::nedVel(*device, external_measurement_time, gnss_antenna_sensor_id,pvt_message.ned_velocity, pvt_message.ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::velNed(*device, external_measurement_time, gnss_antenna_sensor_id,pvt_message.ned_velocity, pvt_message.ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); prev_measurement_update_timestamp = current_timestamp; From 2a476816f1d98883606fb3c2526a75366845a6a6 Mon Sep 17 00:00:00 2001 From: microstrain-build <105877710+microstrain-sam@users.noreply.github.com> Date: Thu, 16 May 2024 21:21:33 +0000 Subject: [PATCH 249/252] Generate MIP definitions from branches/dev@56410. --- src/mip/definitions/commands_3dm.h | 4 +- src/mip/definitions/commands_3dm.hpp | 4 +- src/mip/definitions/commands_aiding.c | 130 ++++++------ src/mip/definitions/commands_aiding.cpp | 66 +++--- src/mip/definitions/commands_aiding.h | 264 ++++++++++++------------ src/mip/definitions/commands_aiding.hpp | 172 +++++++-------- src/mip/definitions/commands_filter.h | 2 +- src/mip/definitions/commands_filter.hpp | 2 +- src/mip/definitions/data_filter.h | 27 +-- src/mip/definitions/data_filter.hpp | 27 +-- 10 files changed, 352 insertions(+), 346 deletions(-) diff --git a/src/mip/definitions/commands_3dm.h b/src/mip/definitions/commands_3dm.h index b98e35c98..c2e3b7925 100644 --- a/src/mip/definitions/commands_3dm.h +++ b/src/mip/definitions/commands_3dm.h @@ -153,8 +153,8 @@ static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_RMC = 4; static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_VTG = 5; ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_HDT = 6; ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_ZDA = 7; ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PKRA = 129; ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. -static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_PKRR = 130; ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_MSRA = 129; ///< MicroStrain proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. +static const mip_nmea_message_message_id MIP_NMEA_MESSAGE_MESSAGE_ID_MSRR = 130; ///< MicroStrain proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. typedef uint8_t mip_nmea_message_talker_id; static const mip_nmea_message_talker_id MIP_NMEA_MESSAGE_TALKER_ID_IGNORED = 0; ///< Talker ID cannot be changed. diff --git a/src/mip/definitions/commands_3dm.hpp b/src/mip/definitions/commands_3dm.hpp index 28987157f..5d771bd68 100644 --- a/src/mip/definitions/commands_3dm.hpp +++ b/src/mip/definitions/commands_3dm.hpp @@ -155,8 +155,8 @@ struct NmeaMessage VTG = 5, ///< Course over Ground. Source can be the Filter or GNSS1/2 datasets. HDT = 6, ///< Heading, True. Source can be the Filter or GNSS1/2 datasets. ZDA = 7, ///< Time & Date. Source must be the GNSS1 or GNSS2 datasets. - PKRA = 129, ///< Parker proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. - PKRR = 130, ///< Parker proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. + MSRA = 129, ///< MicroStrain proprietary Euler angles. Source must be the Filter dataset. The talker ID must be set to IGNORED. + MSRR = 130, ///< MicroStrain proprietary Angular Rate/Acceleration. Source must be the Sensor dataset. The talker ID must be set to IGNORED. }; enum class TalkerID : uint8_t diff --git a/src/mip/definitions/commands_aiding.c b/src/mip/definitions/commands_aiding.c index b6ff74621..dcd292f3d 100644 --- a/src/mip/definitions/commands_aiding.c +++ b/src/mip/definitions/commands_aiding.c @@ -296,50 +296,50 @@ mip_cmd_result mip_aiding_default_frame_config(struct mip_interface* device, uin return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self) +void insert_mip_aiding_echo_control_command(mip_serializer* serializer, const mip_aiding_echo_control_command* self) { insert_mip_function_selector(serializer, self->function); if( self->function == MIP_FUNCTION_WRITE ) { - insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); + insert_mip_aiding_echo_control_command_mode(serializer, self->mode); } } -void extract_mip_aiding_aiding_echo_control_command(mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self) +void extract_mip_aiding_echo_control_command(mip_serializer* serializer, mip_aiding_echo_control_command* self) { extract_mip_function_selector(serializer, &self->function); if( self->function == MIP_FUNCTION_WRITE ) { - extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); + extract_mip_aiding_echo_control_command_mode(serializer, &self->mode); } } -void insert_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self) +void insert_mip_aiding_echo_control_response(mip_serializer* serializer, const mip_aiding_echo_control_response* self) { - insert_mip_aiding_aiding_echo_control_command_mode(serializer, self->mode); + insert_mip_aiding_echo_control_command_mode(serializer, self->mode); } -void extract_mip_aiding_aiding_echo_control_response(mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self) +void extract_mip_aiding_echo_control_response(mip_serializer* serializer, mip_aiding_echo_control_response* self) { - extract_mip_aiding_aiding_echo_control_command_mode(serializer, &self->mode); + extract_mip_aiding_echo_control_command_mode(serializer, &self->mode); } -void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self) +void insert_mip_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_echo_control_command_mode self) { insert_u8(serializer, (uint8_t)(self)); } -void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self) +void extract_mip_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_echo_control_command_mode* self) { uint8_t tmp = 0; extract_u8(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode) +mip_cmd_result mip_aiding_write_echo_control(struct mip_interface* device, mip_aiding_echo_control_command_mode mode) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -347,13 +347,13 @@ mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device insert_mip_function_selector(&serializer, MIP_FUNCTION_WRITE); - insert_mip_aiding_aiding_echo_control_command_mode(&serializer, mode); + insert_mip_aiding_echo_control_command_mode(&serializer, mode); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out) +mip_cmd_result mip_aiding_read_echo_control(struct mip_interface* device, mip_aiding_echo_control_command_mode* mode_out) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -372,14 +372,14 @@ mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_serializer_init_insertion(&deserializer, buffer, responseLength); assert(mode_out); - extract_mip_aiding_aiding_echo_control_command_mode(&deserializer, mode_out); + extract_mip_aiding_echo_control_command_mode(&deserializer, mode_out); if( mip_serializer_remaining(&deserializer) != 0 ) result = MIP_STATUS_ERROR; } return result; } -mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device) +mip_cmd_result mip_aiding_save_echo_control(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -391,7 +391,7 @@ mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device) return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device) +mip_cmd_result mip_aiding_load_echo_control(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -403,7 +403,7 @@ mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device) return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device) +mip_cmd_result mip_aiding_default_echo_control(struct mip_interface* device) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -415,7 +415,7 @@ mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* devi return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_ecef_pos_command(mip_serializer* serializer, const mip_aiding_ecef_pos_command* self) +void insert_mip_aiding_pos_ecef_command(mip_serializer* serializer, const mip_aiding_pos_ecef_command* self) { insert_mip_time(serializer, &self->time); @@ -427,10 +427,10 @@ void insert_mip_aiding_ecef_pos_command(mip_serializer* serializer, const mip_ai for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_ecef_pos_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_pos_ecef_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_ecef_pos_command(mip_serializer* serializer, mip_aiding_ecef_pos_command* self) +void extract_mip_aiding_pos_ecef_command(mip_serializer* serializer, mip_aiding_pos_ecef_command* self) { extract_mip_time(serializer, &self->time); @@ -442,22 +442,22 @@ void extract_mip_aiding_ecef_pos_command(mip_serializer* serializer, mip_aiding_ for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->uncertainty[i]); - extract_mip_aiding_ecef_pos_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_pos_ecef_command_valid_flags(serializer, &self->valid_flags); } -void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self) +void insert_mip_aiding_pos_ecef_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_pos_ecef_command_valid_flags self) { insert_u16(serializer, (uint16_t)(self)); } -void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self) +void extract_mip_aiding_pos_ecef_command_valid_flags(struct mip_serializer* serializer, mip_aiding_pos_ecef_command_valid_flags* self) { uint16_t tmp = 0; extract_u16(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_pos_ecef(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const double* position, const float* uncertainty, mip_aiding_pos_ecef_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -476,13 +476,13 @@ mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* for(unsigned int i=0; i < 3; i++) insert_float(&serializer, uncertainty[i]); - insert_mip_aiding_ecef_pos_command_valid_flags(&serializer, valid_flags); + insert_mip_aiding_pos_ecef_command_valid_flags(&serializer, valid_flags); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_POS_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_llh_pos_command(mip_serializer* serializer, const mip_aiding_llh_pos_command* self) +void insert_mip_aiding_pos_llh_command(mip_serializer* serializer, const mip_aiding_pos_llh_command* self) { insert_mip_time(serializer, &self->time); @@ -497,10 +497,10 @@ void insert_mip_aiding_llh_pos_command(mip_serializer* serializer, const mip_aid for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_llh_pos_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_pos_llh_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_llh_pos_command(mip_serializer* serializer, mip_aiding_llh_pos_command* self) +void extract_mip_aiding_pos_llh_command(mip_serializer* serializer, mip_aiding_pos_llh_command* self) { extract_mip_time(serializer, &self->time); @@ -515,22 +515,22 @@ void extract_mip_aiding_llh_pos_command(mip_serializer* serializer, mip_aiding_l for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->uncertainty[i]); - extract_mip_aiding_llh_pos_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_pos_llh_command_valid_flags(serializer, &self->valid_flags); } -void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self) +void insert_mip_aiding_pos_llh_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_pos_llh_command_valid_flags self) { insert_u16(serializer, (uint16_t)(self)); } -void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self) +void extract_mip_aiding_pos_llh_command_valid_flags(struct mip_serializer* serializer, mip_aiding_pos_llh_command_valid_flags* self) { uint16_t tmp = 0; extract_u16(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_pos_llh(struct mip_interface* device, const mip_time* time, uint8_t frame_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_pos_llh_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -551,13 +551,13 @@ mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* for(unsigned int i=0; i < 3; i++) insert_float(&serializer, uncertainty[i]); - insert_mip_aiding_llh_pos_command_valid_flags(&serializer, valid_flags); + insert_mip_aiding_pos_llh_command_valid_flags(&serializer, valid_flags); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_height_command(mip_serializer* serializer, const mip_aiding_height_command* self) +void insert_mip_aiding_height_above_ellipsoid_command(mip_serializer* serializer, const mip_aiding_height_above_ellipsoid_command* self) { insert_mip_time(serializer, &self->time); @@ -570,7 +570,7 @@ void insert_mip_aiding_height_command(mip_serializer* serializer, const mip_aidi insert_u16(serializer, self->valid_flags); } -void extract_mip_aiding_height_command(mip_serializer* serializer, mip_aiding_height_command* self) +void extract_mip_aiding_height_above_ellipsoid_command(mip_serializer* serializer, mip_aiding_height_above_ellipsoid_command* self) { extract_mip_time(serializer, &self->time); @@ -584,7 +584,7 @@ void extract_mip_aiding_height_command(mip_serializer* serializer, mip_aiding_he } -mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float height, float uncertainty, uint16_t valid_flags) +mip_cmd_result mip_aiding_height_above_ellipsoid(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float height, float uncertainty, uint16_t valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -603,9 +603,9 @@ mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* t assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEIGHT_ABOVE_ELLIPSOID, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_aiding_ecef_vel_command* self) +void insert_mip_aiding_vel_ecef_command(mip_serializer* serializer, const mip_aiding_vel_ecef_command* self) { insert_mip_time(serializer, &self->time); @@ -617,10 +617,10 @@ void insert_mip_aiding_ecef_vel_command(mip_serializer* serializer, const mip_ai for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_ecef_vel_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_vel_ecef_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_ecef_vel_command(mip_serializer* serializer, mip_aiding_ecef_vel_command* self) +void extract_mip_aiding_vel_ecef_command(mip_serializer* serializer, mip_aiding_vel_ecef_command* self) { extract_mip_time(serializer, &self->time); @@ -632,22 +632,22 @@ void extract_mip_aiding_ecef_vel_command(mip_serializer* serializer, mip_aiding_ for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->uncertainty[i]); - extract_mip_aiding_ecef_vel_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_vel_ecef_command_valid_flags(serializer, &self->valid_flags); } -void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self) +void insert_mip_aiding_vel_ecef_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vel_ecef_command_valid_flags self) { insert_u16(serializer, (uint16_t)(self)); } -void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self) +void extract_mip_aiding_vel_ecef_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vel_ecef_command_valid_flags* self) { uint16_t tmp = 0; extract_u16(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_vel_ecef(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vel_ecef_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -666,13 +666,13 @@ mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* for(unsigned int i=0; i < 3; i++) insert_float(&serializer, uncertainty[i]); - insert_mip_aiding_ecef_vel_command_valid_flags(&serializer, valid_flags); + insert_mip_aiding_vel_ecef_command_valid_flags(&serializer, valid_flags); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_ned_vel_command(mip_serializer* serializer, const mip_aiding_ned_vel_command* self) +void insert_mip_aiding_vel_ned_command(mip_serializer* serializer, const mip_aiding_vel_ned_command* self) { insert_mip_time(serializer, &self->time); @@ -684,10 +684,10 @@ void insert_mip_aiding_ned_vel_command(mip_serializer* serializer, const mip_aid for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_ned_vel_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_vel_ned_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_ned_vel_command(mip_serializer* serializer, mip_aiding_ned_vel_command* self) +void extract_mip_aiding_vel_ned_command(mip_serializer* serializer, mip_aiding_vel_ned_command* self) { extract_mip_time(serializer, &self->time); @@ -699,22 +699,22 @@ void extract_mip_aiding_ned_vel_command(mip_serializer* serializer, mip_aiding_n for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->uncertainty[i]); - extract_mip_aiding_ned_vel_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_vel_ned_command_valid_flags(serializer, &self->valid_flags); } -void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self) +void insert_mip_aiding_vel_ned_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vel_ned_command_valid_flags self) { insert_u16(serializer, (uint16_t)(self)); } -void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self) +void extract_mip_aiding_vel_ned_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vel_ned_command_valid_flags* self) { uint16_t tmp = 0; extract_u16(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_vel_ned(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vel_ned_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -733,13 +733,13 @@ mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* for(unsigned int i=0; i < 3; i++) insert_float(&serializer, uncertainty[i]); - insert_mip_aiding_ned_vel_command_valid_flags(&serializer, valid_flags); + insert_mip_aiding_vel_ned_command_valid_flags(&serializer, valid_flags); assert(mip_serializer_is_ok(&serializer)); return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_NED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command* self) +void insert_mip_aiding_vel_body_frame_command(mip_serializer* serializer, const mip_aiding_vel_body_frame_command* self) { insert_mip_time(serializer, &self->time); @@ -751,10 +751,10 @@ void insert_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* seri for(unsigned int i=0; i < 3; i++) insert_float(serializer, self->uncertainty[i]); - insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, self->valid_flags); + insert_mip_aiding_vel_body_frame_command_valid_flags(serializer, self->valid_flags); } -void extract_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command* self) +void extract_mip_aiding_vel_body_frame_command(mip_serializer* serializer, mip_aiding_vel_body_frame_command* self) { extract_mip_time(serializer, &self->time); @@ -766,22 +766,22 @@ void extract_mip_aiding_vehicle_fixed_frame_velocity_command(mip_serializer* ser for(unsigned int i=0; i < 3; i++) extract_float(serializer, &self->uncertainty[i]); - extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(serializer, &self->valid_flags); + extract_mip_aiding_vel_body_frame_command_valid_flags(serializer, &self->valid_flags); } -void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self) +void insert_mip_aiding_vel_body_frame_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vel_body_frame_command_valid_flags self) { insert_u16(serializer, (uint16_t)(self)); } -void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self) +void extract_mip_aiding_vel_body_frame_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vel_body_frame_command_valid_flags* self) { uint16_t tmp = 0; extract_u16(serializer, &tmp); *self = tmp; } -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags) +mip_cmd_result mip_aiding_vel_body_frame(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vel_body_frame_command_valid_flags valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; @@ -800,13 +800,13 @@ mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* dev for(unsigned int i=0; i < 3; i++) insert_float(&serializer, uncertainty[i]); - insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(&serializer, valid_flags); + insert_mip_aiding_vel_body_frame_command_valid_flags(&serializer, valid_flags); assert(mip_serializer_is_ok(&serializer)); - return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_ODOM, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(device, MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_BODY_FRAME, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert_mip_aiding_true_heading_command(mip_serializer* serializer, const mip_aiding_true_heading_command* self) +void insert_mip_aiding_heading_true_command(mip_serializer* serializer, const mip_aiding_heading_true_command* self) { insert_mip_time(serializer, &self->time); @@ -819,7 +819,7 @@ void insert_mip_aiding_true_heading_command(mip_serializer* serializer, const mi insert_u16(serializer, self->valid_flags); } -void extract_mip_aiding_true_heading_command(mip_serializer* serializer, mip_aiding_true_heading_command* self) +void extract_mip_aiding_heading_true_command(mip_serializer* serializer, mip_aiding_heading_true_command* self) { extract_mip_time(serializer, &self->time); @@ -833,7 +833,7 @@ void extract_mip_aiding_true_heading_command(mip_serializer* serializer, mip_aid } -mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float heading, float uncertainty, uint16_t valid_flags) +mip_cmd_result mip_aiding_heading_true(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float heading, float uncertainty, uint16_t valid_flags) { mip_serializer serializer; uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; diff --git a/src/mip/definitions/commands_aiding.cpp b/src/mip/definitions/commands_aiding.cpp index 4b9240e49..b7e1b106b 100644 --- a/src/mip/definitions/commands_aiding.cpp +++ b/src/mip/definitions/commands_aiding.cpp @@ -265,7 +265,7 @@ TypedResult defaultFrameConfig(C::mip_interface& device, uint8_t fr return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_FRAME_CONFIG, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const AidingEchoControl& self) +void insert(Serializer& serializer, const EchoControl& self) { insert(serializer, self.function); @@ -275,7 +275,7 @@ void insert(Serializer& serializer, const AidingEchoControl& self) } } -void extract(Serializer& serializer, AidingEchoControl& self) +void extract(Serializer& serializer, EchoControl& self) { extract(serializer, self.function); @@ -286,18 +286,18 @@ void extract(Serializer& serializer, AidingEchoControl& self) } } -void insert(Serializer& serializer, const AidingEchoControl::Response& self) +void insert(Serializer& serializer, const EchoControl::Response& self) { insert(serializer, self.mode); } -void extract(Serializer& serializer, AidingEchoControl::Response& self) +void extract(Serializer& serializer, EchoControl::Response& self) { extract(serializer, self.mode); } -TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode) +TypedResult writeEchoControl(C::mip_interface& device, EchoControl::Mode mode) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -309,7 +309,7 @@ TypedResult writeAidingEchoControl(C::mip_interface& device, return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut) +TypedResult readEchoControl(C::mip_interface& device, EchoControl::Mode* modeOut) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -318,7 +318,7 @@ TypedResult readAidingEchoControl(C::mip_interface& device, A assert(serializer.isOk()); uint8_t responseLength = sizeof(buffer); - TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); + TypedResult result = mip_interface_run_command_with_response(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer), REPLY_ECHO_CONTROL, buffer, &responseLength); if( result == MIP_ACK_OK ) { @@ -332,7 +332,7 @@ TypedResult readAidingEchoControl(C::mip_interface& device, A } return result; } -TypedResult saveAidingEchoControl(C::mip_interface& device) +TypedResult saveEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -342,7 +342,7 @@ TypedResult saveAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult loadAidingEchoControl(C::mip_interface& device) +TypedResult loadEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -352,7 +352,7 @@ TypedResult loadAidingEchoControl(C::mip_interface& device) return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -TypedResult defaultAidingEchoControl(C::mip_interface& device) +TypedResult defaultEchoControl(C::mip_interface& device) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -362,7 +362,7 @@ TypedResult defaultAidingEchoControl(C::mip_interface& device return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_ECHO_CONTROL, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const EcefPos& self) +void insert(Serializer& serializer, const PosEcef& self) { insert(serializer, self.time); @@ -377,7 +377,7 @@ void insert(Serializer& serializer, const EcefPos& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, EcefPos& self) +void extract(Serializer& serializer, PosEcef& self) { extract(serializer, self.time); @@ -393,7 +393,7 @@ void extract(Serializer& serializer, EcefPos& self) } -TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t frameId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags) +TypedResult posEcef(C::mip_interface& device, const Time& time, uint8_t frameId, const double* position, const float* uncertainty, PosEcef::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -416,7 +416,7 @@ TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_POS_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const LlhPos& self) +void insert(Serializer& serializer, const PosLlh& self) { insert(serializer, self.time); @@ -434,7 +434,7 @@ void insert(Serializer& serializer, const LlhPos& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, LlhPos& self) +void extract(Serializer& serializer, PosLlh& self) { extract(serializer, self.time); @@ -453,7 +453,7 @@ void extract(Serializer& serializer, LlhPos& self) } -TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t frameId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags) +TypedResult posLlh(C::mip_interface& device, const Time& time, uint8_t frameId, double latitude, double longitude, double height, const float* uncertainty, PosLlh::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -478,7 +478,7 @@ TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_POS_LLH, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const Height& self) +void insert(Serializer& serializer, const HeightAboveEllipsoid& self) { insert(serializer, self.time); @@ -491,7 +491,7 @@ void insert(Serializer& serializer, const Height& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, Height& self) +void extract(Serializer& serializer, HeightAboveEllipsoid& self) { extract(serializer, self.time); @@ -505,7 +505,7 @@ void extract(Serializer& serializer, Height& self) } -TypedResult height(C::mip_interface& device, const Time& time, uint8_t frameId, float height, float uncertainty, uint16_t validFlags) +TypedResult heightAboveEllipsoid(C::mip_interface& device, const Time& time, uint8_t frameId, float height, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -522,9 +522,9 @@ TypedResult height(C::mip_interface& device, const Time& time, uint8_t f assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEIGHT_ABS, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_HEIGHT_ABOVE_ELLIPSOID, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const EcefVel& self) +void insert(Serializer& serializer, const VelEcef& self) { insert(serializer, self.time); @@ -539,7 +539,7 @@ void insert(Serializer& serializer, const EcefVel& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, EcefVel& self) +void extract(Serializer& serializer, VelEcef& self) { extract(serializer, self.time); @@ -555,7 +555,7 @@ void extract(Serializer& serializer, EcefVel& self) } -TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags) +TypedResult velEcef(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VelEcef::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -578,7 +578,7 @@ TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEL_ECEF, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const NedVel& self) +void insert(Serializer& serializer, const VelNed& self) { insert(serializer, self.time); @@ -593,7 +593,7 @@ void insert(Serializer& serializer, const NedVel& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, NedVel& self) +void extract(Serializer& serializer, VelNed& self) { extract(serializer, self.time); @@ -609,7 +609,7 @@ void extract(Serializer& serializer, NedVel& self) } -TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags) +TypedResult velNed(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VelNed::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -632,7 +632,7 @@ TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t f return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEL_NED, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) +void insert(Serializer& serializer, const VelBodyFrame& self) { insert(serializer, self.time); @@ -647,7 +647,7 @@ void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) +void extract(Serializer& serializer, VelBodyFrame& self) { extract(serializer, self.time); @@ -663,7 +663,7 @@ void extract(Serializer& serializer, VehicleFixedFrameVelocity& self) } -TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags) +TypedResult velBodyFrame(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VelBodyFrame::ValidFlags validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); @@ -684,9 +684,9 @@ TypedResult vehicleFixedFrameVelocity(C::mip_interfac assert(serializer.isOk()); - return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEL_ODOM, buffer, (uint8_t)mip_serializer_length(&serializer)); + return mip_interface_run_command(&device, DESCRIPTOR_SET, CMD_VEL_BODY_FRAME, buffer, (uint8_t)mip_serializer_length(&serializer)); } -void insert(Serializer& serializer, const TrueHeading& self) +void insert(Serializer& serializer, const HeadingTrue& self) { insert(serializer, self.time); @@ -699,7 +699,7 @@ void insert(Serializer& serializer, const TrueHeading& self) insert(serializer, self.valid_flags); } -void extract(Serializer& serializer, TrueHeading& self) +void extract(Serializer& serializer, HeadingTrue& self) { extract(serializer, self.time); @@ -713,7 +713,7 @@ void extract(Serializer& serializer, TrueHeading& self) } -TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t frameId, float heading, float uncertainty, uint16_t validFlags) +TypedResult headingTrue(C::mip_interface& device, const Time& time, uint8_t frameId, float heading, float uncertainty, uint16_t validFlags) { uint8_t buffer[MIP_FIELD_PAYLOAD_LENGTH_MAX]; Serializer serializer(buffer, sizeof(buffer)); diff --git a/src/mip/definitions/commands_aiding.h b/src/mip/definitions/commands_aiding.h index d2ef69567..51d92b67e 100644 --- a/src/mip/definitions/commands_aiding.h +++ b/src/mip/definitions/commands_aiding.h @@ -31,29 +31,29 @@ struct mip_field; enum { - MIP_AIDING_CMD_DESC_SET = 0x13, + MIP_AIDING_CMD_DESC_SET = 0x13, - MIP_CMD_DESC_AIDING_FRAME_CONFIG = 0x01, - MIP_CMD_DESC_AIDING_LOCAL_FRAME = 0x03, - MIP_CMD_DESC_AIDING_ECHO_CONTROL = 0x1F, - MIP_CMD_DESC_AIDING_POS_LOCAL = 0x20, - MIP_CMD_DESC_AIDING_POS_ECEF = 0x21, - MIP_CMD_DESC_AIDING_POS_LLH = 0x22, - MIP_CMD_DESC_AIDING_HEIGHT_ABS = 0x23, - MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x24, - MIP_CMD_DESC_AIDING_VEL_ECEF = 0x28, - MIP_CMD_DESC_AIDING_VEL_NED = 0x29, - MIP_CMD_DESC_AIDING_VEL_ODOM = 0x2A, - MIP_CMD_DESC_AIDING_WHEELSPEED = 0x2B, - MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x31, - MIP_CMD_DESC_AIDING_MAGNETIC_FIELD = 0x32, - MIP_CMD_DESC_AIDING_PRESSURE = 0x33, - MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x38, - MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x39, - MIP_CMD_DESC_AIDING_LOCAL_ANGULAR_RATE = 0x3A, + MIP_CMD_DESC_AIDING_FRAME_CONFIG = 0x01, + MIP_CMD_DESC_AIDING_LOCAL_FRAME = 0x03, + MIP_CMD_DESC_AIDING_ECHO_CONTROL = 0x1F, + MIP_CMD_DESC_AIDING_POS_LOCAL = 0x20, + MIP_CMD_DESC_AIDING_POS_ECEF = 0x21, + MIP_CMD_DESC_AIDING_POS_LLH = 0x22, + MIP_CMD_DESC_AIDING_HEIGHT_ABOVE_ELLIPSOID = 0x23, + MIP_CMD_DESC_AIDING_HEIGHT_REL = 0x24, + MIP_CMD_DESC_AIDING_VEL_ECEF = 0x28, + MIP_CMD_DESC_AIDING_VEL_NED = 0x29, + MIP_CMD_DESC_AIDING_VEL_BODY_FRAME = 0x2A, + MIP_CMD_DESC_AIDING_WHEELSPEED = 0x2B, + MIP_CMD_DESC_AIDING_HEADING_TRUE = 0x31, + MIP_CMD_DESC_AIDING_MAGNETIC_FIELD = 0x32, + MIP_CMD_DESC_AIDING_PRESSURE = 0x33, + MIP_CMD_DESC_AIDING_DELTA_POSITION = 0x38, + MIP_CMD_DESC_AIDING_DELTA_ATTITUDE = 0x39, + MIP_CMD_DESC_AIDING_ANGULAR_RATE_LOCAL = 0x3A, - MIP_REPLY_DESC_AIDING_FRAME_CONFIG = 0x81, - MIP_REPLY_DESC_AIDING_ECHO_CONTROL = 0x9F, + MIP_REPLY_DESC_AIDING_FRAME_CONFIG = 0x81, + MIP_REPLY_DESC_AIDING_ECHO_CONTROL = 0x9F, }; //////////////////////////////////////////////////////////////////////////////// @@ -163,94 +163,94 @@ mip_cmd_result mip_aiding_default_frame_config(struct mip_interface* device, uin ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [C] +///@defgroup c_aiding_echo_control (0x13,0x1F) Echo Control [C] /// Controls command response behavior to external aiding commands /// ///@{ -typedef uint8_t mip_aiding_aiding_echo_control_command_mode; -static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_SUPPRESS_ACK = 0; ///< Suppresses the usual command ack field for aiding messages. -static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_STANDARD = 1; ///< Normal ack/nack behavior. -static const mip_aiding_aiding_echo_control_command_mode MIP_AIDING_AIDING_ECHO_CONTROL_COMMAND_MODE_RESPONSE = 2; ///< Echo the data back as a response. +typedef uint8_t mip_aiding_echo_control_command_mode; +static const mip_aiding_echo_control_command_mode MIP_AIDING_ECHO_CONTROL_COMMAND_MODE_SUPPRESS_ACK = 0; ///< Suppresses the usual command ack field for aiding messages. +static const mip_aiding_echo_control_command_mode MIP_AIDING_ECHO_CONTROL_COMMAND_MODE_STANDARD = 1; ///< Normal ack/nack behavior. +static const mip_aiding_echo_control_command_mode MIP_AIDING_ECHO_CONTROL_COMMAND_MODE_RESPONSE = 2; ///< Echo the data back as a response. -struct mip_aiding_aiding_echo_control_command +struct mip_aiding_echo_control_command { mip_function_selector function; - mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. + mip_aiding_echo_control_command_mode mode; ///< Controls data echoing. }; -typedef struct mip_aiding_aiding_echo_control_command mip_aiding_aiding_echo_control_command; -void insert_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command* self); -void extract_mip_aiding_aiding_echo_control_command(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command* self); +typedef struct mip_aiding_echo_control_command mip_aiding_echo_control_command; +void insert_mip_aiding_echo_control_command(struct mip_serializer* serializer, const mip_aiding_echo_control_command* self); +void extract_mip_aiding_echo_control_command(struct mip_serializer* serializer, mip_aiding_echo_control_command* self); -void insert_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_command_mode self); -void extract_mip_aiding_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_command_mode* self); +void insert_mip_aiding_echo_control_command_mode(struct mip_serializer* serializer, const mip_aiding_echo_control_command_mode self); +void extract_mip_aiding_echo_control_command_mode(struct mip_serializer* serializer, mip_aiding_echo_control_command_mode* self); -struct mip_aiding_aiding_echo_control_response +struct mip_aiding_echo_control_response { - mip_aiding_aiding_echo_control_command_mode mode; ///< Controls data echoing. + mip_aiding_echo_control_command_mode mode; ///< Controls data echoing. }; -typedef struct mip_aiding_aiding_echo_control_response mip_aiding_aiding_echo_control_response; -void insert_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, const mip_aiding_aiding_echo_control_response* self); -void extract_mip_aiding_aiding_echo_control_response(struct mip_serializer* serializer, mip_aiding_aiding_echo_control_response* self); +typedef struct mip_aiding_echo_control_response mip_aiding_echo_control_response; +void insert_mip_aiding_echo_control_response(struct mip_serializer* serializer, const mip_aiding_echo_control_response* self); +void extract_mip_aiding_echo_control_response(struct mip_serializer* serializer, mip_aiding_echo_control_response* self); -mip_cmd_result mip_aiding_write_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode mode); -mip_cmd_result mip_aiding_read_aiding_echo_control(struct mip_interface* device, mip_aiding_aiding_echo_control_command_mode* mode_out); -mip_cmd_result mip_aiding_save_aiding_echo_control(struct mip_interface* device); -mip_cmd_result mip_aiding_load_aiding_echo_control(struct mip_interface* device); -mip_cmd_result mip_aiding_default_aiding_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_write_echo_control(struct mip_interface* device, mip_aiding_echo_control_command_mode mode); +mip_cmd_result mip_aiding_read_echo_control(struct mip_interface* device, mip_aiding_echo_control_command_mode* mode_out); +mip_cmd_result mip_aiding_save_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_load_echo_control(struct mip_interface* device); +mip_cmd_result mip_aiding_default_echo_control(struct mip_interface* device); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_ecef_pos (0x13,0x21) Ecef Pos [C] +///@defgroup c_aiding_pos_ecef (0x13,0x21) Pos Ecef [C] /// Cartesian vector position aiding command. Coordinates are given in the WGS84 ECEF system. /// ///@{ -typedef uint16_t mip_aiding_ecef_pos_command_valid_flags; -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_X = 0x0001; ///< -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Y = 0x0002; ///< -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_Z = 0x0004; ///< -static const mip_aiding_ecef_pos_command_valid_flags MIP_AIDING_ECEF_POS_COMMAND_VALID_FLAGS_ALL = 0x0007; +typedef uint16_t mip_aiding_pos_ecef_command_valid_flags; +static const mip_aiding_pos_ecef_command_valid_flags MIP_AIDING_POS_ECEF_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_pos_ecef_command_valid_flags MIP_AIDING_POS_ECEF_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_pos_ecef_command_valid_flags MIP_AIDING_POS_ECEF_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_pos_ecef_command_valid_flags MIP_AIDING_POS_ECEF_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_pos_ecef_command_valid_flags MIP_AIDING_POS_ECEF_COMMAND_VALID_FLAGS_ALL = 0x0007; -struct mip_aiding_ecef_pos_command +struct mip_aiding_pos_ecef_command { mip_time time; ///< Timestamp of the measurement. uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3d position; ///< ECEF position [m]. mip_vector3f uncertainty; ///< ECEF position uncertainty [m]. Cannot be 0 unless the corresponding valid flags are 0. - mip_aiding_ecef_pos_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. + mip_aiding_pos_ecef_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; -typedef struct mip_aiding_ecef_pos_command mip_aiding_ecef_pos_command; -void insert_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command* self); -void extract_mip_aiding_ecef_pos_command(struct mip_serializer* serializer, mip_aiding_ecef_pos_command* self); +typedef struct mip_aiding_pos_ecef_command mip_aiding_pos_ecef_command; +void insert_mip_aiding_pos_ecef_command(struct mip_serializer* serializer, const mip_aiding_pos_ecef_command* self); +void extract_mip_aiding_pos_ecef_command(struct mip_serializer* serializer, mip_aiding_pos_ecef_command* self); -void insert_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_pos_command_valid_flags self); -void extract_mip_aiding_ecef_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_pos_command_valid_flags* self); +void insert_mip_aiding_pos_ecef_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_pos_ecef_command_valid_flags self); +void extract_mip_aiding_pos_ecef_command_valid_flags(struct mip_serializer* serializer, mip_aiding_pos_ecef_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const double* position, const float* uncertainty, mip_aiding_ecef_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_pos_ecef(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const double* position, const float* uncertainty, mip_aiding_pos_ecef_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_llh_pos (0x13,0x22) Llh Pos [C] +///@defgroup c_aiding_pos_llh (0x13,0x22) Pos Llh [C] /// Geodetic position aiding command. Coordinates are given in WGS84 geodetic latitude, longitude, and height above the ellipsoid. /// Uncertainty is given in NED coordinates, which are parallel to incremental changes in latitude, longitude, and height. /// ///@{ -typedef uint16_t mip_aiding_llh_pos_command_valid_flags; -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LATITUDE = 0x0001; ///< -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_LONGITUDE = 0x0002; ///< -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_HEIGHT = 0x0004; ///< -static const mip_aiding_llh_pos_command_valid_flags MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_ALL = 0x0007; +typedef uint16_t mip_aiding_pos_llh_command_valid_flags; +static const mip_aiding_pos_llh_command_valid_flags MIP_AIDING_POS_LLH_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_pos_llh_command_valid_flags MIP_AIDING_POS_LLH_COMMAND_VALID_FLAGS_LATITUDE = 0x0001; ///< +static const mip_aiding_pos_llh_command_valid_flags MIP_AIDING_POS_LLH_COMMAND_VALID_FLAGS_LONGITUDE = 0x0002; ///< +static const mip_aiding_pos_llh_command_valid_flags MIP_AIDING_POS_LLH_COMMAND_VALID_FLAGS_HEIGHT = 0x0004; ///< +static const mip_aiding_pos_llh_command_valid_flags MIP_AIDING_POS_LLH_COMMAND_VALID_FLAGS_ALL = 0x0007; -struct mip_aiding_llh_pos_command +struct mip_aiding_pos_llh_command { mip_time time; ///< Timestamp of the measurement. uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). @@ -258,27 +258,27 @@ struct mip_aiding_llh_pos_command double longitude; ///< [deg] double height; ///< [m] mip_vector3f uncertainty; ///< NED position uncertainty. Cannot be 0 unless the corresponding valid flags are 0. - mip_aiding_llh_pos_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. + mip_aiding_pos_llh_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; -typedef struct mip_aiding_llh_pos_command mip_aiding_llh_pos_command; -void insert_mip_aiding_llh_pos_command(struct mip_serializer* serializer, const mip_aiding_llh_pos_command* self); -void extract_mip_aiding_llh_pos_command(struct mip_serializer* serializer, mip_aiding_llh_pos_command* self); +typedef struct mip_aiding_pos_llh_command mip_aiding_pos_llh_command; +void insert_mip_aiding_pos_llh_command(struct mip_serializer* serializer, const mip_aiding_pos_llh_command* self); +void extract_mip_aiding_pos_llh_command(struct mip_serializer* serializer, mip_aiding_pos_llh_command* self); -void insert_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_llh_pos_command_valid_flags self); -void extract_mip_aiding_llh_pos_command_valid_flags(struct mip_serializer* serializer, mip_aiding_llh_pos_command_valid_flags* self); +void insert_mip_aiding_pos_llh_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_pos_llh_command_valid_flags self); +void extract_mip_aiding_pos_llh_command_valid_flags(struct mip_serializer* serializer, mip_aiding_pos_llh_command_valid_flags* self); -mip_cmd_result mip_aiding_llh_pos(struct mip_interface* device, const mip_time* time, uint8_t frame_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_llh_pos_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_pos_llh(struct mip_interface* device, const mip_time* time, uint8_t frame_id, double latitude, double longitude, double height, const float* uncertainty, mip_aiding_pos_llh_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_height (0x13,0x23) Height [C] -/// Estimated value of height. +///@defgroup c_aiding_height_above_ellipsoid (0x13,0x23) Height Above Ellipsoid [C] +/// Estimated value of the height above ellipsoid. /// ///@{ -struct mip_aiding_height_command +struct mip_aiding_height_above_ellipsoid_command { mip_time time; ///< Timestamp of the measurement. uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). @@ -287,120 +287,120 @@ struct mip_aiding_height_command uint16_t valid_flags; }; -typedef struct mip_aiding_height_command mip_aiding_height_command; -void insert_mip_aiding_height_command(struct mip_serializer* serializer, const mip_aiding_height_command* self); -void extract_mip_aiding_height_command(struct mip_serializer* serializer, mip_aiding_height_command* self); +typedef struct mip_aiding_height_above_ellipsoid_command mip_aiding_height_above_ellipsoid_command; +void insert_mip_aiding_height_above_ellipsoid_command(struct mip_serializer* serializer, const mip_aiding_height_above_ellipsoid_command* self); +void extract_mip_aiding_height_above_ellipsoid_command(struct mip_serializer* serializer, mip_aiding_height_above_ellipsoid_command* self); -mip_cmd_result mip_aiding_height(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float height, float uncertainty, uint16_t valid_flags); +mip_cmd_result mip_aiding_height_above_ellipsoid(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float height, float uncertainty, uint16_t valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_ecef_vel (0x13,0x28) Ecef Vel [C] +///@defgroup c_aiding_vel_ecef (0x13,0x28) Vel Ecef [C] /// ECEF velocity aiding command. Coordinates are given in the WGS84 ECEF frame. /// ///@{ -typedef uint16_t mip_aiding_ecef_vel_command_valid_flags; -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_X = 0x0001; ///< -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Y = 0x0002; ///< -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_Z = 0x0004; ///< -static const mip_aiding_ecef_vel_command_valid_flags MIP_AIDING_ECEF_VEL_COMMAND_VALID_FLAGS_ALL = 0x0007; +typedef uint16_t mip_aiding_vel_ecef_command_valid_flags; +static const mip_aiding_vel_ecef_command_valid_flags MIP_AIDING_VEL_ECEF_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_vel_ecef_command_valid_flags MIP_AIDING_VEL_ECEF_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_vel_ecef_command_valid_flags MIP_AIDING_VEL_ECEF_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_vel_ecef_command_valid_flags MIP_AIDING_VEL_ECEF_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_vel_ecef_command_valid_flags MIP_AIDING_VEL_ECEF_COMMAND_VALID_FLAGS_ALL = 0x0007; -struct mip_aiding_ecef_vel_command +struct mip_aiding_vel_ecef_command { mip_time time; ///< Timestamp of the measurement. uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3f velocity; ///< ECEF velocity [m/s]. mip_vector3f uncertainty; ///< ECEF velocity uncertainty [m/s]. Cannot be 0 unless the corresponding valid flags are 0. - mip_aiding_ecef_vel_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. + mip_aiding_vel_ecef_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; -typedef struct mip_aiding_ecef_vel_command mip_aiding_ecef_vel_command; -void insert_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command* self); -void extract_mip_aiding_ecef_vel_command(struct mip_serializer* serializer, mip_aiding_ecef_vel_command* self); +typedef struct mip_aiding_vel_ecef_command mip_aiding_vel_ecef_command; +void insert_mip_aiding_vel_ecef_command(struct mip_serializer* serializer, const mip_aiding_vel_ecef_command* self); +void extract_mip_aiding_vel_ecef_command(struct mip_serializer* serializer, mip_aiding_vel_ecef_command* self); -void insert_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ecef_vel_command_valid_flags self); -void extract_mip_aiding_ecef_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ecef_vel_command_valid_flags* self); +void insert_mip_aiding_vel_ecef_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vel_ecef_command_valid_flags self); +void extract_mip_aiding_vel_ecef_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vel_ecef_command_valid_flags* self); -mip_cmd_result mip_aiding_ecef_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ecef_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_vel_ecef(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vel_ecef_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_ned_vel (0x13,0x29) Ned Vel [C] +///@defgroup c_aiding_vel_ned (0x13,0x29) Vel Ned [C] /// NED velocity aiding command. Coordinates are given in the local North-East-Down frame. /// ///@{ -typedef uint16_t mip_aiding_ned_vel_command_valid_flags; -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_X = 0x0001; ///< -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Y = 0x0002; ///< -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_Z = 0x0004; ///< -static const mip_aiding_ned_vel_command_valid_flags MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_ALL = 0x0007; +typedef uint16_t mip_aiding_vel_ned_command_valid_flags; +static const mip_aiding_vel_ned_command_valid_flags MIP_AIDING_VEL_NED_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_vel_ned_command_valid_flags MIP_AIDING_VEL_NED_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_vel_ned_command_valid_flags MIP_AIDING_VEL_NED_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_vel_ned_command_valid_flags MIP_AIDING_VEL_NED_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_vel_ned_command_valid_flags MIP_AIDING_VEL_NED_COMMAND_VALID_FLAGS_ALL = 0x0007; -struct mip_aiding_ned_vel_command +struct mip_aiding_vel_ned_command { mip_time time; ///< Timestamp of the measurement. uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3f velocity; ///< NED velocity [m/s]. mip_vector3f uncertainty; ///< NED velocity uncertainty [m/s]. Cannot be 0 unless the corresponding valid flags are 0. - mip_aiding_ned_vel_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. + mip_aiding_vel_ned_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; -typedef struct mip_aiding_ned_vel_command mip_aiding_ned_vel_command; -void insert_mip_aiding_ned_vel_command(struct mip_serializer* serializer, const mip_aiding_ned_vel_command* self); -void extract_mip_aiding_ned_vel_command(struct mip_serializer* serializer, mip_aiding_ned_vel_command* self); +typedef struct mip_aiding_vel_ned_command mip_aiding_vel_ned_command; +void insert_mip_aiding_vel_ned_command(struct mip_serializer* serializer, const mip_aiding_vel_ned_command* self); +void extract_mip_aiding_vel_ned_command(struct mip_serializer* serializer, mip_aiding_vel_ned_command* self); -void insert_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_ned_vel_command_valid_flags self); -void extract_mip_aiding_ned_vel_command_valid_flags(struct mip_serializer* serializer, mip_aiding_ned_vel_command_valid_flags* self); +void insert_mip_aiding_vel_ned_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vel_ned_command_valid_flags self); +void extract_mip_aiding_vel_ned_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vel_ned_command_valid_flags* self); -mip_cmd_result mip_aiding_ned_vel(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_ned_vel_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_vel_ned(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vel_ned_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_vehicle_fixed_frame_velocity (0x13,0x2A) Vehicle Fixed Frame Velocity [C] +///@defgroup c_aiding_vel_body_frame (0x13,0x2A) Vel Body Frame [C] /// Estimate of velocity of the vehicle in the frame associated -/// with the given sensor ID. +/// with the given sensor ID, relative to the vehicle frame. /// ///@{ -typedef uint16_t mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags; -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_NONE = 0x0000; -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_X = 0x0001; ///< -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Y = 0x0002; ///< -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_Z = 0x0004; ///< -static const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags MIP_AIDING_VEHICLE_FIXED_FRAME_VELOCITY_COMMAND_VALID_FLAGS_ALL = 0x0007; +typedef uint16_t mip_aiding_vel_body_frame_command_valid_flags; +static const mip_aiding_vel_body_frame_command_valid_flags MIP_AIDING_VEL_BODY_FRAME_COMMAND_VALID_FLAGS_NONE = 0x0000; +static const mip_aiding_vel_body_frame_command_valid_flags MIP_AIDING_VEL_BODY_FRAME_COMMAND_VALID_FLAGS_X = 0x0001; ///< +static const mip_aiding_vel_body_frame_command_valid_flags MIP_AIDING_VEL_BODY_FRAME_COMMAND_VALID_FLAGS_Y = 0x0002; ///< +static const mip_aiding_vel_body_frame_command_valid_flags MIP_AIDING_VEL_BODY_FRAME_COMMAND_VALID_FLAGS_Z = 0x0004; ///< +static const mip_aiding_vel_body_frame_command_valid_flags MIP_AIDING_VEL_BODY_FRAME_COMMAND_VALID_FLAGS_ALL = 0x0007; -struct mip_aiding_vehicle_fixed_frame_velocity_command +struct mip_aiding_vel_body_frame_command { mip_time time; ///< Timestamp of the measurement. uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). mip_vector3f velocity; ///< [m/s] mip_vector3f uncertainty; ///< [m/s] 1-sigma uncertainty. Cannot be 0 unless the corresponding valid flags are 0. - mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. + mip_aiding_vel_body_frame_command_valid_flags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. }; -typedef struct mip_aiding_vehicle_fixed_frame_velocity_command mip_aiding_vehicle_fixed_frame_velocity_command; -void insert_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command* self); -void extract_mip_aiding_vehicle_fixed_frame_velocity_command(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command* self); +typedef struct mip_aiding_vel_body_frame_command mip_aiding_vel_body_frame_command; +void insert_mip_aiding_vel_body_frame_command(struct mip_serializer* serializer, const mip_aiding_vel_body_frame_command* self); +void extract_mip_aiding_vel_body_frame_command(struct mip_serializer* serializer, mip_aiding_vel_body_frame_command* self); -void insert_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags self); -void extract_mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags* self); +void insert_mip_aiding_vel_body_frame_command_valid_flags(struct mip_serializer* serializer, const mip_aiding_vel_body_frame_command_valid_flags self); +void extract_mip_aiding_vel_body_frame_command_valid_flags(struct mip_serializer* serializer, mip_aiding_vel_body_frame_command_valid_flags* self); -mip_cmd_result mip_aiding_vehicle_fixed_frame_velocity(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vehicle_fixed_frame_velocity_command_valid_flags valid_flags); +mip_cmd_result mip_aiding_vel_body_frame(struct mip_interface* device, const mip_time* time, uint8_t frame_id, const float* velocity, const float* uncertainty, mip_aiding_vel_body_frame_command_valid_flags valid_flags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup c_aiding_true_heading (0x13,0x31) True Heading [C] +///@defgroup c_aiding_heading_true (0x13,0x31) Heading True [C] /// ///@{ -struct mip_aiding_true_heading_command +struct mip_aiding_heading_true_command { mip_time time; ///< Timestamp of the measurement. uint8_t frame_id; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). @@ -409,11 +409,11 @@ struct mip_aiding_true_heading_command uint16_t valid_flags; }; -typedef struct mip_aiding_true_heading_command mip_aiding_true_heading_command; -void insert_mip_aiding_true_heading_command(struct mip_serializer* serializer, const mip_aiding_true_heading_command* self); -void extract_mip_aiding_true_heading_command(struct mip_serializer* serializer, mip_aiding_true_heading_command* self); +typedef struct mip_aiding_heading_true_command mip_aiding_heading_true_command; +void insert_mip_aiding_heading_true_command(struct mip_serializer* serializer, const mip_aiding_heading_true_command* self); +void extract_mip_aiding_heading_true_command(struct mip_serializer* serializer, mip_aiding_heading_true_command* self); -mip_cmd_result mip_aiding_true_heading(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float heading, float uncertainty, uint16_t valid_flags); +mip_cmd_result mip_aiding_heading_true(struct mip_interface* device, const mip_time* time, uint8_t frame_id, float heading, float uncertainty, uint16_t valid_flags); ///@} /// diff --git a/src/mip/definitions/commands_aiding.hpp b/src/mip/definitions/commands_aiding.hpp index 85ef6e274..3987d1c12 100644 --- a/src/mip/definitions/commands_aiding.hpp +++ b/src/mip/definitions/commands_aiding.hpp @@ -30,29 +30,29 @@ namespace commands_aiding { enum { - DESCRIPTOR_SET = 0x13, - - CMD_FRAME_CONFIG = 0x01, - CMD_LOCAL_FRAME = 0x03, - CMD_ECHO_CONTROL = 0x1F, - CMD_POS_LOCAL = 0x20, - CMD_POS_ECEF = 0x21, - CMD_POS_LLH = 0x22, - CMD_HEIGHT_ABS = 0x23, - CMD_HEIGHT_REL = 0x24, - CMD_VEL_ECEF = 0x28, - CMD_VEL_NED = 0x29, - CMD_VEL_ODOM = 0x2A, - CMD_WHEELSPEED = 0x2B, - CMD_HEADING_TRUE = 0x31, - CMD_MAGNETIC_FIELD = 0x32, - CMD_PRESSURE = 0x33, - CMD_DELTA_POSITION = 0x38, - CMD_DELTA_ATTITUDE = 0x39, - CMD_LOCAL_ANGULAR_RATE = 0x3A, - - REPLY_FRAME_CONFIG = 0x81, - REPLY_ECHO_CONTROL = 0x9F, + DESCRIPTOR_SET = 0x13, + + CMD_FRAME_CONFIG = 0x01, + CMD_LOCAL_FRAME = 0x03, + CMD_ECHO_CONTROL = 0x1F, + CMD_POS_LOCAL = 0x20, + CMD_POS_ECEF = 0x21, + CMD_POS_LLH = 0x22, + CMD_HEIGHT_ABOVE_ELLIPSOID = 0x23, + CMD_HEIGHT_REL = 0x24, + CMD_VEL_ECEF = 0x28, + CMD_VEL_NED = 0x29, + CMD_VEL_BODY_FRAME = 0x2A, + CMD_WHEELSPEED = 0x2B, + CMD_HEADING_TRUE = 0x31, + CMD_MAGNETIC_FIELD = 0x32, + CMD_PRESSURE = 0x33, + CMD_DELTA_POSITION = 0x38, + CMD_DELTA_ATTITUDE = 0x39, + CMD_ANGULAR_RATE_LOCAL = 0x3A, + + REPLY_FRAME_CONFIG = 0x81, + REPLY_ECHO_CONTROL = 0x9F, }; //////////////////////////////////////////////////////////////////////////////// @@ -203,12 +203,12 @@ TypedResult defaultFrameConfig(C::mip_interface& device, uint8_t fr ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_aiding_echo_control (0x13,0x1F) Aiding Echo Control [CPP] +///@defgroup cpp_aiding_echo_control (0x13,0x1F) Echo Control [CPP] /// Controls command response behavior to external aiding commands /// ///@{ -struct AidingEchoControl +struct EchoControl { enum class Mode : uint8_t { @@ -223,7 +223,7 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingEchoControl"; + static constexpr const char* NAME = "EchoControl"; static constexpr const char* DOC_NAME = "Aiding Command Echo Control"; static constexpr const bool HAS_FUNCTION_SELECTOR = true; @@ -245,9 +245,9 @@ struct AidingEchoControl return std::make_tuple(std::ref(mode)); } - static AidingEchoControl create_sld_all(::mip::FunctionSelector function) + static EchoControl create_sld_all(::mip::FunctionSelector function) { - AidingEchoControl cmd; + EchoControl cmd; cmd.function = function; return cmd; } @@ -257,7 +257,7 @@ struct AidingEchoControl static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::REPLY_ECHO_CONTROL; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "AidingEchoControl::Response"; + static constexpr const char* NAME = "EchoControl::Response"; static constexpr const char* DOC_NAME = "Aiding Command Echo Control Response"; static constexpr const uint32_t ECHOED_PARAMS = 0x0000; @@ -271,27 +271,27 @@ struct AidingEchoControl } }; }; -void insert(Serializer& serializer, const AidingEchoControl& self); -void extract(Serializer& serializer, AidingEchoControl& self); +void insert(Serializer& serializer, const EchoControl& self); +void extract(Serializer& serializer, EchoControl& self); -void insert(Serializer& serializer, const AidingEchoControl::Response& self); -void extract(Serializer& serializer, AidingEchoControl::Response& self); +void insert(Serializer& serializer, const EchoControl::Response& self); +void extract(Serializer& serializer, EchoControl::Response& self); -TypedResult writeAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode mode); -TypedResult readAidingEchoControl(C::mip_interface& device, AidingEchoControl::Mode* modeOut); -TypedResult saveAidingEchoControl(C::mip_interface& device); -TypedResult loadAidingEchoControl(C::mip_interface& device); -TypedResult defaultAidingEchoControl(C::mip_interface& device); +TypedResult writeEchoControl(C::mip_interface& device, EchoControl::Mode mode); +TypedResult readEchoControl(C::mip_interface& device, EchoControl::Mode* modeOut); +TypedResult saveEchoControl(C::mip_interface& device); +TypedResult loadEchoControl(C::mip_interface& device); +TypedResult defaultEchoControl(C::mip_interface& device); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_ecef_pos (0x13,0x21) Ecef Pos [CPP] +///@defgroup cpp_aiding_pos_ecef (0x13,0x21) Pos Ecef [CPP] /// Cartesian vector position aiding command. Coordinates are given in the WGS84 ECEF system. /// ///@{ -struct EcefPos +struct PosEcef { struct ValidFlags : Bitfield { @@ -333,7 +333,7 @@ struct EcefPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefPos"; + static constexpr const char* NAME = "PosEcef"; static constexpr const char* DOC_NAME = "ECEF Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; @@ -350,21 +350,21 @@ struct EcefPos } typedef void Response; }; -void insert(Serializer& serializer, const EcefPos& self); -void extract(Serializer& serializer, EcefPos& self); +void insert(Serializer& serializer, const PosEcef& self); +void extract(Serializer& serializer, PosEcef& self); -TypedResult ecefPos(C::mip_interface& device, const Time& time, uint8_t frameId, const double* position, const float* uncertainty, EcefPos::ValidFlags validFlags); +TypedResult posEcef(C::mip_interface& device, const Time& time, uint8_t frameId, const double* position, const float* uncertainty, PosEcef::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_llh_pos (0x13,0x22) Llh Pos [CPP] +///@defgroup cpp_aiding_pos_llh (0x13,0x22) Pos Llh [CPP] /// Geodetic position aiding command. Coordinates are given in WGS84 geodetic latitude, longitude, and height above the ellipsoid. /// Uncertainty is given in NED coordinates, which are parallel to incremental changes in latitude, longitude, and height. /// ///@{ -struct LlhPos +struct PosLlh { struct ValidFlags : Bitfield { @@ -408,7 +408,7 @@ struct LlhPos static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_POS_LLH; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "LlhPos"; + static constexpr const char* NAME = "PosLlh"; static constexpr const char* DOC_NAME = "LLH Position"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; @@ -425,20 +425,20 @@ struct LlhPos } typedef void Response; }; -void insert(Serializer& serializer, const LlhPos& self); -void extract(Serializer& serializer, LlhPos& self); +void insert(Serializer& serializer, const PosLlh& self); +void extract(Serializer& serializer, PosLlh& self); -TypedResult llhPos(C::mip_interface& device, const Time& time, uint8_t frameId, double latitude, double longitude, double height, const float* uncertainty, LlhPos::ValidFlags validFlags); +TypedResult posLlh(C::mip_interface& device, const Time& time, uint8_t frameId, double latitude, double longitude, double height, const float* uncertainty, PosLlh::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_height (0x13,0x23) Height [CPP] -/// Estimated value of height. +///@defgroup cpp_aiding_height_above_ellipsoid (0x13,0x23) Height Above Ellipsoid [CPP] +/// Estimated value of the height above ellipsoid. /// ///@{ -struct Height +struct HeightAboveEllipsoid { Time time; ///< Timestamp of the measurement. uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). @@ -447,10 +447,10 @@ struct Height uint16_t valid_flags = 0; static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEIGHT_ABS; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEIGHT_ABOVE_ELLIPSOID; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "Height"; - static constexpr const char* DOC_NAME = "Height"; + static constexpr const char* NAME = "HeightAboveEllipsoid"; + static constexpr const char* DOC_NAME = "Height Above Ellipsoid"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -466,20 +466,20 @@ struct Height } typedef void Response; }; -void insert(Serializer& serializer, const Height& self); -void extract(Serializer& serializer, Height& self); +void insert(Serializer& serializer, const HeightAboveEllipsoid& self); +void extract(Serializer& serializer, HeightAboveEllipsoid& self); -TypedResult height(C::mip_interface& device, const Time& time, uint8_t frameId, float height, float uncertainty, uint16_t validFlags); +TypedResult heightAboveEllipsoid(C::mip_interface& device, const Time& time, uint8_t frameId, float height, float uncertainty, uint16_t validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_ecef_vel (0x13,0x28) Ecef Vel [CPP] +///@defgroup cpp_aiding_vel_ecef (0x13,0x28) Vel Ecef [CPP] /// ECEF velocity aiding command. Coordinates are given in the WGS84 ECEF frame. /// ///@{ -struct EcefVel +struct VelEcef { struct ValidFlags : Bitfield { @@ -521,7 +521,7 @@ struct EcefVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ECEF; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "EcefVel"; + static constexpr const char* NAME = "VelEcef"; static constexpr const char* DOC_NAME = "ECEF Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; @@ -538,20 +538,20 @@ struct EcefVel } typedef void Response; }; -void insert(Serializer& serializer, const EcefVel& self); -void extract(Serializer& serializer, EcefVel& self); +void insert(Serializer& serializer, const VelEcef& self); +void extract(Serializer& serializer, VelEcef& self); -TypedResult ecefVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, EcefVel::ValidFlags validFlags); +TypedResult velEcef(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VelEcef::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_ned_vel (0x13,0x29) Ned Vel [CPP] +///@defgroup cpp_aiding_vel_ned (0x13,0x29) Vel Ned [CPP] /// NED velocity aiding command. Coordinates are given in the local North-East-Down frame. /// ///@{ -struct NedVel +struct VelNed { struct ValidFlags : Bitfield { @@ -593,7 +593,7 @@ struct NedVel static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_NED; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "NedVel"; + static constexpr const char* NAME = "VelNed"; static constexpr const char* DOC_NAME = "NED Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; @@ -610,21 +610,21 @@ struct NedVel } typedef void Response; }; -void insert(Serializer& serializer, const NedVel& self); -void extract(Serializer& serializer, NedVel& self); +void insert(Serializer& serializer, const VelNed& self); +void extract(Serializer& serializer, VelNed& self); -TypedResult nedVel(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, NedVel::ValidFlags validFlags); +TypedResult velNed(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VelNed::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_vehicle_fixed_frame_velocity (0x13,0x2A) Vehicle Fixed Frame Velocity [CPP] +///@defgroup cpp_aiding_vel_body_frame (0x13,0x2A) Vel Body Frame [CPP] /// Estimate of velocity of the vehicle in the frame associated -/// with the given sensor ID. +/// with the given sensor ID, relative to the vehicle frame. /// ///@{ -struct VehicleFixedFrameVelocity +struct VelBodyFrame { struct ValidFlags : Bitfield { @@ -664,10 +664,10 @@ struct VehicleFixedFrameVelocity ValidFlags valid_flags; ///< Valid flags. Axes with 0 will be completely ignored. static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; - static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_ODOM; + static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_VEL_BODY_FRAME; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "VehicleFixedFrameVelocity"; - static constexpr const char* DOC_NAME = "Vehicle Frame Velocity"; + static constexpr const char* NAME = "VelBodyFrame"; + static constexpr const char* DOC_NAME = "Body Frame Velocity"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; static constexpr const uint32_t COUNTER_PARAMS = 0x00000000; @@ -683,19 +683,19 @@ struct VehicleFixedFrameVelocity } typedef void Response; }; -void insert(Serializer& serializer, const VehicleFixedFrameVelocity& self); -void extract(Serializer& serializer, VehicleFixedFrameVelocity& self); +void insert(Serializer& serializer, const VelBodyFrame& self); +void extract(Serializer& serializer, VelBodyFrame& self); -TypedResult vehicleFixedFrameVelocity(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VehicleFixedFrameVelocity::ValidFlags validFlags); +TypedResult velBodyFrame(C::mip_interface& device, const Time& time, uint8_t frameId, const float* velocity, const float* uncertainty, VelBodyFrame::ValidFlags validFlags); ///@} /// //////////////////////////////////////////////////////////////////////////////// -///@defgroup cpp_aiding_true_heading (0x13,0x31) True Heading [CPP] +///@defgroup cpp_aiding_heading_true (0x13,0x31) Heading True [CPP] /// ///@{ -struct TrueHeading +struct HeadingTrue { Time time; ///< Timestamp of the measurement. uint8_t frame_id = 0; ///< Source ID for this estimate (source_id == 0 indicates this sensor, source_id > 0 indicates an external estimate). @@ -706,7 +706,7 @@ struct TrueHeading static constexpr const uint8_t DESCRIPTOR_SET = ::mip::commands_aiding::DESCRIPTOR_SET; static constexpr const uint8_t FIELD_DESCRIPTOR = ::mip::commands_aiding::CMD_HEADING_TRUE; static constexpr const CompositeDescriptor DESCRIPTOR = {DESCRIPTOR_SET, FIELD_DESCRIPTOR}; - static constexpr const char* NAME = "TrueHeading"; + static constexpr const char* NAME = "HeadingTrue"; static constexpr const char* DOC_NAME = "True Heading"; static constexpr const bool HAS_FUNCTION_SELECTOR = false; @@ -723,10 +723,10 @@ struct TrueHeading } typedef void Response; }; -void insert(Serializer& serializer, const TrueHeading& self); -void extract(Serializer& serializer, TrueHeading& self); +void insert(Serializer& serializer, const HeadingTrue& self); +void extract(Serializer& serializer, HeadingTrue& self); -TypedResult trueHeading(C::mip_interface& device, const Time& time, uint8_t frameId, float heading, float uncertainty, uint16_t validFlags); +TypedResult headingTrue(C::mip_interface& device, const Time& time, uint8_t frameId, float heading, float uncertainty, uint16_t validFlags); ///@} /// diff --git a/src/mip/definitions/commands_filter.h b/src/mip/definitions/commands_filter.h index 8fddf4cd7..1c48785ec 100644 --- a/src/mip/definitions/commands_filter.h +++ b/src/mip/definitions/commands_filter.h @@ -1765,7 +1765,7 @@ static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILT static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING = 5; ///< External heading input static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_ALTIMETER = 6; ///< External pressure altimeter input static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER = 7; ///< External magnetomer input -static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_VEHICLE_FRAME_VEL = 8; ///< External vehicle frame velocity input +static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_BODY_FRAME_VEL = 8; ///< External body frame velocity input static const mip_filter_aiding_measurement_enable_command_aiding_source MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_ALL = 65535; ///< Save/load/reset all options struct mip_filter_aiding_measurement_enable_command diff --git a/src/mip/definitions/commands_filter.hpp b/src/mip/definitions/commands_filter.hpp index 8f54608a1..3e4b5ce68 100644 --- a/src/mip/definitions/commands_filter.hpp +++ b/src/mip/definitions/commands_filter.hpp @@ -3323,7 +3323,7 @@ struct AidingMeasurementEnable EXTERNAL_HEADING = 5, ///< External heading input EXTERNAL_ALTIMETER = 6, ///< External pressure altimeter input EXTERNAL_MAGNETOMETER = 7, ///< External magnetomer input - VEHICLE_FRAME_VEL = 8, ///< External vehicle frame velocity input + BODY_FRAME_VEL = 8, ///< External body frame velocity input ALL = 65535, ///< Save/load/reset all options }; diff --git a/src/mip/definitions/data_filter.h b/src/mip/definitions/data_filter.h index 8898c974b..316db62c9 100644 --- a/src/mip/definitions/data_filter.h +++ b/src/mip/definitions/data_filter.h @@ -160,18 +160,21 @@ void insert_mip_filter_status_flags(struct mip_serializer* serializer, const mip void extract_mip_filter_status_flags(struct mip_serializer* serializer, mip_filter_status_flags* self); typedef uint8_t mip_filter_aiding_measurement_type; -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_GNSS = 1; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_DUAL_ANTENNA = 2; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING = 3; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_PRESSURE = 4; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_MAGNETOMETER = 5; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_SPEED = 6; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_POS_ECEF = 33; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_POS_LLH = 34; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_ECEF = 40; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_NED = 41; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_VEL_VEHICLE_FRAME = 42; ///< -static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING_TRUE = 49; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_GNSS = 1; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_DUAL_ANTENNA = 2; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_HEADING = 3; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_PRESSURE = 4; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_MAGNETOMETER = 5; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_SPEED = 6; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_POS_ECEF = 33; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_POS_LLH = 34; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_HEIGHT_ABOVE_ELLIPSOID = 35; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_VEL_ECEF = 40; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_VEL_NED = 41; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_VEL_BODY_FRAME = 42; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_HEADING_TRUE = 49; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_MAGNETIC_FIELD = 50; ///< +static const mip_filter_aiding_measurement_type MIP_FILTER_AIDING_MEASUREMENT_TYPE_AIDING_PRESSURE = 51; ///< void insert_mip_filter_aiding_measurement_type(struct mip_serializer* serializer, const mip_filter_aiding_measurement_type self); void extract_mip_filter_aiding_measurement_type(struct mip_serializer* serializer, mip_filter_aiding_measurement_type* self); diff --git a/src/mip/definitions/data_filter.hpp b/src/mip/definitions/data_filter.hpp index 5b2b04fc5..96ab927e2 100644 --- a/src/mip/definitions/data_filter.hpp +++ b/src/mip/definitions/data_filter.hpp @@ -229,18 +229,21 @@ struct FilterStatusFlags : Bitfield enum class FilterAidingMeasurementType : uint8_t { - GNSS = 1, ///< - DUAL_ANTENNA = 2, ///< - HEADING = 3, ///< - PRESSURE = 4, ///< - MAGNETOMETER = 5, ///< - SPEED = 6, ///< - POS_ECEF = 33, ///< - POS_LLH = 34, ///< - VEL_ECEF = 40, ///< - VEL_NED = 41, ///< - VEL_VEHICLE_FRAME = 42, ///< - HEADING_TRUE = 49, ///< + GNSS = 1, ///< + DUAL_ANTENNA = 2, ///< + HEADING = 3, ///< + PRESSURE = 4, ///< + MAGNETOMETER = 5, ///< + SPEED = 6, ///< + AIDING_POS_ECEF = 33, ///< + AIDING_POS_LLH = 34, ///< + AIDING_HEIGHT_ABOVE_ELLIPSOID = 35, ///< + AIDING_VEL_ECEF = 40, ///< + AIDING_VEL_NED = 41, ///< + AIDING_VEL_BODY_FRAME = 42, ///< + AIDING_HEADING_TRUE = 49, ///< + AIDING_MAGNETIC_FIELD = 50, ///< + AIDING_PRESSURE = 51, ///< }; struct FilterMeasurementIndicator : Bitfield From 77388b7806246f4eb80226247bb3d0b972c95bec Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 20 May 2024 14:55:01 -0400 Subject: [PATCH 250/252] Fix CV7-INS example. --- examples/CV7_INS/CV7_INS_simple_example.cpp | 42 +++++++++++++-------- 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_example.cpp b/examples/CV7_INS/CV7_INS_simple_example.cpp index 19df98a76..5a9177557 100644 --- a/examples/CV7_INS/CV7_INS_simple_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_example.cpp @@ -143,7 +143,7 @@ int main(int argc, const char* argv[]) //External bodyframe velocity reference frame // commands_aiding::FrameConfig::Rotation external_velocity_sensor_to_vehicle_frame_rotation; - external_gnss_antenna_to_vehicle_frame_rotation.euler= mip::Vector3f(0.0f, 0.0f, 1.57f); // Rotated 90 deg around yaw axis + external_velocity_sensor_to_vehicle_frame_rotation.euler= mip::Vector3f(0.0f, 0.0f, 1.57f); // Rotated 90 deg around yaw axis float external_velocity_sensor_to_vehicle_frame_translation[3] = {1.0, 0.0, 0.0}; // Sensor is translated 1 meter in X direction if(commands_aiding::writeFrameConfig(*device, vehicle_frame_velocity_sensor_id, mip::commands_aiding::FrameConfig::Format::EULER, true, external_velocity_sensor_to_vehicle_frame_translation, external_velocity_sensor_to_vehicle_frame_rotation) != CmdResult::ACK_OK) @@ -163,16 +163,26 @@ int main(int argc, const char* argv[]) const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; std::array filter_descriptors = {{ - { data_shared::DATA_GPS_TIME, filter_decimation }, - { data_filter::DATA_FILTER_STATUS, filter_decimation }, - { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, - { data_filter::DATA_POS_LLH, filter_decimation }, - { data_filter::DATA_VEL_NED, filter_decimation }, - }}; + { data_shared::DATA_GPS_TIME, filter_decimation }, + { data_filter::DATA_FILTER_STATUS, filter_decimation }, + { data_filter::DATA_ATT_EULER_ANGLES, filter_decimation }, + { data_filter::DATA_POS_LLH, filter_decimation }, + { data_filter::DATA_VEL_NED, filter_decimation }, + }}; if(commands_3dm::writeMessageFormat(*device, data_filter::DESCRIPTOR_SET, filter_descriptors.size(), filter_descriptors.data()) != CmdResult::ACK_OK) exit_gracefully("ERROR: Could not set filter message format!"); + // + //Configure the filter to accept external heading + // + + const auto initConfig = commands_filter::InitializationConfiguration::InitialConditionSource::AUTO_POS_VEL_PITCH_ROLL; + commands_filter::InitializationConfiguration::AlignmentSelector alignment; + alignment.external(true); + const Vector3f zero3({0, 0, 0}); + if(commands_filter::writeInitializationConfiguration(*device, 0, initConfig, alignment, 0, 0, 0, zero3, zero3, commands_filter::FilterReferenceFrame::LLH) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set heading source!"); // //Reset the filter (note: this is good to do after filter setup is complete) @@ -241,28 +251,28 @@ int main(int argc, const char* argv[]) // External heading command float external_heading = 0.0; - float external_heading_uncertainty = .001; - if(commands_aiding::headingTrue(*device, external_measurement_time, external_heading_sensor_id, external_heading, external_heading_uncertainty, 1) != CmdResult::ACK_OK) + float external_heading_uncertainty = 0.001; + if(commands_aiding::headingTrue(*device, external_measurement_time, external_heading_sensor_id, external_heading, external_heading_uncertainty, 0x0001) != CmdResult::ACK_OK) printf("WARNING: Failed to send external heading to CV7-INS\n"); // External position command double latitude = 44.43729093897896; // Lat/Lon for MicroStrain headquarters double longitude = -73.10628129871753; - double height = 0.0; + double height = 122.0; float llh_uncertainty[3] = {1.0, 1.0, 1.0}; - if(commands_aiding::posLlh(*device, external_measurement_time, gnss_antenna_sensor_id, latitude, longitude, height, llh_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::posLlh(*device, external_measurement_time, gnss_antenna_sensor_id, latitude, longitude, height, llh_uncertainty, 0x0007) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command float ned_velocity[3] = {0.0, 0.0, 0.0}; float ned_velocity_uncertainty[3] = {0.1, 0.1, 0.1}; - if(commands_aiding::velNed(*device, external_measurement_time, gnss_antenna_sensor_id, ned_velocity, ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::velNed(*device, external_measurement_time, gnss_antenna_sensor_id, ned_velocity, ned_velocity_uncertainty, 0x0007) != CmdResult::ACK_OK) printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); // External vehicle frame velocity command float vehicle_frame_velocity[3] = {0.0, 0.0, 0.0}; float vehicle_frame_velocity_uncertainty[3] = {0.1, 0.1, 0.1}; - if(commands_aiding::velBodyFrame(*device, external_measurement_time, vehicle_frame_velocity_sensor_id, vehicle_frame_velocity, vehicle_frame_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if(commands_aiding::velBodyFrame(*device, external_measurement_time, vehicle_frame_velocity_sensor_id, vehicle_frame_velocity, vehicle_frame_velocity_uncertainty, 0x0007) != CmdResult::ACK_OK) printf("WARNING: Failed to send external vehicle frame velocity to CV7-INS\n"); prev_measurement_update_timestamp = current_timestamp; @@ -270,9 +280,9 @@ int main(int argc, const char* argv[]) //Once in full nav, print out data at 1 Hz if((filter_status.filter_state == data_filter::FilterMode::FULL_NAV) && (elapsed_time_from_last_message_print >= 1000)) - { - printf("\n\n****Filter navigation state****\n"); - printf("TIMESTAMP: %f\n", filter_gps_time.tow); + { + printf("\n\n****Filter navigation state****\n"); + printf("TIMESTAMP: %f\n", filter_gps_time.tow); printf("ATTITUDE_EULER = [%f %f %f]\n", filter_euler_angles.roll, filter_euler_angles.pitch, filter_euler_angles.yaw); printf("LLH_POSITION = [%f %f %f]\n", filter_llh_position.latitude, filter_llh_position.longitude, filter_llh_position.ellipsoid_height); printf("NED_VELOCITY = [%f %f %f]\n", filter_ned_velocity.north, filter_ned_velocity.east, filter_ned_velocity.down); From e6d4b51aaf9bb4dd152a89b88ccaf14203cd6990 Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Mon, 20 May 2024 15:50:55 -0400 Subject: [PATCH 251/252] Fix CV7-INS Ublox example. --- .../CV7_INS/CV7_INS_simple_ublox_example.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp index 768a6c6dd..470eaa627 100644 --- a/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp +++ b/examples/CV7_INS/CV7_INS_simple_ublox_example.cpp @@ -204,6 +204,18 @@ int main(int argc, const char* argv[]) if(commands_3dm::factoryStreaming(*device, commands_3dm::FactoryStreaming::Action::MERGE, 0) != CmdResult::ACK_OK) exit_gracefully("ERROR: Could not enable factory streaming support!"); + // + //Configure the filter to use magnetometer or GNSS kinematic heading + // + + const auto initConfig = commands_filter::InitializationConfiguration::InitialConditionSource::AUTO_POS_VEL_PITCH_ROLL; + commands_filter::InitializationConfiguration::AlignmentSelector alignment; + alignment.magnetometer(true); + alignment.kinematic(true); + const Vector3f zero3({0, 0, 0}); + if(commands_filter::writeInitializationConfiguration(*device, 0, initConfig, alignment, 0, 0, 0, zero3, zero3, commands_filter::FilterReferenceFrame::LLH) != CmdResult::ACK_OK) + exit_gracefully("ERROR: Could not set heading source!"); + // //Reset the filter (note: this is good to do after filter setup is complete) // @@ -332,14 +344,15 @@ int main(int argc, const char* argv[]) { // If no PPS sync is supplied, use device time of arrival for data timestamping method external_measurement_time.timebase = commands_aiding::Time::Timebase::TIME_OF_ARRIVAL; + external_measurement_time.nanoseconds = 0; // Not used, but should be set to 0 } // External position command - if (commands_aiding::posLlh(*device, external_measurement_time, gnss_antenna_sensor_id, pvt_message.latitude, pvt_message.longitude, pvt_message.height_above_ellipsoid, pvt_message.llh_position_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::posLlh(*device, external_measurement_time, gnss_antenna_sensor_id, pvt_message.latitude, pvt_message.longitude, pvt_message.height_above_ellipsoid, pvt_message.llh_position_uncertainty, 0x0007) != CmdResult::ACK_OK) printf("WARNING: Failed to send external position to CV7-INS\n"); // External global velocity command - if (commands_aiding::velNed(*device, external_measurement_time, gnss_antenna_sensor_id,pvt_message.ned_velocity, pvt_message.ned_velocity_uncertainty, 1) != CmdResult::ACK_OK) + if (commands_aiding::velNed(*device, external_measurement_time, gnss_antenna_sensor_id,pvt_message.ned_velocity, pvt_message.ned_velocity_uncertainty, 0x0007) != CmdResult::ACK_OK) printf("WARNING: Failed to send external NED velocity to CV7-INS\n"); prev_measurement_update_timestamp = current_timestamp; From 478948e15ea70a288fb22f205fba02f5eb2cf45a Mon Sep 17 00:00:00 2001 From: microstrain-sam <105877710+microstrain-sam@users.noreply.github.com> Date: Wed, 22 May 2024 14:21:31 -0400 Subject: [PATCH 252/252] Update release.sh to include a link to the changelog on the release page. --- scripts/release.sh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/release.sh b/scripts/release.sh index 3b24216dc..be872ad39 100755 --- a/scripts/release.sh +++ b/scripts/release.sh @@ -61,8 +61,10 @@ popd # Generate a release notes file documentation_link="https://lord-microstrain.github.io/mip_sdk_documentation/${release_name}" +changelog_link="https://github.com/LORD-MicroStrain/mip_sdk/blob/${release_name}/CHANGELOG.md" release_notes_file="${tmp_dir}/mip-sdk-release-notes-${release_name}.md" echo "## Useful Links" > ${release_notes_file} +echo "* [Changelog](${changelog_link})" >> ${release_notes_file} echo "* [Documentation](${documentation_link})" >> ${release_notes_file} # Deploy the artifacts to Github @@ -106,4 +108,4 @@ if ! git diff-index --quiet HEAD --; then else echo "No changes to commit to documentation" fi -popd \ No newline at end of file +popd