187 if (
xsens.parser.id == XSENS_ReqOutputModeAck_ID) {
189 }
else if (
xsens.parser.id == XSENS_ReqOutputSettings_ID) {
191 }
else if (
xsens.parser.id == XSENS_ReqMagneticDeclinationAck_ID) {
196 }
else if (
xsens.parser.id == XSENS_ReqLeverArmGpsAck_ID) {
203 }
else if (
xsens.parser.id == XSENS_Error_ID) {
207 else if (
xsens.parser.id == XSENS_GPSStatus_ID) {
208 xsens.gps.nb_channels = XSENS_GPSStatus_nch(
xsens.parser.msg_buf);
209 xsens.gps.num_sv = 0;
214 uint8_t ch = XSENS_GPSStatus_chn(
xsens.parser.msg_buf, i);
215 if (ch >
xsens.gps.nb_channels) {
continue; }
216 xsens.gps.svinfos[ch].svid = XSENS_GPSStatus_svid(
xsens.parser.msg_buf, i);
217 xsens.gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(
xsens.parser.msg_buf, i);
218 xsens.gps.svinfos[ch].qi = XSENS_GPSStatus_qi(
xsens.parser.msg_buf, i);
219 xsens.gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(
xsens.parser.msg_buf, i);
220 if (
xsens.gps.svinfos[ch].flags > 0) {
226 else if (
xsens.parser.id == XSENS_MTData_ID) {
235 offset += XSENS_DATA_RAWInertial_LENGTH;
238#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
241 xsens.gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(
xsens.parser.msg_buf,
offset);
242 xsens.gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(
xsens.parser.msg_buf,
offset);
243 xsens.gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(
xsens.parser.msg_buf,
offset);
248 xsens.gps.hmsl = XSENS_DATA_RAWGPS_alt(
xsens.parser.msg_buf,
offset) - hmsl;
251 xsens.gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(
xsens.parser.msg_buf,
offset);
252 xsens.gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(
xsens.parser.msg_buf,
offset);
253 xsens.gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(
xsens.parser.msg_buf,
offset);
255 xsens.gps.pacc = XSENS_DATA_RAWGPS_hacc(
xsens.parser.msg_buf,
offset) / 100;
256 xsens.gps.sacc = XSENS_DATA_RAWGPS_sacc(
xsens.parser.msg_buf,
offset) / 100;
261 offset += XSENS_DATA_RAWGPS_LENGTH;
265 offset += XSENS_DATA_Temp_LENGTH;
290 offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
299 offset += XSENS_DATA_Quaternion_LENGTH;
302 xsens.euler.phi = XSENS_DATA_Euler_roll(
xsens.parser.msg_buf,
offset) * M_PI / 180;
303 xsens.euler.theta = XSENS_DATA_Euler_pitch(
xsens.parser.msg_buf,
offset) * M_PI / 180;
304 xsens.euler.psi = XSENS_DATA_Euler_yaw(
xsens.parser.msg_buf,
offset) * M_PI / 180;
305 offset += XSENS_DATA_Euler_LENGTH;
308 offset += XSENS_DATA_Matrix_LENGTH;
320 offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
323 xsens.lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(
xsens.parser.msg_buf,
offset));
324 xsens.lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(
xsens.parser.msg_buf,
offset));
326 offset += XSENS_DATA_Position_LENGTH;
328#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
338 offset += XSENS_DATA_Velocity_LENGTH;
354 offset += XSENS_DATA_Status_LENGTH;
358 offset += XSENS_DATA_TimeStamp_LENGTH;
369 offset += XSENS_DATA_UTC_LENGTH;