Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add compensated IMU message [AP-1475] #1446

Merged
merged 9 commits into from
Oct 3, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 33 additions & 0 deletions c/include/libsbp/cpp/message_traits.h
Original file line number Diff line number Diff line change
Expand Up @@ -3527,6 +3527,39 @@ struct MessageTraits<sbp_msg_imu_aux_t> {
}
};

template <>
struct MessageTraits<sbp_msg_imu_comp_t> {
static constexpr sbp_msg_type_t id = SbpMsgImuComp;
static constexpr const char *name = "MSG_IMU_COMP";
static const sbp_msg_imu_comp_t &get(const sbp_msg_t &msg) {
return msg.imu_comp;
}
static sbp_msg_imu_comp_t &get(sbp_msg_t &msg) { return msg.imu_comp; }
static void to_sbp_msg(const sbp_msg_imu_comp_t &msg, sbp_msg_t *sbp_msg) {
sbp_msg->imu_comp = msg;
}
static sbp_msg_t to_sbp_msg(const sbp_msg_imu_comp_t &msg) {
sbp_msg_t sbp_msg;
sbp_msg.imu_comp = msg;
return sbp_msg;
}
static s8 send(sbp_state_t *state, u16 sender_id,
const sbp_msg_imu_comp_t &msg, sbp_write_fn_t write) {
return sbp_msg_imu_comp_send(state, sender_id, &msg, write);
}
static s8 encode(uint8_t *buf, uint8_t len, uint8_t *n_written,
const sbp_msg_imu_comp_t &msg) {
return sbp_msg_imu_comp_encode(buf, len, n_written, &msg);
}
static s8 decode(const uint8_t *buf, uint8_t len, uint8_t *n_read,
sbp_msg_imu_comp_t *msg) {
return sbp_msg_imu_comp_decode(buf, len, n_read, msg);
}
static size_t encoded_len(const sbp_msg_imu_comp_t &msg) {
return sbp_msg_imu_comp_encoded_len(&msg);
}
};

