Example setup program for the 3DM-GQ7-GNSS/INS, and 3DM-CV7-GNSS/INS using C++.
More...
|
static void | logCallback (void *_user, const microstrain_log_level _level, const char *_format, va_list _args) |
| Custom logging callback for MIP SDK message formatting and output. More...
|
|
static void | captureGyroBias (mip::Interface &_device) |
| Captures and configures device gyro bias. More...
|
|
static void | configureGnssMessageFormat (mip::Interface &_device, const uint8_t _gnssDataDescriptorSet) |
| Configures message format for GNSS data streaming. More...
|
|
static void | configureFilterMessageFormat (mip::Interface &_device) |
| Configures message format for filter data streaming. More...
|
|
static void | configureAntennaOffset (mip::Interface &_device, const mip::Vector3f _antennaOffset, const uint8_t _antennaId) |
| Configures the GNSS antenna offset parameters for the device. More...
|
|
static void | initializeFilter (mip::Interface &_device) |
| Initializes and resets the navigation filter. More...
|
|
static void | displayGnssFixState (const mip::data_gnss::FixInfo *_fixInfoArray, const uint8_t _arrayIndex) |
| Displays the current GNSS fix state for a specific antenna when changes occur. More...
|
|
static void | displayFilterState (const mip::data_filter::FilterMode _filterState) |
| Displays the current filter state when changes occur. More...
|
|
static mip::Timestamp | getCurrentTimestamp () |
| Gets the current system timestamp in milliseconds. More...
|
|
static void | initializeDevice (mip::Interface &_device) |
| Initializes and configures a MIP device interface. More...
|
|
static void | terminate (microstrain::Connection *_connection, const char *_message, const bool _successful) |
| Handles graceful program termination and cleanup. More...
|
|
static void | terminate (mip::Interface &_device, const mip::CmdResult _cmdResult, const char *_format,...) |
| Handles graceful program termination and command failure cleanup. More...
|
|
This example shows a basic setup to configure the navigation filter with GNSS heading, and GNSS position and velocity as the heading sources to stream filter data for the 3DM-GQ7-GNSS/INS, and 3DM-CV7-GNSS/INS devices using C++. This is not an exhaustive example of all settings for those devices. If this example does not meet your specific setup needs, please consult the MIP SDK API documentation for the proper commands.
License
- Copyright
- Copyright (c) 2025 MicroStrain by HBK Licensed under MIT License
◆ WHEELED_VEHICLE_APPLICATION
#define WHEELED_VEHICLE_APPLICATION false |
◆ logCallback()
static void logCallback |
( |
void * |
_user, |
|
|
const microstrain_log_level |
_level, |
|
|
const char * |
_format, |
|
|
va_list |
_args |
|
) |
| |
|
static |
Processes and formats log messages from the MIP SDK based on severity level. Routes messages to appropriate output streams - errors and fatal messages go to stderr while other levels go to stdout. Each message is prefixed with its severity level name.
- Parameters
-
_user | Pointer to user data (unused in this implementation) |
_level | Log message severity level from microstrain_log_level enum |
_format | Printf-style format string for the message |
_args | Variable argument list containing message parameters |
◆ captureGyroBias()
- Parameters
-
_device | Reference to the initialized MIP device interface |
◆ configureGnssMessageFormat()
static void configureGnssMessageFormat |
( |
mip::Interface & |
_device, |
|
|
const uint8_t |
_gnssDataDescriptorSet |
|
) |
| |
|
static |
Sets up GNSS data output by:
- Querying device base rate
- Validating desired sample rate against base rate
- Calculating proper decimation
- Configuring message format with:
- Parameters
-
_device | Reference to the initialized MIP device interface |
_gnssDataDescriptorSet | Data descriptor set for the GNSS ID to configure |
◆ configureFilterMessageFormat()
Sets up filter data output by:
- Querying device base rate
- Validating desired sample rate against base rate
- Calculating proper decimation
- Configuring message format with:
- GPS timestamp
- Filter status
- LLH position
- NED velocity
- Euler angles
- Parameters
-
_device | Reference to the initialized MIP device interface |
◆ configureAntennaOffset()
Sets up the physical antenna offset values relative to the device's reference point. The offset is specified in meters using a 3D vector:
- X: forward/back
- Y: left/right
- Z: up/down
- Parameters
-
_device | Reference to the initialized MIP device interface |
_antennaOffset | Antenna offset to set |
_antennaId | Antenna ID to configure the offset for |
- Note
- Offset values are specific to physical device setup and may need to be adjusted based on actual antenna placement
◆ initializeFilter()
Configures the navigation filter by:
- Enabling GNSS position and velocity aiding measurements
- Enabling dual-antenna GNSS heading aiding measurements
- Configuring filter initialization settings:
- Setting initial position and velocity to zero
- Enabling automatic position/velocity/attitude determination
- Configuring dual-antenna kinematic alignment
- Resetting the filter to apply new settings
- Parameters
-
_device | Reference to the initialized MIP device interface |
◆ displayGnssFixState()
Outputs readable messages for GNSS fix state transitions:
- 3D fix
- 2D fix
- Time-only fix
- No fix
- Invalid fix
- RTK float
- RTK fixed
- Differential fix Also indicates whether the current fix is valid based on flag checks.
- Parameters
-
_fixInfoArray | Pointer to an array of GNSS fix information structures containing fix types and validity flags |
_arrayIndex | Zero-based index into the fix info array identifying which GNSS receiver to report (0 = primary antenna, 1 = secondary antenna) |
◆ displayFilterState()
Outputs readable messages for filter state transitions:
- Initialization mode
- Vertical gyro mode
- AHRS mode
- Full navigation mode
- Parameters
-
_filterState | Current filter mode from the MIP device interface |
◆ getCurrentTimestamp()
Provides system time measurement using std::chrono for milliseconds since epoch. Uses system_clock to get wall-clock time that corresponds to calendar time and can be synchronized with external time sources.
- Note
- Update this function to use a different time source if needed for your specific application requirements
- Returns
- Current timestamp in milliseconds since epoch
◆ initializeDevice()
Performs a complete device initialization sequence:
- Verifies device communication with a ping command
- Sets the device to idle mode to ensure reliable configuration
- Queries and displays detailed device information
- Loads default device settings for a known state
- Parameters
-
_device | Reference to a MIP device interface to initialize |
◆ terminate() [1/2]
Handles graceful shutdown when errors occur:
- Outputs provided error message
- Closes device connection if open
- Exits with appropriate status code
- Parameters
-
_connection | Pointer to the device connection to close |
_message | Error message to display |
_successful | Whether termination is due to success or failure |
◆ terminate() [2/2]
Handles command failure scenarios:
- Formats and displays an error message with command result
- Closes device connection
- Exits with failure status
- Parameters
-
_device | MIP device interface for the command that failed |
_cmdResult | Result code from a failed command |
_format | Printf-style format string for error message |
... | Variable arguments for format string |
◆ PORT_NAME
constexpr const char* PORT_NAME = "/dev/ttyACM0" |
|
staticconstexpr |
◆ BAUDRATE
constexpr uint32_t BAUDRATE = 115200 |
|
staticconstexpr |
- Note
- For native serial connections this needs to be 115200 due to the device default settings command Use mip::commands_base::*CommSpeed() to write and save the baudrate on the device
◆ SAMPLE_RATE_HZ
constexpr uint16_t SAMPLE_RATE_HZ = 1 |
|
staticconstexpr |
◆ RUN_TIME_SECONDS
constexpr uint32_t RUN_TIME_SECONDS = 30 |
|
staticconstexpr |