430 lines
13 KiB
C++
430 lines
13 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_I_H
|
|
#define SENSOR_DATA_I_H
|
|
#include <string>
|
|
#include <sstream>
|
|
#include <iomanip>
|
|
#include <cstdio>
|
|
#include "LpMatrix.h"
|
|
|
|
#if defined(_MSC_VER) && _MSC_VER < 1900
|
|
#define snprintf _snprintf
|
|
#else
|
|
#include <stdio.h> //sprintf
|
|
#endif
|
|
|
|
/* --- PRINTF_BYTE_TO_BINARY macro's --- */
|
|
//https://stackoverflow.com/questions/111928/is-there-a-printf-converter-to-print-in-binary-format
|
|
#define PRINTF_BINARY_PATTERN_INT8 "%c%c%c%c%c%c%c%c"
|
|
#define PRINTF_BYTE_TO_BINARY_INT8(i) \
|
|
(((i) & 0x80ll) ? '1' : '0'), \
|
|
(((i) & 0x40ll) ? '1' : '0'), \
|
|
(((i) & 0x20ll) ? '1' : '0'), \
|
|
(((i) & 0x10ll) ? '1' : '0'), \
|
|
(((i) & 0x08ll) ? '1' : '0'), \
|
|
(((i) & 0x04ll) ? '1' : '0'), \
|
|
(((i) & 0x02ll) ? '1' : '0'), \
|
|
(((i) & 0x01ll) ? '1' : '0')
|
|
|
|
#define PRINTF_BYTE_TO_BINARY_INT16(i) \
|
|
PRINTF_BYTE_TO_BINARY_INT8((i) >> 8), PRINTF_BYTE_TO_BINARY_INT8(i)
|
|
#define PRINTF_BYTE_TO_BINARY_INT32(i) \
|
|
PRINTF_BYTE_TO_BINARY_INT16((i) >> 16), PRINTF_BYTE_TO_BINARY_INT16(i)
|
|
#define PRINTF_BYTE_TO_BINARY_INT64(i) \
|
|
PRINTF_BYTE_TO_BINARY_INT32((i) >> 32), PRINTF_BYTE_TO_BINARY_INT32(i)
|
|
|
|
#define PRINTF_BINARY_PATTERN_INT16_PP \
|
|
PRINTF_BINARY_PATTERN_INT8 " " PRINTF_BINARY_PATTERN_INT8
|
|
#define PRINTF_BINARY_PATTERN_INT32_PP \
|
|
PRINTF_BINARY_PATTERN_INT16_PP " " PRINTF_BINARY_PATTERN_INT16_PP
|
|
#define PRINTF_BINARY_PATTERN_INT64_PP \
|
|
PRINTF_BINARY_PATTERN_INT32_PP " " PRINTF_BINARY_PATTERN_INT32_PP
|
|
|
|
#define PRINTF_BINARY_PATTERN_INT16 \
|
|
PRINTF_BINARY_PATTERN_INT8 PRINTF_BINARY_PATTERN_INT8
|
|
#define PRINTF_BINARY_PATTERN_INT32 \
|
|
PRINTF_BINARY_PATTERN_INT16 PRINTF_BINARY_PATTERN_INT16
|
|
/* --- end macros --- */
|
|
|
|
|
|
struct IG1ImuDataI
|
|
{
|
|
static const int MAX_DATA_SIZE = 50;
|
|
unsigned int timestamp;
|
|
int dataSize;
|
|
float data[MAX_DATA_SIZE];
|
|
LpVector3f accRaw;
|
|
LpVector3f accCalibrated;
|
|
LpVector3f gyroIRaw;
|
|
LpVector3f gyroIBiasCalibrated;
|
|
LpVector3f gyroIAlignmentCalibrated;
|
|
LpVector3f gyroIIRaw;
|
|
LpVector3f gyroIIBiasCalibrated;
|
|
LpVector3f gyroIIAlignmentCalibrated;
|
|
LpVector3f magRaw;
|
|
LpVector3f magCalibrated;
|
|
LpVector3f angularVelocity;
|
|
LpVector4f quaternion;
|
|
LpVector3f euler;
|
|
LpVector3f linAcc;
|
|
float pressure;
|
|
float altitude;
|
|
float temperature;
|
|
|
|
IG1ImuDataI()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
void reset()
|
|
{
|
|
dataSize = 0;
|
|
timestamp = 0;
|
|
for (int i = 0; i < MAX_DATA_SIZE; ++i)
|
|
data[i] = 0.0f;
|
|
|
|
vectZero3x1(&accRaw);
|
|
vectZero3x1(&accCalibrated);
|
|
vectZero3x1(&gyroIRaw);
|
|
vectZero3x1(&gyroIBiasCalibrated);
|
|
vectZero3x1(&gyroIAlignmentCalibrated);
|
|
vectZero3x1(&gyroIIRaw);
|
|
vectZero3x1(&gyroIIBiasCalibrated);
|
|
vectZero3x1(&gyroIIAlignmentCalibrated);
|
|
|
|
vectZero3x1(&magRaw);
|
|
vectZero3x1(&magCalibrated);
|
|
|
|
vectZero3x1(&angularVelocity);
|
|
vectZero4x1(&quaternion);
|
|
vectZero3x1(&euler);
|
|
vectZero3x1(&linAcc);
|
|
pressure = 0.0f;
|
|
altitude = 0.0f;
|
|
temperature = 0.0f;
|
|
}
|
|
};
|
|
|
|
struct IG1InfoI
|
|
{
|
|
std::string firmwareInfo;
|
|
std::string deviceName;
|
|
std::string serialNumber;
|
|
std::string filterVersion;
|
|
bool iapCheckStatus;
|
|
|
|
IG1InfoI()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
|
|
void reset()
|
|
{
|
|
firmwareInfo = "";
|
|
deviceName = "";
|
|
serialNumber = "";
|
|
filterVersion = "";
|
|
iapCheckStatus = 0;
|
|
}
|
|
|
|
|
|
std::string toString()
|
|
{
|
|
std::stringstream ss;
|
|
int w = 20;
|
|
ss << std::setw(w) << std::left << "Device name: " << deviceName << "\n";
|
|
ss << std::setw(w) << std::left << "Firmware info: " << firmwareInfo << "\n";
|
|
ss << std::setw(w) << std::left << "Filter version: " << filterVersion << "\n";
|
|
ss << std::setw(w) << std::left << "Serial no.: " << serialNumber << "\n";
|
|
ss << std::setw(w) << std::left << "IAP check status: " << (iapCheckStatus ? "Ready" : "Not Ready") << "\n";
|
|
return ss.str();
|
|
}
|
|
};
|
|
|
|
struct IG1SettingsI
|
|
{
|
|
uint32_t transmitDataConfig;
|
|
uint32_t sensorId;
|
|
uint32_t internalProcessingFrequency;
|
|
uint32_t dataStreamFrequency;
|
|
uint32_t useRadianOutput;
|
|
uint32_t enableGyroAutocalibration;
|
|
float gyroThreshold;
|
|
|
|
// Acc
|
|
uint32_t accRange;
|
|
|
|
// Gyro
|
|
uint32_t gyroRange;
|
|
|
|
// Mag
|
|
uint32_t magRange;
|
|
float magCalibrationTimeout;
|
|
|
|
// Filter
|
|
uint32_t filterMode;
|
|
|
|
// Uart communication
|
|
uint32_t uartBaudrate;
|
|
uint32_t uartDataFormat;
|
|
uint32_t uartDataPrecision;
|
|
char uartAsciiStart;
|
|
char uartAsciiStop;
|
|
|
|
// CAN Communication
|
|
uint32_t canStartId;
|
|
uint32_t canBaudrate;
|
|
uint32_t canDataPrecision; // Fix point 16bit or Floating point
|
|
uint32_t canMode; // Sequential / CANOpen
|
|
uint32_t canMapping[16];
|
|
uint32_t canHeartbeatTime;
|
|
|
|
//offset mode
|
|
uint32_t offsetMode;
|
|
|
|
//gps
|
|
uint32_t gpsTransmitDataConfig[2];
|
|
|
|
|
|
IG1SettingsI()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
void reset()
|
|
{
|
|
// general
|
|
transmitDataConfig = 0;
|
|
sensorId = 0;
|
|
dataStreamFrequency = 0;
|
|
useRadianOutput = 0;
|
|
enableGyroAutocalibration = 0;
|
|
gyroThreshold = 0;
|
|
|
|
// Acc
|
|
accRange = 0;
|
|
|
|
// Gyro
|
|
gyroRange = 0;
|
|
|
|
// Mag
|
|
magRange = 0;
|
|
magCalibrationTimeout = 0;
|
|
|
|
// Filter parameters
|
|
filterMode = 0;
|
|
|
|
// uart communication
|
|
uartBaudrate = 0;
|
|
uartDataFormat = 0;
|
|
uartDataPrecision = 0;
|
|
uartAsciiStart = '$';
|
|
uartAsciiStop = '\n';
|
|
|
|
// CAN Communication
|
|
canStartId = 0;
|
|
canBaudrate = 0;
|
|
canDataPrecision = 0; // Fix point 16bit or Floating point
|
|
canMode = 0; // Sequential / CANOpen
|
|
memset(canMapping, 0, sizeof(canMapping[0]) * 16);
|
|
canHeartbeatTime = 0;
|
|
|
|
//offset mode
|
|
offsetMode = 0;
|
|
|
|
//gps
|
|
memset(gpsTransmitDataConfig, 0, sizeof(gpsTransmitDataConfig[0]) * 2);
|
|
}
|
|
|
|
std::string toString()
|
|
{
|
|
int w = 38;
|
|
std::stringstream ss;
|
|
// general
|
|
ss << "=== General === \n";
|
|
ss << "Transmit Data: " << transmitDataConfig << " (" << uint32ToBinaryPP(transmitDataConfig) << ")\n";
|
|
ss << std::setw(w) << std::left << "Sensor ID: " << sensorId << "\n";
|
|
ss << std::setw(w) << std::left << "Data stream freq: " << dataStreamFrequency << "Hz\n";
|
|
ss << std::setw(w) << std::left << "Gyro output unit: " << (useRadianOutput ? "radian" : "deg") << "\n";
|
|
ss << std::setw(w) << std::left << "Gyro autocalibration: " << (enableGyroAutocalibration ? "Enable" : "Disable") << "\n";
|
|
ss << std::setw(w) << std::left << "Gyro threshold: " << gyroThreshold << "dps\n";
|
|
|
|
ss << "=== Acc === \n";
|
|
ss << std::setw(w) << std::left << "Acc range: " << accRange << "G\n";
|
|
|
|
ss << "=== Gyro === \n";
|
|
ss << std::setw(w) << std::left << "Gyro range: " << gyroRange << "dps\n";
|
|
|
|
ss << "=== Mag === \n";
|
|
ss << std::setw(w) << std::left << "Mag range: " << magRange << "Gauss\n";
|
|
ss << std::setw(w) << std::left << "Mag CalibrationTimeout: " << magCalibrationTimeout << " s\n";
|
|
|
|
ss << "=== Filter === \n";
|
|
ss << std::setw(w) << std::left << "Filter mode: " << filterMode << "\n";
|
|
|
|
ss << "=== Uart === \n";
|
|
ss << std::setw(w) << std::left << "Uart baudrate: " << uartBaudrate << "\n";
|
|
ss << std::setw(w) << std::left << "Uart data format: " << (uartDataFormat ? "ASCII" : "LPBus") << "\n";
|
|
ss << std::setw(w) << std::left << "Uart data precision: " << (uartDataPrecision ? "Floating point" : "Fixed point") << "\n";
|
|
ss << std::setw(w) << std::left << "Uart ascii start: " << std::hex << "0x" << (int)uartAsciiStart << "\n";
|
|
ss << std::setw(w) << std::left << "Uart ascii stop: " << std::hex << "0x" << (int)uartAsciiStop << "\n";
|
|
ss << std::dec;
|
|
|
|
ss << "=== CAN === \n";
|
|
ss << std::setw(w) << std::left << "CAN Start ID: " << canStartId << std::hex << "(0x" << canStartId << std::dec << ")" << "\n";
|
|
ss << std::setw(w) << std::left << "CAN baudrate: " << canBaudrate << "\n";
|
|
ss << std::setw(w) << std::left << "CAN data precision: " << (canDataPrecision ? "Floating point" : "Fixed point") << "\n";
|
|
ss << std::setw(w) << std::left << "CAN Mode: " << (canMode ? "Sequential" : "CANOpen") << "\n";
|
|
|
|
ss << std::setw(w / 2) << std::left << "CAN Mapping:";
|
|
for (int i = 0; i < 16; ++i)
|
|
{
|
|
ss << canMapping[i] << " ";
|
|
}
|
|
ss << "\n";
|
|
ss << std::setw(w) << std::left << "CAN Heartbeat: " << canHeartbeatTime << "\n";
|
|
|
|
ss << "=== Offset === \n";
|
|
ss << std::setw(w) << std::left << "Offset Mode: " << offsetMode << "\n";
|
|
|
|
ss << "=== GPS === \n";
|
|
ss << "GPS Transmit Data0: " << gpsTransmitDataConfig[0] << " (" << uint32ToBinaryPP(gpsTransmitDataConfig[0]) << ")\n";
|
|
ss << "GPS Transmit Data1: " << gpsTransmitDataConfig[1] << " (" << uint32ToBinaryPP(gpsTransmitDataConfig[1]) << ")\n";
|
|
|
|
return ss.str();
|
|
}
|
|
|
|
std::string uint32ToBinaryPP(uint32_t data)
|
|
{
|
|
char binaryFormat[36] = { 0 };
|
|
snprintf(binaryFormat, 36, PRINTF_BINARY_PATTERN_INT32_PP, PRINTF_BYTE_TO_BINARY_INT32(data));
|
|
return std::string(binaryFormat);
|
|
}
|
|
|
|
std::string uint32ToBinary(uint32_t data)
|
|
{
|
|
char binaryFormat[36] = { 0 };
|
|
snprintf(binaryFormat, 36, PRINTF_BINARY_PATTERN_INT32, PRINTF_BYTE_TO_BINARY_INT32(data));
|
|
return std::string(binaryFormat);
|
|
}
|
|
};
|
|
|
|
////////////////////////////////
|
|
// GPS Data
|
|
////////////////////////////////
|
|
typedef struct _NAV_PVT
|
|
{
|
|
uint32_t iTOW;
|
|
uint16_t year;
|
|
uint8_t month;
|
|
uint8_t day;
|
|
uint8_t hour;
|
|
uint8_t min;
|
|
uint8_t sec;
|
|
uint8_t valid; //0xF3 Means "Time(Mask:0x02) and Date(Mask:0x01) is valid"
|
|
uint32_t tAcc;
|
|
int32_t nano; //unit:ns
|
|
uint8_t fixType; //enum fixType;
|
|
uint8_t flags; //differential correction; head of Vehicle is valid
|
|
uint8_t flags2;
|
|
uint8_t numSV;
|
|
int32_t longitude;
|
|
int32_t latitude;
|
|
int32_t height;
|
|
int32_t hMSL;
|
|
uint32_t hAcc;
|
|
uint32_t vAcc;
|
|
int32_t velN;
|
|
int32_t velE;
|
|
int32_t velD;
|
|
int32_t gSpeed;
|
|
int32_t headMot;
|
|
uint32_t sAcc;
|
|
uint32_t headAcc;
|
|
uint16_t pDOP;
|
|
int32_t headVeh;
|
|
|
|
} NAV_PVT;
|
|
|
|
typedef struct _NAV_ATT
|
|
{
|
|
uint32_t iTOW;
|
|
uint8_t version;
|
|
int32_t roll;
|
|
int32_t pitch;
|
|
int32_t heading;
|
|
uint32_t accRoll;
|
|
uint32_t accPitch;
|
|
uint32_t accHeading;
|
|
|
|
} NAV_ATT;
|
|
|
|
typedef struct _ESF_STATUS
|
|
{
|
|
uint32_t iTOW;
|
|
uint8_t version;
|
|
uint8_t initStatus1;
|
|
uint8_t initStatus2;
|
|
uint8_t fusionMode;
|
|
uint8_t numSens;
|
|
uint32_t sensStatus[7];
|
|
|
|
} ESF_STATUS;
|
|
|
|
struct IG1GpsDataI
|
|
{
|
|
unsigned int timestamp;
|
|
NAV_PVT pvt;
|
|
NAV_ATT att;
|
|
ESF_STATUS esf;
|
|
unsigned int udrStatus;
|
|
|
|
IG1GpsDataI()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
void reset()
|
|
{
|
|
timestamp = 0;
|
|
memset(&pvt, 0, sizeof(NAV_PVT));
|
|
memset(&att, 0, sizeof(NAV_ATT));
|
|
memset(&esf, 0, sizeof(ESF_STATUS));
|
|
udrStatus = 0;
|
|
}
|
|
|
|
};
|
|
|
|
#endif
|