467 lines
21 KiB
C++
467 lines
21 KiB
C++
/***********************************************************************
|
|
** Copyright (C) 2019 LP-Research
|
|
** All rights reserved.
|
|
** Contact: LP-Research (klaus@lp-research.com)
|
|
**
|
|
** This file is part of the Open Motion Analysis Toolkit (OpenMAT).
|
|
**
|
|
** Redistribution and use in source and binary forms, with
|
|
** or without modification, are permitted provided that the
|
|
** following conditions are met:
|
|
**
|
|
** Redistributions of source code must retain the above copyright
|
|
** notice, this list of conditions and the following disclaimer.
|
|
** Redistributions in binary form must reproduce the above copyright
|
|
** notice, this list of conditions and the following disclaimer in
|
|
** the documentation and/or other materials provided with the
|
|
** distribution.
|
|
**
|
|
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
** FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
|
** HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
|
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
|
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
|
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
|
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
***********************************************************************/
|
|
|
|
#ifndef SENSOR_DATA
|
|
#define SENSOR_DATA
|
|
#include <string>
|
|
#include <sstream>
|
|
#include <cstdio>
|
|
#include "LpMatrix.h"
|
|
#include "LpmsIG1Registers.h"
|
|
#include "LpUtil.h"
|
|
|
|
#include "SensorDataI.h"
|
|
|
|
struct IG1ImuData :IG1ImuDataI
|
|
{
|
|
IG1ImuData()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
void setData(uint32_t transmitDataConfig, unsigned char *_data, int length) {
|
|
|
|
dataSize = length / 4 -1;
|
|
memcpy(data, _data+4, dataSize*4);
|
|
|
|
uint2char i2c;
|
|
floatArray2char f2c;
|
|
|
|
int dataIdx = 0;
|
|
|
|
memcpy(i2c.c, _data, 4);
|
|
timestamp = i2c.int_val;
|
|
dataIdx += 4;
|
|
|
|
// 32-bit Floating Point
|
|
if (transmitDataConfig & TDR_ACC_RAW_OUTPUT_ENABLED) {
|
|
memcpy(accRaw.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
if (transmitDataConfig & TDR_ACC_CALIBRATED_OUTPUT_ENABLED) {
|
|
memcpy(accCalibrated.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
if (transmitDataConfig & TDR_GYR0_RAW_OUTPUT_ENABLED) {
|
|
memcpy(gyroIRaw.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
if (transmitDataConfig & TDR_GYR1_RAW_OUTPUT_ENABLED) {
|
|
memcpy(gyroIIRaw.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_GYR0_BIAS_CALIBRATED_OUTPUT_ENABLED) {
|
|
memcpy(gyroIBiasCalibrated.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
if (transmitDataConfig & TDR_GYR1_BIAS_CALIBRATED_OUTPUT_ENABLED) {
|
|
memcpy(gyroIIBiasCalibrated.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_GYR0_ALIGN_CALIBRATED_OUTPUT_ENABLED) {
|
|
memcpy(gyroIAlignmentCalibrated.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
if (transmitDataConfig & TDR_GYR1_ALIGN_CALIBRATED_OUTPUT_ENABLED) {
|
|
memcpy(gyroIIAlignmentCalibrated.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_MAG_RAW_OUTPUT_ENABLED) {
|
|
memcpy(magRaw.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_MAG_CALIBRATED_OUTPUT_ENABLED) {
|
|
memcpy(f2c.c, _data + dataIdx, 12);
|
|
memcpy(magCalibrated.data, f2c.float_val, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_ANGULAR_VELOCITY_OUTPUT_ENABLED) {
|
|
memcpy(angularVelocity.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_QUAT_OUTPUT_ENABLED) {
|
|
memcpy(quaternion.data, _data + dataIdx, 16);
|
|
dataIdx += 16;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_EULER_OUTPUT_ENABLED) {
|
|
memcpy(euler.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_LINACC_OUTPUT_ENABLED) {
|
|
memcpy(linAcc.data, _data + dataIdx, 12);
|
|
dataIdx += 12;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_PRESSURE_OUTPUT_ENABLED) {
|
|
memcpy(f2c.c, _data + dataIdx, 4);
|
|
pressure = f2c.float_val[0];
|
|
dataIdx += 4;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_ALTITUDE_OUTPUT_ENABLED) {
|
|
memcpy(f2c.c, _data + dataIdx, 4);
|
|
altitude = f2c.float_val[0];
|
|
dataIdx += 4;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_TEMPERATURE_OUTPUT_ENABLED) {
|
|
memcpy(f2c.c, _data + dataIdx, 4);
|
|
temperature = f2c.float_val[0];
|
|
dataIdx += 4;
|
|
}
|
|
}
|
|
|
|
void setData16bit(uint32_t useRadianOutput, uint32_t transmitDataConfig, unsigned char *_data) {
|
|
|
|
uint2char i2c;
|
|
int dataIdx = 0;
|
|
|
|
memcpy(i2c.c, _data, 4);
|
|
timestamp = i2c.int_val;
|
|
dataIdx += 4;
|
|
|
|
float useRadianOutputFactor;
|
|
|
|
if (useRadianOutput)
|
|
{
|
|
useRadianOutputFactor = 100.0f;
|
|
}
|
|
else {
|
|
useRadianOutputFactor = 10.0f;
|
|
}
|
|
|
|
//16-bit fixed point
|
|
if (transmitDataConfig & TDR_ACC_RAW_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
accRaw.data[i] = ((float)s) / 1000.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_ACC_CALIBRATED_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
accCalibrated.data[i] = ((float)s) / 1000.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
if (transmitDataConfig & TDR_GYR0_RAW_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
gyroIRaw.data[i] = ((float)s) / useRadianOutputFactor;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
if (transmitDataConfig & TDR_GYR1_RAW_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
gyroIIRaw.data[i] = ((float)s) / useRadianOutputFactor;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_GYR0_BIAS_CALIBRATED_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
gyroIBiasCalibrated.data[i] = ((float)s) / useRadianOutputFactor;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
if (transmitDataConfig & TDR_GYR1_BIAS_CALIBRATED_OUTPUT_ENABLED) {
|
|
//if (useRadianOutput) //100.0f
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
gyroIIBiasCalibrated.data[i] = ((float)s) / useRadianOutputFactor;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_GYR0_ALIGN_CALIBRATED_OUTPUT_ENABLED) {
|
|
//if (useRadianOutput) //100.0f
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
gyroIAlignmentCalibrated.data[i] = ((float)s) / useRadianOutputFactor;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
if (transmitDataConfig & TDR_GYR1_ALIGN_CALIBRATED_OUTPUT_ENABLED) {
|
|
//if (useRadianOutput) //100.0f
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
gyroIIAlignmentCalibrated.data[i] = ((float)s) / useRadianOutputFactor;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_MAG_RAW_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
magRaw.data[i] = ((float)s) / 100.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_MAG_CALIBRATED_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
magCalibrated.data[i] = ((float)s) / 100.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_ANGULAR_VELOCITY_OUTPUT_ENABLED) {
|
|
//if (useRadianOutput)
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
angularVelocity.data[i] = ((float)s) / 100.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_QUAT_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 4; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
quaternion.data[i] = ((float)s) / 10000.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_EULER_OUTPUT_ENABLED) {
|
|
|
|
if (useRadianOutput) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
euler.data[i] = ((float) s) / 10000.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
else {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
euler.data[i] = ((float)s) / 100.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_LINACC_OUTPUT_ENABLED) {
|
|
for (int i = 0; i < 3; i++) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
linAcc.data[i] = ((float)s) / 1000.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_PRESSURE_OUTPUT_ENABLED) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
pressure = ((float)s) / 100.0f;
|
|
dataIdx += 2;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_ALTITUDE_OUTPUT_ENABLED) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
altitude = ((float)s) / 10.0f;
|
|
dataIdx += 2;
|
|
}
|
|
|
|
if (transmitDataConfig & TDR_TEMPERATURE_OUTPUT_ENABLED) {
|
|
int16_t s = _data[dataIdx] + _data[dataIdx + 1] * 256;
|
|
temperature = ((float)s) / 100.0f;
|
|
dataIdx += 2;
|
|
}
|
|
}
|
|
};
|
|
|
|
struct IG1Info : IG1InfoI
|
|
{
|
|
|
|
IG1Info()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
};
|
|
|
|
|
|
struct IG1AdvancedSettings : IG1SettingsI
|
|
{
|
|
IG1AdvancedSettings()
|
|
{
|
|
reset();
|
|
}
|
|
};
|
|
|
|
////////////////////////////////
|
|
// GPS Data
|
|
////////////////////////////////
|
|
struct IG1GpsData : IG1GpsDataI
|
|
{
|
|
IG1GpsData()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
void setData(uint32_t gpsPVT_DataConfig, uint32_t gpsATT_DataConfig, unsigned char *_data)
|
|
{
|
|
int dataIdx = 0;
|
|
|
|
uint2char i2c;
|
|
memcpy(i2c.c, _data, 4);
|
|
timestamp = i2c.int_val;
|
|
dataIdx += 4;
|
|
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_ITOW_ENABLE) { memcpy(&pvt.iTOW, _data + dataIdx, sizeof(pvt.iTOW)); dataIdx += sizeof(pvt.iTOW); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_YEAR_ENABLE) { memcpy(&pvt.year, _data + dataIdx, sizeof(pvt.year)); dataIdx += sizeof(pvt.year); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_MONTH_ENABLE) { memcpy(&pvt.month, _data + dataIdx, sizeof(pvt.month)); dataIdx += sizeof(pvt.month); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_DAY_ENABLE) { memcpy(&pvt.day, _data + dataIdx, sizeof(pvt.day)); dataIdx += sizeof(pvt.day); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_HOUR_ENABLE) { memcpy(&pvt.hour, _data + dataIdx, sizeof(pvt.hour)); dataIdx += sizeof(pvt.hour); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_MIN_ENABLE) { memcpy(&pvt.min, _data + dataIdx, sizeof(pvt.min)); dataIdx += sizeof(pvt.min); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_SEC_ENABLE) { memcpy(&pvt.sec, _data + dataIdx, sizeof(pvt.sec)); dataIdx += sizeof(pvt.sec); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_VALID_ENABLE) { memcpy(&pvt.valid, _data + dataIdx, sizeof(pvt.valid)); dataIdx += sizeof(pvt.valid); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_TACC_ENABLE) { memcpy(&pvt.tAcc, _data + dataIdx, sizeof(pvt.tAcc)); dataIdx += sizeof(pvt.tAcc); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_NANO_ENABLE) { memcpy(&pvt.nano, _data + dataIdx, sizeof(pvt.nano)); dataIdx += sizeof(pvt.nano); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_FIXTYPE_ENABLE) { memcpy(&pvt.fixType, _data + dataIdx, sizeof(pvt.fixType)); dataIdx += sizeof(pvt.fixType); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_FLAGS_ENABLE) { memcpy(&pvt.flags, _data + dataIdx, sizeof(pvt.flags)); dataIdx += sizeof(pvt.flags); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_FLAGS2_ENABLE) { memcpy(&pvt.flags2, _data + dataIdx, sizeof(pvt.flags2)); dataIdx += sizeof(pvt.flags2); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_NUMSV_ENABLE) { memcpy(&pvt.numSV, _data + dataIdx, sizeof(pvt.numSV)); dataIdx += sizeof(pvt.numSV); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_LONGITUDE_ENABLE) { memcpy(&pvt.longitude, _data + dataIdx, sizeof(pvt.longitude)); dataIdx += sizeof(pvt.longitude); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_LATITUDE_ENABLE) { memcpy(&pvt.latitude, _data + dataIdx, sizeof(pvt.latitude)); dataIdx += sizeof(pvt.latitude); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_HEIGHT_ENABLE) { memcpy(&pvt.height, _data + dataIdx, sizeof(pvt.height)); dataIdx += sizeof(pvt.height); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_HMSL_ENABLE) { memcpy(&pvt.hMSL, _data + dataIdx, sizeof(pvt.hMSL)); dataIdx += sizeof(pvt.hMSL); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_HACC_ENABLE) { memcpy(&pvt.hAcc, _data + dataIdx, sizeof(pvt.hAcc)); dataIdx += sizeof(pvt.hAcc); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_VACC_ENABLE) { memcpy(&pvt.vAcc, _data + dataIdx, sizeof(pvt.vAcc)); dataIdx += sizeof(pvt.vAcc); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_VELN_ENABLE) { memcpy(&pvt.velN, _data + dataIdx, sizeof(pvt.velN)); dataIdx += sizeof(pvt.velN); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_VELE_ENABLE) { memcpy(&pvt.velE, _data + dataIdx, sizeof(pvt.velE)); dataIdx += sizeof(pvt.velE); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_VELD_ENABLE) { memcpy(&pvt.velD, _data + dataIdx, sizeof(pvt.velD)); dataIdx += sizeof(pvt.velD); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_GSPEED_ENABLE) { memcpy(&pvt.gSpeed, _data + dataIdx, sizeof(pvt.gSpeed)); dataIdx += sizeof(pvt.gSpeed); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_HEADMOT_ENABLE) { memcpy(&pvt.headMot, _data + dataIdx, sizeof(pvt.headMot)); dataIdx += sizeof(pvt.headMot); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_SACC_ENABLE) { memcpy(&pvt.sAcc, _data + dataIdx, sizeof(pvt.sAcc)); dataIdx += sizeof(pvt.sAcc); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_HEADACC_ENABLE) { memcpy(&pvt.headAcc, _data + dataIdx, sizeof(pvt.headAcc)); dataIdx += sizeof(pvt.headAcc); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_PDOP_ENABLE) { memcpy(&pvt.pDOP, _data + dataIdx, sizeof(pvt.pDOP)); dataIdx += sizeof(pvt.pDOP); }
|
|
if (gpsPVT_DataConfig & GPS_NAV_PVT_HEADVEH_ENABLE) { memcpy(&pvt.headVeh, _data + dataIdx, sizeof(pvt.headVeh)); dataIdx += sizeof(pvt.headVeh); }
|
|
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_ITOW_ENABLE) { memcpy(&att.iTOW, _data + dataIdx, sizeof(pvt.iTOW)); dataIdx += sizeof(pvt.iTOW); }
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_VERSION_ENABLE) { memcpy(&att.version, _data + dataIdx, sizeof(att.version)); dataIdx += sizeof(att.version); }
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_ROLL_ENABLE) { memcpy(&att.roll, _data + dataIdx, sizeof(att.roll)); dataIdx += sizeof(att.roll); }
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_PITCH_ENABLE) { memcpy(&att.pitch, _data + dataIdx, sizeof(att.pitch)); dataIdx += sizeof(att.pitch); }
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_HEADING_ENABLE) { memcpy(&att.heading, _data + dataIdx, sizeof(att.heading)); dataIdx += sizeof(att.heading); }
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_ACCROLL_ENABLE) { memcpy(&att.accRoll, _data + dataIdx, sizeof(att.accRoll)); dataIdx += sizeof(att.accRoll); }
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_ACCPITCH_ENABLE) { memcpy(&att.accPitch, _data + dataIdx, sizeof(att.accPitch)); dataIdx += sizeof(att.accPitch); }
|
|
if (gpsATT_DataConfig & GPS_NAV_ATT_ACCHEADING_ENABLE) { memcpy(&att.accHeading, _data + dataIdx, sizeof(att.accHeading)); dataIdx += sizeof(att.accHeading); }
|
|
|
|
if (gpsATT_DataConfig & GPS_ESF_STATUS_ITOW_ENABLE) { memcpy(&esf.iTOW, _data + dataIdx, sizeof(esf.iTOW)); dataIdx += sizeof(esf.iTOW); }
|
|
if (gpsATT_DataConfig & GPS_ESF_STATUS_VERSION_ENABLE) { memcpy(&esf.version, _data + dataIdx, sizeof(esf.version)); dataIdx += sizeof(esf.version); }
|
|
if (gpsATT_DataConfig & GPS_ESF_STATUS_INITSTATUS1_ENABLE) { memcpy(&esf.initStatus1, _data + dataIdx, sizeof(esf.initStatus1)); dataIdx += sizeof(esf.initStatus1); }
|
|
if (gpsATT_DataConfig & GPS_ESF_STATUS_INITSTATUS2_ENABLE) { memcpy(&esf.initStatus2, _data + dataIdx, sizeof(esf.initStatus2)); dataIdx += sizeof(esf.initStatus2); }
|
|
if (gpsATT_DataConfig & GPS_ESF_STATUS_FUSIONMODE_ENABLE) { memcpy(&esf.fusionMode, _data + dataIdx, sizeof(esf.fusionMode)); dataIdx += sizeof(esf.fusionMode); }
|
|
if (gpsATT_DataConfig & GPS_ESF_STATUS_NUMSENS_ENABLE) { memcpy(&esf.numSens, _data + dataIdx, sizeof(esf.numSens)); dataIdx += sizeof(esf.numSens); }
|
|
if (gpsATT_DataConfig & GPS_ESF_STATUS_SENSSTATUS_ENABLE) { memcpy(&esf.sensStatus, _data + dataIdx, sizeof(esf.sensStatus)); dataIdx += sizeof(esf.sensStatus); }
|
|
|
|
if (gpsATT_DataConfig & GPS_UDR_STATUS_ENABLE) { memcpy(&udrStatus, _data + dataIdx, sizeof(udrStatus)); dataIdx += sizeof(udrStatus); }
|
|
|
|
}
|
|
|
|
std::string getStringCSV()
|
|
{
|
|
std::stringstream ss;
|
|
// int precision = 12;
|
|
// ss << std::setprecision(precision) << std::fixed;
|
|
ss << timestamp << ",";
|
|
ss << pvt.iTOW << ",";
|
|
ss << pvt.year
|
|
<< std::setw(2) << std::setfill('0') << (int)pvt.month
|
|
<< std::setw(2) << std::setfill('0') << (int)pvt.day << ",";
|
|
ss << std::setw(2) << std::setfill('0') << (int)pvt.hour
|
|
<< std::setw(2) << std::setfill('0') << (int)pvt.min
|
|
<< std::setw(2) << std::setfill('0') << (int)pvt.sec << ",";
|
|
ss << (int)pvt.valid << ",";
|
|
ss << pvt.tAcc << ",";
|
|
ss << pvt.nano << ",";
|
|
ss << (int)pvt.fixType << ",";
|
|
ss << (int)pvt.flags << ",";
|
|
ss << (int)pvt.flags2 << ",";
|
|
ss << pvt.longitude<< ",";
|
|
ss << pvt.latitude << ",";
|
|
ss << (int)pvt.numSV << ",";
|
|
ss << pvt.height << ",";
|
|
ss << pvt.hMSL << ",";
|
|
ss << pvt.hAcc << ",";
|
|
ss << pvt.vAcc << ",";
|
|
ss << pvt.velN << ",";
|
|
ss << pvt.velE << ",";
|
|
ss << pvt.velD << ",";
|
|
ss << pvt.gSpeed << ",";
|
|
ss << pvt.headMot*1e-5 << ",";
|
|
ss << pvt.sAcc << ",";
|
|
ss << pvt.headAcc*1e-5 << ",";
|
|
ss << pvt.pDOP*0.01 << ",";
|
|
ss << pvt.headVeh*1e-5 << ",";
|
|
|
|
ss << att.iTOW << ",";
|
|
ss << (int)att.version << ",";
|
|
ss << att.roll*1e-5 << ",";
|
|
ss << att.pitch*1e-5 << ",";
|
|
ss << att.heading*1e-5 << ",";
|
|
ss << att.accRoll*1e-5 << ",";
|
|
ss << att.accPitch*1e-5 << ",";
|
|
ss << att.accHeading*1e-5 << ",";
|
|
|
|
ss << esf.iTOW << ",";
|
|
ss << (int)esf.version << ",";
|
|
ss << (int)esf.initStatus1 << ",";
|
|
ss << (int)esf.initStatus2 << ",";
|
|
ss << (int)esf.fusionMode << ",";
|
|
ss << (int)esf.numSens << ",";
|
|
ss << esf.sensStatus[0] << ",";
|
|
ss << esf.sensStatus[1] << ",";
|
|
ss << esf.sensStatus[2] << ",";
|
|
ss << esf.sensStatus[3] << ",";
|
|
ss << esf.sensStatus[4] << ",";
|
|
ss << esf.sensStatus[5] << ",";
|
|
ss << esf.sensStatus[6] << ",";
|
|
|
|
ss << udrStatus;
|
|
|
|
return ss.str();
|
|
}
|
|
};
|
|
|
|
#endif
|