template <>
struct MessageTraits<sbp_msg_imu_raw_t> {
static constexpr sbp_msg_type_t id = SbpMsgImuRaw;
Expand Down
1 change: 1 addition & 0 deletions c/include/libsbp/imu.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#ifndef LIBSBP_IMU_MESSAGES_H
#define LIBSBP_IMU_MESSAGES_H
#include <libsbp/imu/MSG_IMU_AUX.h>
#include <libsbp/imu/MSG_IMU_COMP.h>
#include <libsbp/imu/MSG_IMU_RAW.h>

#endif /* LIBSBP_IMU_MESSAGES_H */
215 changes: 215 additions & 0 deletions c/include/libsbp/imu/MSG_IMU_COMP.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,215 @@
/*
* Copyright (C) 2015-2021 Swift Navigation Inc.
* Contact: https://support.swiftnav.com
*
* This source is subject to the license found in the file 'LICENSE' which must
* be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/

/*****************************************************************************
* Automatically generated from yaml/swiftnav/sbp/imu.yaml
* with generate.py. Please do not hand edit!
*****************************************************************************/

#ifndef LIBSBP_IMU_MSG_IMU_COMP_H
#define LIBSBP_IMU_MSG_IMU_COMP_H

#include <math.h>
#include <stdarg.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>

#include <libsbp/common.h>
#include <libsbp/imu_macros.h>
#include <libsbp/string/sbp_string.h>

#ifdef __cplusplus
extern "C" {
#endif

/******************************************************************************
*
* SBP_MSG_IMU_COMP
*
*****************************************************************************/
/** Compensated IMU data
*
* Data from the Inertial Measurement Unit, containing accelerometer and
* gyroscope readings compensated for estimated errors and constant physical
* effects. The output is valid for inertially referenced center of navigation
* (IMU body frame) represented in vehicle body frame.
*/
typedef struct {
/**
* Microseconds since reference epoch [microseconds]
*/
u64 time;

/**
* Contains the applied compensation parameters and time synchronization mode
*/
u16 flags;

/**
* Compensated acceleration X axis [1e-6 m/s^2]
*/
s32 acc_comp_x;

/**
* Compensated acceleration Y axis [1e-6 m/s^2]
*/
s32 acc_comp_y;

/**
* Compensated acceleration Z axis [1e-6 m/s^2]
*/
s32 acc_comp_z;

/**
* Compensated angular rate X axis [1e-6 deg/s]
*/
s32 gyr_comp_x;

/**
* Compensated angular rate Y axis [1e-6 deg/s]
*/
s32 gyr_comp_y;

/**
* Compensated angular rate Z axis [1e-6 deg/s]
*/
s32 gyr_comp_z;
} sbp_msg_imu_comp_t;

/**
* Get encoded size of an instance of sbp_msg_imu_comp_t
*
* @param msg sbp_msg_imu_comp_t instance
* @return Length of on-wire representation
*/
static inline size_t sbp_msg_imu_comp_encoded_len(
const sbp_msg_imu_comp_t *msg) {
(void)msg;
return SBP_MSG_IMU_COMP_ENCODED_LEN;
}

/**
* Encode an instance of sbp_msg_imu_comp_t to wire representation
*
* This function encodes the given instance in to the user provided buffer. The
* buffer provided to this function must be large enough to store the encoded
* message otherwise it will return SBP_ENCODE_ERROR without writing anything to
* the buffer.
*
* Specify the length of the destination buffer in the \p len parameter. If
* non-null the number of bytes written to the buffer will be returned in \p
* n_written.
*
* @param buf Destination buffer
* @param len Length of \p buf
* @param n_written If not null, on success will be set to the number of bytes
* written to \p buf
* @param msg Instance of sbp_msg_imu_comp_t to encode
* @return SBP_OK on success, or other libsbp error code
*/
SBP_EXPORT s8 sbp_msg_imu_comp_encode(uint8_t *buf, uint8_t len,
uint8_t *n_written,
const sbp_msg_imu_comp_t *msg);

/**
* Decode an instance of sbp_msg_imu_comp_t from wire representation
*
* This function decodes the wire representation of a sbp_msg_imu_comp_t message
* to the given instance. The caller must specify the length of the buffer in
* the \p len parameter. If non-null the number of bytes read from the buffer
* will be returned in \p n_read.
*
* @param buf Wire representation of the sbp_msg_imu_comp_t instance
* @param len Length of \p buf
* @param n_read If not null, on success will be set to the number of bytes read
* from \p buf
* @param msg Destination
* @return SBP_OK on success, or other libsbp error code
*/
SBP_EXPORT s8 sbp_msg_imu_comp_decode(const uint8_t *buf, uint8_t len,
uint8_t *n_read, sbp_msg_imu_comp_t *msg);
/**
* Send an instance of sbp_msg_imu_comp_t with the given write function
*
* An equivalent of #sbp_message_send which operates specifically on
* sbp_msg_imu_comp_t
*
* The given message will be encoded to wire representation and passed in to the
* given write function callback. The write callback will be called several
* times for each invocation of this function.
*
* @param s SBP state
* @param sender_id SBP sender id
* @param msg Message to send
* @param write Write function
* @return SBP_OK on success, or other libsbp error code
*/
SBP_EXPORT s8 sbp_msg_imu_comp_send(sbp_state_t *s, u16 sender_id,
const sbp_msg_imu_comp_t *msg,
sbp_write_fn_t write);

/**
* Compare two instances of sbp_msg_imu_comp_t
*
* The two instances will be compared and a value returned consistent with the
* return codes of comparison functions from the C standard library
*
* 0 will be returned if \p a and \p b are considered equal
* A value less than 0 will be returned if \p a is considered to be less than \p
* b A value greater than 0 will be returned if \p b is considered to be greater
* than \p b
*
* @param a sbp_msg_imu_comp_t instance
* @param b sbp_msg_imu_comp_t instance
* @return 0, <0, >0
*/
SBP_EXPORT int sbp_msg_imu_comp_cmp(const sbp_msg_imu_comp_t *a,
const sbp_msg_imu_comp_t *b);

#ifdef __cplusplus
}

static inline bool operator==(const sbp_msg_imu_comp_t &lhs,
const sbp_msg_imu_comp_t &rhs) {
return sbp_msg_imu_comp_cmp(&lhs, &rhs) == 0;
}

static inline bool operator!=(const sbp_msg_imu_comp_t &lhs,
const sbp_msg_imu_comp_t &rhs) {
return sbp_msg_imu_comp_cmp(&lhs, &rhs) != 0;
}

static inline bool operator<(const sbp_msg_imu_comp_t &lhs,
const sbp_msg_imu_comp_t &rhs) {
return sbp_msg_imu_comp_cmp(&lhs, &rhs) < 0;
}

static inline bool operator<=(const sbp_msg_imu_comp_t &lhs,
const sbp_msg_imu_comp_t &rhs) {
return sbp_msg_imu_comp_cmp(&lhs, &rhs) <= 0;
}

static inline bool operator>(const sbp_msg_imu_comp_t &lhs,
const sbp_msg_imu_comp_t &rhs) {
return sbp_msg_imu_comp_cmp(&lhs, &rhs) > 0;
}

static inline bool operator>=(const sbp_msg_imu_comp_t &lhs,
const sbp_msg_imu_comp_t &rhs) {
return sbp_msg_imu_comp_cmp(&lhs, &rhs) >= 0;
}

#endif // ifdef __cplusplus

#endif /* LIBSBP_IMU_MSG_IMU_COMP_H */
93 changes: 93 additions & 0 deletions c/include/libsbp/imu_macros.h
Original file line number Diff line number Diff line change
Expand Up @@ -129,4 +129,97 @@
*/
#define SBP_MSG_IMU_AUX_ENCODED_LEN 4u

#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK (0x1u)
#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT (6u)
#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_GET(flags) \
((u16)((u16)((flags) >> SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT) & \
SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK))
#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SET(flags, val) \
do { \
(flags) = (u16)( \
(flags & (~(SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK \
<< SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT))) | \
(((val) & (SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK)) \
<< (SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT))); \
} while (0)

#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK (0x1u)
#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT (5u)
#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_GET(flags) \
((u16)((u16)((flags) >> SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT) & \
SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK))
#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SET(flags, val) \
do { \
(flags) = (u16)( \
(flags & (~(SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK \
<< SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT))) | \
(((val) & (SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK)) \
<< (SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT))); \
} while (0)

#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK (0x1u)
#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT (4u)
#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_GET(flags) \
((u16)((u16)((flags) >> SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT) & \
SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK))
#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SET(flags, val) \
do { \
(flags) = \
(u16)((flags & (~(SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK \
<< SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT))) | \
(((val) & (SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK)) \
<< (SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT))); \
} while (0)

#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK (0x1u)
#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT (3u)
#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_GET(flags) \
((u16)((u16)((flags) >> SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT) & \
SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK))
#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SET(flags, val) \
do { \
(flags) = (u16)( \
(flags & (~(SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK \
<< SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT))) | \
(((val) & (SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK)) \
<< (SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT))); \
} while (0)

#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK (0x1u)
#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT (2u)
#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_GET(flags) \
((u16)((u16)((flags) >> SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT) & \
SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK))
#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SET(flags, val) \
do { \
(flags) = \
(u16)((flags & (~(SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK \
<< SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT))) | \
(((val) & (SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK)) \
<< (SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT))); \
} while (0)

#define SBP_IMU_COMP_TIME_STATUS_MASK (0x3u)
#define SBP_IMU_COMP_TIME_STATUS_SHIFT (0u)
#define SBP_IMU_COMP_TIME_STATUS_GET(flags) \
((u16)((u16)((flags) >> SBP_IMU_COMP_TIME_STATUS_SHIFT) & \
SBP_IMU_COMP_TIME_STATUS_MASK))
#define SBP_IMU_COMP_TIME_STATUS_SET(flags, val) \
do { \
(flags) = (u16)((flags & (~(SBP_IMU_COMP_TIME_STATUS_MASK \
<< SBP_IMU_COMP_TIME_STATUS_SHIFT))) | \
(((val) & (SBP_IMU_COMP_TIME_STATUS_MASK)) \
<< (SBP_IMU_COMP_TIME_STATUS_SHIFT))); \
} while (0)

#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_START_OF_CURRENT_GPS_WEEK \
(0)
#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_TIME_OF_SYSTEM_STARTUP (1)
#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_UNKNOWN (2)
#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_LAST_PPS (3)
/**
* Encoded length of sbp_msg_imu_comp_t
*/
#define SBP_MSG_IMU_COMP_ENCODED_LEN 34u

#endif /* LIBSBP_IMU_MACROS_H */
Loading
Loading