45 msg.clock_status.clock_stable = (pLogData->
utcData.
status & (1 << 0)) >> 0;
46 msg.clock_status.clock_status = (pLogData->
utcData.
status & (0b1111 << 1)) >> 1;
47 msg.clock_status.clock_utc_sync = (pLogData->
utcData.
status & (1 << 5)) >> 5;
48 msg.clock_status.clock_utc_status = (pLogData->
utcData.
status & (0b1111 << 6)) >> 6;
63 msg.imu_status.imu_com = (pLogData->
imuData.
status & (1 << 0)) >> 0;
64 msg.imu_status.imu_status = (pLogData->
imuData.
status & (1 << 1)) >> 1;
65 msg.imu_status.imu_accel_x = (pLogData->
imuData.
status & (1 << 2)) >> 2;
66 msg.imu_status.imu_accel_y = (pLogData->
imuData.
status & (1 << 3)) >> 3;
67 msg.imu_status.imu_accel_z = (pLogData->
imuData.
status & (1 << 4)) >> 4;
68 msg.imu_status.imu_gyro_x = (pLogData->
imuData.
status & (1 << 5)) >> 5;
69 msg.imu_status.imu_gyro_y = (pLogData->
imuData.
status & (1 << 6)) >> 6;
70 msg.imu_status.imu_gyro_z = (pLogData->
imuData.
status & (1 << 7)) >> 7;
71 msg.imu_status.imu_accels_in_range = (pLogData->
imuData.
status & (1 << 8)) >> 8;
72 msg.imu_status.imu_gyros_in_range = (pLogData->
imuData.
status & (1 << 9)) >> 9;
90 msg.solution_mode = val & (0b1111);
91 msg.attitude_valid = (val & (1 << 4)) >> 4;
92 msg.heading_valid = (val & (1 << 5)) >> 5;
93 msg.velocity_valid = (val & (1 << 6)) >> 6;
94 msg.position_valid = (val & (1 << 7)) >> 7;
95 msg.vert_ref_used = (val & (1 << 8)) >> 8;
96 msg.mag_ref_used = (val & (1 << 9)) >> 9;
97 msg.gps1_vel_used = (val & (1 << 10)) >> 10;
98 msg.gps1_pos_used = (val & (1 << 11)) >> 11;
99 msg.gps1_course_used = (val & (1 << 12)) >> 12;
100 msg.gps1_hdt_used = (val & (1 << 13)) >> 13;
101 msg.gps2_vel_used = (val & (1 << 14)) >> 14;
102 msg.gps2_pos_used = (val & (1 << 15)) >> 15;
103 msg.gps2_course_used = (val & (1 << 16)) >> 16;
104 msg.gps2_hdt_used = (val & (1 << 17)) >> 17;
105 msg.odo_used = (val & (1 << 18)) >> 18;
173 msg.mag_x = (val & (1 << 0)) >> 0;
174 msg.mag_y = (val & (1 << 1)) >> 1;
175 msg.mag_z = (val & (1 << 2)) >> 2;
176 msg.accel_x = (val & (1 << 3)) >> 3;
177 msg.accel_y = (val & (1 << 4)) >> 4;
178 msg.accel_z = (val & (1 << 5)) >> 5;
179 msg.mags_in_range = (val & (1 << 6)) >> 6;
180 msg.accels_in_range = (val & (1 << 7)) >> 7;
181 msg.calibration = (val & (1 << 8)) >> 8;
200 ROS_INFO(
"SBG DRIVER - SbgMagCalib message not implemented");
204 msg.vel_status = (val & (0b111111 << 0)) >> 0;
205 msg.vel_type = (val & (0b111111 << 6)) >> 6;
226 msg.status = (val & (0b111111 << 0)) >> 6;
227 msg.type = (val & (0b111111 << 6)) >> 6;
228 msg.gps_l1_used = (val & (1 << 12)) >> 12;
229 msg.gps_l2_used = (val & (1 << 13)) >> 13;
230 msg.gps_l5_used = (val & (1 << 14)) >> 14;
231 msg.glo_l1_used = (val & (1 << 15)) >> 15;
232 msg.glo_l2_used = (val & (1 << 16)) >> 16;
SbgLogEkfNavData ekfNavData
void read_mag_status(sbg_driver::SbgMagStatus &msg, const uint16 &val)
SbgLogEkfEulerData ekfEulerData
SbgLogEkfQuatData ekfQuatData
void read_ecom_log_status(sbg_driver::SbgStatus &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_ekf_nav(sbg_driver::SbgEkfNav &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_gps_pos(sbg_driver::SbgGpsPos &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_ship_motion(sbg_driver::SbgShipMotion &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_pressure(sbg_driver::SbgPressure &msg, const SbgBinaryLogData *pLogData)
uint8 productCode[SBG_ECOM_INFO_PRODUCT_CODE_LENGTH]
void read_ecom_log_utc_time(sbg_driver::SbgUtcTime &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_mag(sbg_driver::SbgMag &msg, const SbgBinaryLogData *pLogData)
SbgLogOdometerData odometerData
void read_ekf_solution_status(sbg_driver::SbgEkfStatus &msg, const uint32 &val)
uint8 rawBuffer[SBG_ECOM_GPS_RAW_MAX_BUFFER_SIZE]
SBG_INLINE const char * sbgErrorCodeToString(SbgErrorCode errorCode)
void read_gps_vel_status(sbg_driver::SbgGpsVelStatus &msg, const uint32 &val)
void read_gps_pos_status(sbg_driver::SbgGpsPosStatus &msg, const uint32 &val)
void read_ecom_log_odo_vel(sbg_driver::SbgOdoVel &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_ekf_quat(sbg_driver::SbgEkfQuat &msg, const SbgBinaryLogData *pLogData)
void read_get_info(SbgEComHandle *comHandle)
void read_ecom_log_gps_vel(sbg_driver::SbgGpsVel &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_gps_raw(sbg_driver::SbgGpsRaw &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_imu_data(sbg_driver::SbgImuData &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_gps_hdt(sbg_driver::SbgGpsHdt &msg, const SbgBinaryLogData *pLogData)
SbgLogPressureData pressureData
SbgErrorCode sbgEComCmdGetInfo(SbgEComHandle *pHandle, SbgEComDeviceInfo *pInfo)
void read_ecom_log_event(sbg_driver::SbgEvent &msg, const SbgBinaryLogData *pLogData)
SbgLogShipMotionData shipMotionData
SbgLogStatusData statusData
enum _SbgErrorCode SbgErrorCode
void read_ecom_log_mag_calib(sbg_driver::SbgMagCalib &msg, const SbgBinaryLogData *pLogData)
void read_ecom_log_ekf_euler(sbg_driver::SbgEkfEuler &msg, const SbgBinaryLogData *pLogData)