houyongdi/UnmannedShip
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 14:50:01 +08:00
commit 36193aec44
1006 changed files with 163575 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/.gitignore
+3
View File
@@ -0,0 +1,3 @@
build/
bin/
.vscode/
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/CMakeLists.txt
+213
View File
@@ -0,0 +1,213 @@
#***********************************************************************
#* Copyright (C) 2019 LP-Research
#* All rights reserved.
#* Contact: LP-Research (info@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.
#**********************************************************************/
cmake_minimum_required(VERSION 2.4.6)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release" )
set(BUILD_ARCHITECTURE "32-bit" CACHE STRING "")
#set(BUILD_ARCHITECTURE "64-bit" CACHE STRING "")
set_property(CACHE BUILD_ARCHITECTURE PROPERTY STRINGS "32-bit" "64-bit")
if(CMAKE_BUILD_TYPE STREQUAL "Release")
project (LpmsIG1_OpenSourceLib)
endif()
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
project (LpmsIG1_OpenSourceLibD)
endif()
if(COMMAND cmake_policy)
cmake_policy(SET CMP0003 NEW)
cmake_policy(SET CMP0015 NEW)
cmake_policy(SET CMP0040 NEW)
endif(COMMAND cmake_policy)
if (${CMAKE_SYSTEM_NAME} MATCHES "Windows")
# Windows SDK
set(WINDOWS_SDK_PATH "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1A" CACHE STRING "")
# LpmsIG1
include_directories("./")
ADD_DEFINITIONS(-DUSE_EIGEN)
ADD_DEFINITIONS(-DNOMINMAX)
ADD_DEFINITIONS(-DEIGEN_DONT_ALIGN_STATICALLY)
ADD_DEFINITIONS(-D_WIN32_WINNT=0x05010200)
ADD_DEFINITIONS(-DWIN32_LEAN_AND_MEAN)
ADD_DEFINITIONS(-DPLATFORM_X86)
ADD_DEFINITIONS(-D_CRT_SECURE_NO_DEPRECATE)
ADD_DEFINITIONS(-DDLL_EXPORT)
if(CMAKE_BUILD_TYPE STREQUAL "Release")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /fp:fast /O2")
endif()
#link_directories("${WINDOWS_SDK_PATH}/Lib")
endif()
if (${CMAKE_SYSTEM_NAME} MATCHES "Windows")
set(sources
MicroMeasureWindows.cpp
SerialPort.cpp
LpmsIG1.cpp
LpMatrix.c
LpUtil.cpp
version.rc
)
elseif(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
set(sources
MicroMeasure.cpp
SerialPortLinux.cpp
LpmsIG1.cpp
LpMatrix.c
LpUtil.cpp
version.rc
)
set(headers_linux_install
LpMatrix.h
LpmsIG1Registers.h
SensorDataI.h
LpmsIG1I.h
LpLog.h
)
endif()
set(headers
MicroMeasure.h
SerialPort.h
SensorData.h
SensorDataI.h
LpUtil.h
LpmsIG1.h
LpmsIG1I.h
LpmsIG1Registers.h
LpMatrix.h
LpLog.h
)
# Silicon labs
set(WINDOWS_SL_PATH "E:/openSourceLibSiliconLabs/MCU/USBXpress_SDK/Library/Host/Windows" CACHE STRING "")
include_directories("${WINDOWS_SL_PATH}")
link_directories("${WINDOWS_SL_PATH}/x86")
find_library(SILABS_LIB SiUSBXp.lib PATHS "${WINDOWS_SL_PATH}/x86")
if (BUILD_ARCHITECTURE STREQUAL "32-bit")
set(LIBRARY_OUTPUT_PATH ./ CACHE STRING "")
set(EXECUTABLE_OUTPUT_PATH ./ CACHE STRING "")
endif()
if (BUILD_ARCHITECTURE STREQUAL "64-bit")
set(LIBRARY_OUTPUT_PATH ./ CACHE STRING "")
set(EXECUTABLE_OUTPUT_PATH ./ CACHE STRING "")
endif()
if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -std=gnu++11")
#SET ( CMAKE_CXX_FLAGS "-D_GLIBCXX_USE_CXX11_ABI=1" )
endif()
if (${CMAKE_SYSTEM_NAME} MATCHES "Windows")
if (BUILD_ARCHITECTURE STREQUAL "32-bit")
if(CMAKE_BUILD_TYPE STREQUAL "Release")
add_library(${CMAKE_PROJECT_NAME} SHARED ${sources} ${headers})
target_link_libraries(${CMAKE_PROJECT_NAME} Ws2_32.lib)
target_link_libraries(${CMAKE_PROJECT_NAME} SiUSBXp.lib)
endif()
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
add_library(${CMAKE_PROJECT_NAME} SHARED ${sources} ${headers})
target_link_libraries(${CMAKE_PROJECT_NAME} Ws2_32.lib)
target_link_libraries(${CMAKE_PROJECT_NAME} SiUSBXp.lib)
endif()
endif()
if (BUILD_ARCHITECTURE STREQUAL "64-bit")
if(CMAKE_BUILD_TYPE STREQUAL "Release")
add_library(${CMAKE_PROJECT_NAME} SHARED ${sources} ${headers})
target_link_libraries(${CMAKE_PROJECT_NAME} Ws2_32.lib)
target_link_libraries(${CMAKE_PROJECT_NAME} SiUSBXp.lib)
endif()
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
add_library(${CMAKE_PROJECT_NAME} SHARED ${sources} ${headers})
target_link_libraries(${CMAKE_PROJECT_NAME} Ws2_32.lib)
target_link_libraries(${CMAKE_PROJECT_NAME} SiUSBXp.lib)
endif()
endif()
endif()
if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
include(InstallRequiredSystemLibraries)
set(MAJOR_VERSION "0")
set(MINOR_VERSION "3")
set(PATCH_VERSION "0")
SET(CPACK_GENERATOR "DEB")
SET(CPACK_PACKAGE_NAME "libLpmsIG1_OpenSource")
set(CPACK_PACKAGE_VENDOR "LP-Research Inc. <www.lp-research.com>")
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Library for communicating and interfacing with LP-Research sensors.")
set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/SUMMARY.txt")
set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.txt")
set(CPACK_PACKAGE_VERSION_MAJOR "${MAJOR_VERSION}")
set(CPACK_PACKAGE_VERSION_MINOR "${MINOR_VERSION}")
set(CPACK_PACKAGE_VERSION_PATCH "${PATCH_VERSION}")
SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "H.E. YAP <yap@lp-research.com>") #required
include(CPack)
if(CMAKE_BUILD_TYPE STREQUAL "Release")
add_library(${CMAKE_PROJECT_NAME} SHARED ${sources} ${headers})
target_link_libraries(${CMAKE_PROJECT_NAME} pthread)
target_link_libraries(${CMAKE_PROJECT_NAME} rt)
target_link_libraries(${CMAKE_PROJECT_NAME} udev)
endif()
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
add_library(${CMAKE_PROJECT_NAME} SHARED ${sources} ${headers})
target_link_libraries(${CMAKE_PROJECT_NAME} pthread)
target_link_libraries(${CMAKE_PROJECT_NAME} rt)
target_link_libraries(${CMAKE_PROJECT_NAME} udev)
endif()
install(FILES ${headers_linux_install} DESTINATION include/lpsensor)
install(TARGETS ${CMAKE_PROJECT_NAME}
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
)
endif()
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LICENSE.txt
+27
View File
@@ -0,0 +1,27 @@
Copyright (C) 2019 LP-Research Inc.
All rights reserved.
Contact: LP-Research Inc.(info@lp-research.com)
Homepage: http://www.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.
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpLog.h
+78
View File
@@ -0,0 +1,78 @@
#ifndef LPLOG_H
#define LPLOG_H
#include <cstdio>
#include <cstdarg>
#include <string>
#include <stdint.h>
#include <time.h>
#include <sstream>
// Verbose level
enum {
VERBOSE_NONE,
VERBOSE_INFO,
VERBOSE_DEBUG
};
class LpLog
{
int verboseLevel;
public:
static LpLog& getInstance()
{
static LpLog instance; // Guaranteed to be destroyed.
return instance;
}
private:
LpLog() {
verboseLevel = VERBOSE_INFO;
};
public:
LpLog(LpLog const&) = delete;
void operator=(LpLog const&) = delete;
void setVerbose(int level)
{
verboseLevel = level;
}
void d(std::string tag, const char* str, ...)
{
if (verboseLevel < VERBOSE_DEBUG)
return;
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "DBUG", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
void i(std::string tag, const char* str, ...)
{
if (verboseLevel < VERBOSE_INFO)
return;
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
void e(std::string tag, const char* str, ...)
{
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "ERR", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
};
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpMatrix.c
+134
View File
@@ -0,0 +1,134 @@
/***********************************************************************
** Copyright (C) 2019 LP-Research
** All rights reserved.
** Contact: LP-Research (info@lp-research.com)
***********************************************************************/
#include "LpMatrix.h"
void createIdentity3x3(LpMatrix3x3f* dest)
{
matZero3x3(dest);
dest->data[0][0] = 1;
dest->data[1][1] = 1;
dest->data[2][2] = 1;
}
void createIdentity4x4(LpMatrix4x4f* dest)
{
matZero4x4(dest);
dest->data[0][0] = 1;
dest->data[1][1] = 1;
dest->data[2][2] = 1;
dest->data[3][3] = 1;
}
void matZero3x3(LpMatrix3x3f* dest)
{
dest->data[0][0] = 0;
dest->data[0][1] = 0;
dest->data[0][2] = 0;
dest->data[1][0] = 0;
dest->data[1][1] = 0;
dest->data[1][2] = 0;
dest->data[2][0] = 0;
dest->data[2][1] = 0;
dest->data[2][2] = 0;
}
void matZero3x4(LpMatrix3x4f* dest)
{
dest->data[0][0] = 0;
dest->data[0][1] = 0;
dest->data[0][2] = 0;
dest->data[0][3] = 0;
dest->data[1][0] = 0;
dest->data[1][1] = 0;
dest->data[1][2] = 0;
dest->data[1][3] = 0;
dest->data[2][0] = 0;
dest->data[2][1] = 0;
dest->data[2][2] = 0;
dest->data[2][3] = 0;
}
void matZero4x3(LpMatrix4x3f* dest)
{
dest->data[0][0] = 0;
dest->data[0][1] = 0;
dest->data[0][2] = 0;
dest->data[1][0] = 0;
dest->data[1][1] = 0;
dest->data[1][2] = 0;
dest->data[2][0] = 0;
dest->data[2][1] = 0;
dest->data[2][2] = 0;
dest->data[3][0] = 0;
dest->data[3][1] = 0;
dest->data[3][2] = 0;
}
void matZero4x4(LpMatrix4x4f* dest)
{
dest->data[0][0] = 0;
dest->data[0][1] = 0;
dest->data[0][2] = 0;
dest->data[0][3] = 0;
dest->data[1][0] = 0;
dest->data[1][1] = 0;
dest->data[1][2] = 0;
dest->data[1][3] = 0;
dest->data[2][0] = 0;
dest->data[2][1] = 0;
dest->data[2][2] = 0;
dest->data[2][3] = 0;
dest->data[3][0] = 0;
dest->data[3][1] = 0;
dest->data[3][2] = 0;
dest->data[3][3] = 0;
}
void vectZero3x1(LpVector3f* dest)
{
dest->data[0] = 0;
dest->data[1] = 0;
dest->data[2] = 0;
}
void vectZero4x1(LpVector4f* dest)
{
dest->data[0] = 0;
dest->data[1] = 0;
dest->data[2] = 0;
dest->data[3] = 0;
}
#ifdef __WIN32
#include "stdio.h"
void print4x4(LpMatrix4x4f m)
{
int i, j;
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) {
printf("%f, ", m.data[i][j]);
}
printf("\n");
}
}
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpMatrix.h
+76
View File
@@ -0,0 +1,76 @@
/***********************************************************************
** Copyright (C) 2019 LP-Research
** All rights reserved.
** Contact: LP-Research (info@lp-research.com)
***********************************************************************/
#ifndef LP_MATRIX
#define LP_MATRIX
#include <math.h>
#ifdef __IAR_SYSTEMS_ICC__
#include <stdint.h>
typedef union _float2int {
uint32_t u32_val;
float float_val;
} float2int;
#endif
typedef struct _LpMatrix3x3f {
float data[3][3];
} LpMatrix3x3f;
typedef struct _LpMatrix3x4f {
float data[3][4];
} LpMatrix3x4f;
typedef struct _LpMatrix4x3f {
float data[4][3];
} LpMatrix4x3f;
typedef struct _LpMatrix4x4f {
float data[4][4];
} LpMatrix4x4f;
typedef struct _LpVector3f {
float data[3];
} LpVector3f;
typedef struct _LpVector4f {
float data[4];
} LpVector4f;
#ifdef __IAR_SYSTEMS_ICC__
typedef struct _LpVector3i {
int16_t data[3];
} LpVector3i;
typedef struct _LpVector4i {
int16_t data[4];
} LpVector4i;
#endif
#ifdef __cplusplus
extern "C" {
#endif
void createIdentity3x3(LpMatrix3x3f* dest);
void createIdentity4x4(LpMatrix4x4f* dest);
void matZero3x3(LpMatrix3x3f* dest);
void matZero4x4(LpMatrix4x4f* dest);
void matZero3x4(LpMatrix3x4f* dest);
void matZero4x3(LpMatrix4x3f* dest);
void vectZero3x1(LpVector3f* dest);
void vectZero4x1(LpVector4f* dest);
#ifdef __WIN32
void print4x4(LpMatrix4x4f m)
#endif
#ifdef __cplusplus
}
#endif
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpUtil.cpp
+49
View File
@@ -0,0 +1,49 @@
#include "LpUtil.h"
#include <iomanip>
/*
void logd(std::string tag, const char* str, ...)
{
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %-8s] ", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
*/
const std::string currentDateTime(const char* format)
{
time_t now = time(0);
struct tm tstruct;
char buf[80];
tstruct = *localtime(&now);
// Visit http://en.cppreference.com/w/cpp/chrono/c/strftime
// for more information about date/time format
strftime(buf, sizeof(buf), format, &tstruct);
return buf;
}
const int currentDateTimeInt()
{
time_t now = time(0);
struct tm tstruct;
tstruct = *localtime(&now);
// Visit http://en.cppreference.com/w/cpp/chrono/c/strftime
// for more information about date/time format
int ret = (2000+tstruct.tm_year-100) * 10000 + (tstruct.tm_mon+1) * 100 + tstruct.tm_mday;
return ret;
}
std::string trimString(std::string s)
{
std::string ret = s;
//ret.resize(ret.find('\0'));
ret.erase(std::find(ret.begin(), ret.end(), '\0'), ret.end());
return ret;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpUtil.h
+73
View File
@@ -0,0 +1,73 @@
#ifndef UTIL_H
#define UTIL_H
#include <cstdio>
#include <cstdarg>
#include <string>
#include <stdint.h>
#include <time.h>
#include <sstream>
#include <algorithm>
#include "LpMatrix.h"
#define FORMAT_SPACE 0
#define FORMAT_CSV 1
#define FORMAT_YAML 2
union float2char {
float float_val;
uint8_t c[4];
};
union double2char {
double double_val;
uint8_t c[8];
};
union uint2char {
uint32_t int_val;
uint8_t c[4];
};
union uint162char {
uint16_t int_val;
uint8_t c[2];
};
union int2char {
int int_val;
uint8_t c[4];
};
union cArray2intArray
{
int16_t int_val[5];
uint8_t c[10];
};
union floatArray2char {
float float_val[6];
uint32_t uint32_val[6];
uint8_t c[24];
};
//void logd(std::string tag, const char* str, ...);
const std::string currentDateTime(const char* format);
const int currentDateTimeInt();
struct MyException : public std::exception
{
std::string s;
MyException(std::string ss) : s(ss) {}
~MyException() throw () {} // Updated
const char* what() const throw() { return s.c_str(); }
};
std::string trimString(std::string s);
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpmsIG1.cpp
+3120
View File
File diff suppressed because it is too large Load Diff
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpmsIG1.h
+830
View File
@@ -0,0 +1,830 @@
#ifndef LPMSIG1_H
#define LPMSIG1_H
#include <string>
#include <cstring>
#include <thread>
#include <iostream>
#include <fstream>
#ifdef _WIN32
#include <comutil.h>
#endif
#include <mutex>
#include <queue>
#include <vector>
#include <cstdio>
#include <sstream>
#include <iomanip>
#include "SerialPort.h"
#include "MicroMeasure.h"
#include "SensorData.h"
#include "LpmsIG1Registers.h"
#include "LpmsIG1I.h"
///////////////////////////////////////
// LPMod bus
///////////////////////////////////////
#define LPPACKET_MAX_BUFFER 512
enum {
PACKET_START,
PACKET_ADDRESS0,
PACKET_ADDRESS1,
PACKET_FUNCTION0,
PACKET_FUNCTION1,
PACKET_LENGTH0,
PACKET_LENGTH1,
PACKET_RAW_DATA,
PACKET_LRC_CHECK0,
PACKET_LRC_CHECK1,
PACKET_END0,
PACKET_END1
};
struct LPPacket
{
int rxState;
uint16_t address;
uint16_t length;
uint16_t function;
uint8_t data[LPPACKET_MAX_BUFFER];
uint16_t rawDataIndex;
uint16_t chksum;
uint16_t cs;
LPPacket()
{
reset();
}
void reset()
{
rxState = PACKET_START;
address = 0;
length = 0;
function = 0;
rawDataIndex = 0;
chksum = 0;
memset(data, 0, LPPACKET_MAX_BUFFER);
}
};
enum {
CONNECTION_STATE_DISCONNECTED,
CONNECTION_STATE_GET_SENSOR_INFO,
CONNECTION_STATE_WAITING_SENSOR_INFO,
CONNECTION_STATE_VALID_SENSOR_INFO,
CONNECTION_STATE_TDR_ERROR,
CONNECTION_STATE_FIRMWARE_UPDATE,
CONNECTION_STATE_CONNECTED
};
#define BYTE_START 0x3A
#define BYTE_END0 0x0D
#define BYTE_END1 0x0A
#define TDR_INVALID 0
#define TDR_VALID 1
#define TDR_UPDATING 2
#define TDR_ERROR 3
// Others
#define SAVE_DATA_LIMIT 720000 //2 hours at 100hz
#define SENSOR_DATA_QUEUE_SIZE 10
#define SENSOR_RESPONSE_QUEUE_SIZE 50
#define FIRMWARE_PACKET_LENGTH 256
// Timeout settings (us)
#define TIMEOUT_COMMAND_TIMER 10000 // 0.01 secs
#define TIMEOUT_TDR_STATUS 5000000 // 5 secs
#define TIMEOUT_FIRMWARE_UPDATE 2000000 // 2 secs
#define TIMEOUT_IDLE 5000000 // 5 secs
// sensor response logic
enum {
WAIT_IGNORE ,
WAIT_FOR_ACKNACK ,
WAIT_FOR_DATA ,
WAIT_FOR_TRANSMIT_DATA_REGISTER ,
WAIT_FOR_CALIBRATION_DATA ,
WAIT_FOR_SENSOR_SETTINGS ,
WAIT_FOR_LPBUS_DATA_PRECISION ,
WAIT_FOR_DEGRAD_OUTPUT ,
// Sensor Info
WAIT_FOR_SERIAL_NUMBER,
WAIT_FOR_SENSOR_MODEL,
WAIT_FOR_FIRMWARE_INFO,
WAIT_FOR_FILTER_VERSION,
WAIT_FOR_IAP_CHECKSTATUS,
// Sensor Settings
WAIT_INTERNAL_PROCESSING_FREQ
};
#define SYSFS_GPIO_DIR "/sys/class/gpio"
#define DEFAULT_GPIO_TOGGLE_WAIT_MS 400 //at 230400 bps
#define MAX_BUF 64
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
int expectedResponse;
int retryCount;
bool sent;
bool processed;
IG1Command(short cmd = 0, int response = WAIT_FOR_ACKNACK)
{
command = cmd;
dataLength = 0;
memset(data.c, 0, 256);
expectedResponse = response;
retryCount = 0;
processed = false;
sent = false;
}
void setData(const LpVector3f &d)
{
dataLength = 12;
memcpy(data.c, &d, dataLength);
}
void setData(const LpMatrix3x3f &d)
{
dataLength = 36;
memcpy(data.c, &d, dataLength);
}
};
class IG1 : public IG1I
{
public:
/////////////////////////////////////////////
// Constructor/Destructor
/////////////////////////////////////////////
IG1(void);
//IG1(int portno, int baudrate);
IG1(const IG1 &obj);
~IG1(void);
void init();
void release() { disconnect(); delete this; };
/////////////////////////////////////////////
// Connection
/////////////////////////////////////////////
void setPCBaudrate(int baud);
#ifdef _WIN32
void setPCPort(int port);
int connect(int _portno, int _baudrate);
int connect(std::string sensorName, int _baudrate);
#else
void setPCPort(std::string port);
int connect(std::string _portno, int _baudrate);
//int connect(std::string sensorName);
#endif
bool disconnect();
int getReconnectCount();
void setConnectionMode(int mode);
void setConnectionInterface(int interface);
void setControlGPIOForRs485(int gpio);
void setControlGPIOToggleWaitMs(unsigned int ms);
/////////////////////////////////////////////
// Commands:
// - send command to sensor
// - commandXXX functions will add appropriate commands to command queue
// and processed in the internal thread
/////////////////////////////////////////////
/*
Function: set sensor to command mode
Parameters: none
Returns: none
*/
void commandGotoCommandMode(void);
/*
Function: set sensor to streaming mode
Parameters: none
Returns: none
*/
void commandGotoStreamingMode(void);
/*
Function:
- send command to sensor to get sensor ID
Parameters: none
Returns: int32
*/
void commandGetSensorID(void);
/*
Function:
- send command to sensor to set sensor ID
Parameters: int32
Returns: none
*/
void commandSetSensorID(uint32_t id);
/*
Function:
- send command to sensor to get sensor Frenquency
- hasInfo() will return true once sensor info is available
Parameters: none
Returns: none
*/
void commandGetSensorFrequency(void);
/*
Function:
- send command to sensor to set sensor Frenquency
Parameters: int32
Returns: none
*/
void commandSetSensorFrequency(uint32_t freq);
void commandGetSensorGyroRange(void);
void commandSetSensorGyroRange(uint32_t range);
void commandStartGyroCalibration(void);
void commandGetSensorAccRange(void);
void commandSetSensorAccRange(uint32_t range);
void commandGetSensorMagRange(void);
void commandSetSensorMagRange(uint32_t range);
void commandGetSensorUseRadianOutput(void);
void commandSetSensorUseRadianOutput(bool b);
void commandStartMagCalibration(void);
void commandStopMagCalibration(void);
void commandGetMagRefrence(void);
void commandSetMagRefrence(uint32_t reference);
void commandGetSensorMagCalibrationTimeout(void);
void commandSetSensorMagCalibrationTimeout(float second);
/*
Function:
- send command to sensor to get sensor info
- hasInfo() will return true once sensor info is available
Parameters: none
Returns: none
*/
void commandGetSensorInfo(void);
void commandSaveParameters(void);
void commandResetFactory(void);
/*
Function:
- send command to set transmit data
Parameters: as defined in transmit data register TDR
#define TDR_XXX_ENABLED
Returns: none
*/
void commandSetTransmitData(uint32_t config);
// CAN
void commandGetCanStartId(void);
void commandSetCanStartId(uint32_t data);
void commandGetCanBaudrate(void);
void commandSetCanBaudrate(uint32_t baudrate);
void commandGetCanDataPrecision(void);
void commandSetCanDataPrecision(uint32_t data);
void commandGetCanChannelMode(void);
void commandSetCanChannelMode(uint32_t data);
void commandGetCanMapping(void);
void commandSetCanMapping(uint32_t map[16]);
void commandGetCanHeartbeat(void);
void commandSetCanHeartbeat(uint32_t data);
//Filter
void commandSetFilterMode(uint32_t data);
void commandGetFilterMode(void);
void commandSetGyroAutoCalibration(bool enable);
void commandGetGyroAutoCalibration(void);
void commandSetOffsetMode(uint32_t data);
void commandResetOffsetMode(void);
//GPS
void commandSaveGPSState(void);
void commandClearGPSState(void);
void commandSetGpsTransmitData(uint32_t data, uint32_t data1);
void commandGetGpsTransmitData(void);
// Uart
void commandSetUartBaudRate(uint32_t data);
void commandGetUartBaudRate(void);
void commandSetUartDataFormat(uint32_t data);
void commandGetUartDataFormat(void);
void commandSetUartDataPrecision(uint32_t data);
void commandGetUartDataPrecision(void);
/*
Function:
- send command to sensor directly bypassing internal command queue
Parameters:
- cmd: command register
- length: length of data to send
- data: pointer to data buffer
Returns: none
*/
void sendCommand(uint16_t cmd, uint16_t length, uint8_t* data);
/////////////////////////////////////////////
// Sensor interface
/////////////////////////////////////////////
// General
/*
Function: Specify sensor to streaming or command mode after connect
Parameters:
- SENSOR_MODE_COMMAND: start up in command mode
- SENSOR_MODE_STREAMING: start up in streaming mode
Returns: none
*/
void setStartupSensorMode(int mode);
/*
Function: enable auto reconnection
Parameters:
- true: enable
- false: disable
Returns: none
*/
void setAutoReconnectStatus(bool b);
/*
Function: get enable auto reconnection status
Parameters: none
Returns:
- true: enable
- false: disable
*/
bool getAutoReconnectStatus(void);
/*
Function: get status of sensor
Parameters:none
Returns: as defined in STATUS_XXX
#define STATUS_DISCONNECTED 0
#define STATUS_CONNECTING 1
#define STATUS_CONNECTED 2
#define STATUS_CONNECTION_ERROR 3
#define STATUS_DATA_TIMEOUT 4
#define STATUS_UPDATING 5
*/
int getStatus(void);
/*
Function: get frequency of incoming data
Parameters:none
Returns: data frequency in Hz
*/
float getDataFrequency(void);
// info
/*
Function: has new sensor info. consume once
Parameters:none
Returns:
- true: new sensor info available
- false: no new sensor info available
*/
bool hasInfo(void);
/*
Function: get sensor info
Parameters: reference IG1Info
Returns: none
*/
void getInfo(IG1InfoI &info);
// settings
/*
Function: has new sensor settings. consume once
Parameters:none
Returns:
- true: new sensor settings available
- false: no new sensor settings available
*/
bool hasSettings(void);
/*
Function: get sensor settings
Parameters: reference IG1Settings
Returns: none
*/
void getSettings(IG1SettingsI &settings);
// response from sensor
/*
Function: has feedback from sensor after command sent
Parameters : none
Returns : number of feedback in queue
*/
int hasResponse(void);
/*
Function: get feedback from internal response queue
Parameters : string reference
Returns :
- true : feedback valid and available
- false : no feedback available
*/
bool getResponse(std::string &s);
// Imu data
/*
Function: has new imu data
Parameters:none
Returns: number of imu data available in queue
*/
int hasImuData(void);
/*
Function: get imu data from queue
Parameters : IG1ImuData reference
Returns :
- true : data pop from queue
- false : queue is empty, return latest imu data
*/
bool getImuData(IG1ImuDataI &sd);
// gps data
/*
Function: has new gps data
Parameters:none
Returns: number of gps data available in queue
*/
int hasGpsData();
/*
Function: get gps data from queue
Parameters : IG1ImuData reference
Returns :
- true : data pop from queue
- false : queue is empty, return latest gps data
*/
bool getGpsData(IG1GpsDataI &data);
// Sensor data
/*
Function: is acc raw data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isAccRawEnabled(void);
/*
Function: is calibrated acc data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isAccCalibratedEnabled(void);
/*
Function: is gyro raw (high accuracy) data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isGyroIRawEnabled(void);
/*
Function: is bias calibrated gyro (high accuracy) data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isGyroIBiasCalibratedEnabled(void);
/*
Function: is alignment + bias calibrated gyro (high accuracy) data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isGyroIAlignCalibratedEnabled(void);
/*
Function: is gyro raw data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isGyroIIRawEnabled(void);
/*
Function: is bias calibrated gyro data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isGyroIIBiasCalibratedEnabled(void);
/*
Function: is alignment + bias calibrated gyro data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isGyroIIAlignCalibratedEnabled(void);
/*
Function: is mag raw data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isMagRawEnabled(void);
/*
Function: is calibrated mag data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isMagCalibratedEnabled(void);
/*
Function: is angular velocity data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isAngularVelocityEnabled(void);
/*
Function: is quaternion data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isQuaternionEnabled(void);
/*
Function: is euler data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isEulerEnabled(void);
/*
Function: is linear acceleration data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isLinearAccelerationEnabled(void);
/*
Function: is pressure data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isPressureEnabled(void);
/*
Function: is altitude data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isAltitudeEnabled(void);
/*
Function: is temperature data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
bool isTemperatureEnabled(void);
/*
Function: is useRadianOutput
Parameters: void
Returns:
- true: use radian output
- false: use degree output
*/
bool isUseRadianOutput(void);
void setSensorDataQueueSize(unsigned int size);
int getSensorDataQueueSize(void);
int getFilePages();
bool getIsUpdatingStatus();
// Error
std::string getLastErrMsg();
/*
Function: Set library verbosity output
Parameters:
- VERBOSE_NONE
- VERBOSE_INFO
- VERBOSE_DEBUG
Returns: void
*/
void setVerbose(int level);
///////////////////////////////////////////
//Data saving
///////////////////////////////////////////
bool startDataSaving();
bool stopDataSaving();
// imu
int getSavedImuDataCount();
IG1ImuData getSavedImuData(int i);
// gps
int getSavedGpsDataCount();
IG1GpsData getSavedGpsData(int i);
private:
void triggerRS485CommandMode();
void processCommandQueue();
void processIncomingData();
void clearSensorDataQueue();
void updateData();
bool parseModbusByte(int n);
bool parseASCII(int n);
bool parseSensorData(const LPPacket &p);
void addSensorResponseToQueue(std::string);
void addCommandQueue(IG1Command cmd);
void clearCommandQueue();
void gpioInit();
void gpioDeinit();
int gpioExport(unsigned int gpio);
int gpioUnexport(unsigned int gpio);
int gpioSetDirection(unsigned int gpio, unsigned int out_flag);
int gpioSetValue(unsigned int gpio, unsigned int value);
int gpioGetValue(unsigned int gpio, unsigned int *value);
void cleanup();
private:
// General settings
bool autoReconnect;
int reconnectCount;
// Serial settings
Serial sp;
#ifdef _WIN32
int portno;
#else
std::string portno;
#endif
int baudrate;
std::string sensorId;
int connectionMode; // VCP / USB Xpress
int connectionInterface; // 232-TTL/485
// RS485 settings
int ctrlGpio;
unsigned int ctrlGpioToggleWaitMs;
// LPBus
LPPacket packet;
bool ackReceived;
bool nackReceived;
bool dataReceived;
// Internal thread
std::thread *t;
bool isStopThread;
int connectionState;
MicroMeasure mmDataFreq;
MicroMeasure mmDataIdle;
MicroMeasure mmUpdating;
// Sensor
int startupSensorMode;
int currentSensorMode;
int sensorStatus;
std::string errMsg;
long long timeoutThreshold;
unsigned char incomingData[INCOMING_DATA_MAX_LENGTH];
// Stats
float incomingDataRate;
// Command queue
std::mutex mLockCommandQueue;
std::queue<IG1Command> commandQueue; //TODO use custom object to accomodate command data
MicroMeasure mmCommandTimer;
long long lastSendCommandTime;
// Response
std::mutex mLockSensorResponseQueue;
std::queue<std::string> sensorResponseQueue;
// Info
bool hasNewInfo;
IG1Info sensorInfo;
// Sensor settings
bool hasNewSettings;
IG1AdvancedSettings sensorSettings;
MicroMeasure mmTransmitDataRegisterStatus;
bool useNewChecksum;
// Imu data
IG1ImuData latestImuData;
std::mutex mLockImuDataQueue;
std::queue<IG1ImuData> imuDataQueue;
unsigned int sensorDataQueueSize;
// Gps data
IG1GpsData latestGpsData;
std::mutex mLockGpsDataQueue;
std::queue<IG1GpsData> gpsDataQueue;
// Firmware update
std::ifstream ifs;
long long firmwarePages;
int firmwarePageSize;
unsigned long firmwareRemainder;
uint32_t fileCheckSum;
uint16_t updateCommand;
unsigned char cBuffer[1024];
// Data saving
// Imu
bool isDataSaving;
std::mutex mLockSavedImuDataQueue;
std::vector<IG1ImuData> savedImuDataBuffer;
int savedImuDataCount;
// GPS
std::mutex mLockSavedGpsDataQueue;
std::vector<IG1GpsData> savedGpsDataBuffer;
int savedGpsDataCount;
// debug
MicroMeasure mmDebug;
LpLog& log = LpLog::getInstance();
};
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpmsIG1I.h
+803
View File
@@ -0,0 +1,803 @@
#ifndef LPMSIG1_I_H
#define LPMSIG1_I_H
#ifdef _WIN32
#include "windows.h"
#endif
#include "SensorDataI.h"
#include "LpLog.h"
// Note: To be deprecated
#define COMMUNICATION_INTERFACE_232 0
#define COMMUNICATION_INTERFACE_485 1
// Connection Interface
#define CONNECTION_INTERFACE_RS232_USB 0
#define CONNECTION_INTERFACE_RS485 1
// Sensor status
#define STATUS_DISCONNECTED 0
#define STATUS_CONNECTING 1
#define STATUS_CONNECTED 2
#define STATUS_CONNECTION_ERROR 3
#define STATUS_DATA_TIMEOUT 4
#define STATUS_UPDATING 5
// Sensor mode
#define SENSOR_MODE_COMMAND 0
#define SENSOR_MODE_STREAMING 1
class IG1I
{
public:
const std::string TAG = "IG1";
/////////////////////////////////////////////
// Constructor/Destructor
/////////////////////////////////////////////
~IG1I(void) {};
virtual void release() = 0;
/////////////////////////////////////////////
// Connection
/////////////////////////////////////////////
virtual void setPCBaudrate(int baud) =0;
#ifdef _WIN32
virtual void setPCPort(int port) =0;
#else
virtual void setPCPort(std::string port) = 0;
#endif
#ifdef _WIN32
/*
Function: Connect to sensor
Parameters:
- _portno: In windows, this the number attached to COM port. eg 3 for COM3
- baudrate: serial baudrate
Returns: sensor status
- STATUS_DISCONNECTED
- STATUS_CONNECTING
- STATUS_CONNECTED
- STATUS_CONNECTION_ERROR
- STATUS_DATA_TIMEOUT
- STATUS_UPDATING
*/
virtual int connect(int _portno, int _baudrate) = 0;
/*
Function: Connect to sensor
Parameters:
- sensorName: sensor ID
- baudrate: serial baudrate
Returns: sensor status
- STATUS_DISCONNECTED
- STATUS_CONNECTING
- STATUS_CONNECTED
- STATUS_CONNECTION_ERROR
- STATUS_DATA_TIMEOUT
- STATUS_UPDATING
*/
virtual int connect(std::string sensorName, int _baudrate) = 0;
#else
/*
Function: Connect to sensor
Parameters:
- _portno: In linux, this value can be serial mount point /dev/ttyXXX
or sensor ID
- baudrate: serial baudrate
Returns: sensor status
- STATUS_DISCONNECTED
- STATUS_CONNECTING
- STATUS_CONNECTED
- STATUS_CONNECTION_ERROR
- STATUS_DATA_TIMEOUT
- STATUS_UPDATING
*/
virtual int connect(std::string _portno, int _baudrate) = 0;
#endif
virtual bool disconnect() = 0;
virtual int getReconnectCount() = 0;
/*
Function: Set connection interface
Parameters:
- interface:
CONNECTION_INTERFACE_RS232_USB for USB/RS232 connection
CONNECTION_INTERFACE_RS485 for RS485 connection
*/
virtual void setConnectionInterface(int interface) = 0;
/*
Function: Set gpio control pin
Parameters:
- interface:
CONNECTION_INTERFACE_RS232_USB for USB/RS232 connection
CONNECTION_INTERFACE_RS485 for RS485 connection
*/
virtual void setControlGPIOForRs485(int gpio) = 0;
/*
Function: Set time to wait before toggling io pin during host TX transfer.
Only applies to RS485 connection
Parameters: wait time in ms
Return: none
*/
virtual void setControlGPIOToggleWaitMs(unsigned int ms) = 0;
/////////////////////////////////////////////
// Commands:
// - send command to sensor
// - commandXXX functions will add appropriate commands to command queue
// and processed in the internal thread
/////////////////////////////////////////////
/*
Function: set sensor to command mode
Parameters: none
Returns: none
*/
virtual void commandGotoCommandMode(void) = 0;
/*
Function: set sensor to streaming mode
Parameters: none
Returns: none
*/
virtual void commandGotoStreamingMode(void) = 0;
/*
Function:
- send command to sensor to get sensor ID
- hasSettings() will return true once sensor info is available
Parameters: none
Returns: none
*/
virtual void commandGetSensorID(void) = 0;
/*
Function:
- send command to sensor to set sensor ID
Parameters: int32
Returns: none
*/
virtual void commandSetSensorID(uint32_t id) = 0;
/*
Function:
- send command to sensor to get/set sensor Frenquency
- hasInfo() will return true once sensor info is available
Parameters:
#define DATA_STREAM_FREQ_5HZ 5
#define DATA_STREAM_FREQ_10HZ 10
#define DATA_STREAM_FREQ_50HZ 50
#define DATA_STREAM_FREQ_100HZ 100
#define DATA_STREAM_FREQ_250HZ 250
#define DATA_STREAM_FREQ_500HZ 500
#define DATA_STREAM_FREQ_1000HZ 1000
Returns: none
*/
virtual void commandGetSensorFrequency(void) = 0;
virtual void commandSetSensorFrequency(uint32_t freq) = 0;
/*
Function:
- send command to sensor to get/set sensor gyro range
- hasSettings() will return true once sensor info is available
Parameters:
#define GYR_RANGE_400DPS 400
#define GYR_RANGE_1000DPS 1000
#define GYR_RANGE_2000DPS 2000
Returns: none
*/
virtual void commandGetSensorGyroRange(void) = 0;
virtual void commandSetSensorGyroRange(uint32_t range) = 0;
/*
Function:
- send command to sensor to start calibration static gyro
Please keep imu still 4 seconds.
Parameters: none
Returns: none
*/
virtual void commandStartGyroCalibration(void) = 0;
/*
Function:
- send command to sensor to get/set sensor acc range
- hasSettings() will return true once sensor info is available
Parameters:
#define ACC_RANGE_2G 2
#define ACC_RANGE_4G 4
#define ACC_RANGE_8G 8
#define ACC_RANGE_16G 16
Returns: none
*/
virtual void commandGetSensorAccRange(void) = 0;
virtual void commandSetSensorAccRange(uint32_t range) = 0;
/*
Function:
- send command to sensor to get/set sensor mag range
- hasSettings() will return true once sensor info is available
Parameters:
#define MAG_RANGE_2GUASS 2
#define MAG_RANGE_8GUASS 8
Returns: none
*/
virtual void commandGetSensorMagRange(void) = 0;
virtual void commandSetSensorMagRange(uint32_t range) = 0;
/*
Function:
- send command to sensor to start/stop sensor mag calibration
You can set the time for the magnetometer calibration use commandSetSensorMagCalibrationTimeout(second)
[Default is 20 seconds]
Parameters: none
Returns: none
*/
virtual void commandStartMagCalibration(void) = 0;
virtual void commandStopMagCalibration(void) = 0;
/*
Function:
- send command to sensor to get/set sensor mag refrence
- hasSettings() will return true once sensor info is available
Parameters: none
Returns: none
*/
//virtual void commandGetMagRefrence(void) = 0;
//virtual void commandSetMagRefrence(uint32_t reference) = 0;
/*
Function:
- send command to sensor to get/set sensor mag calibration time
- hasSettings() will return true once sensor info is available
Parameters: none
Returns: none
*/
virtual void commandGetSensorMagCalibrationTimeout(void) = 0;
virtual void commandSetSensorMagCalibrationTimeout(float second) = 0;
/*
Function:
- send command to sensor to get sensor info
- hasInfo() will return true once sensor info is available
Parameters: none
Returns: none
*/
virtual void commandGetSensorInfo(void) = 0;
/*
Function:
- send command to save sensor parameters to flash
Parameters: none
Returns: none
*/
virtual void commandSaveParameters(void) = 0;
/*
Function:
- send command to reset factory
Parameters: none
Returns: none
*/
virtual void commandResetFactory(void) = 0;
/*
Function:
- send command to set transmit data
Parameters: as defined in transmit data register TDR
#define TDR_XXX_ENABLED
Returns: none
*/
virtual void commandSetTransmitData(uint32_t config) = 0;
// CAN
/*
Function:
- send command to sensor to get/set sensor can id
- hasSettings() will return true once sensor info is available
Parameters:int32
Returns: none
*/
virtual void commandGetCanStartId(void) = 0;
virtual void commandSetCanStartId(uint32_t data) = 0;
/*
Function:
- send command to sensor to get/set sensor can baudrate
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_CAN_BAUDRATE_125K 125
#define LPMS_CAN_BAUDRATE_250K 250
#define LPMS_CAN_BAUDRATE_500K 500
#define LPMS_CAN_BAUDRATE_800K 800
#define LPMS_CAN_BAUDRATE_1M 1000
Returns: none
*/
virtual void commandGetCanBaudrate(void) = 0;
virtual void commandSetCanBaudrate(uint32_t baudrate) = 0;
/*
Function:
- send command to sensor to get/set sensor can precision (16/32 bit)
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_CAN_DATA_PRECISION_FIXED_POINT 0
#define LPMS_CAN_DATA_PRECISION_FLOATING_POINT 1
Returns: none
*/
virtual void commandGetCanDataPrecision(void) = 0;
virtual void commandSetCanDataPrecision(uint32_t data) = 0;
/*
Function:
- send command to sensor to get/set sensor can mode
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_CAN_MODE_CANOPEN 0
#define LPMS_CAN_MODE_SEQUENTIAL 1
Returns: none
*/
virtual void commandGetCanChannelMode(void) = 0;
virtual void commandSetCanChannelMode(uint32_t data) = 0;
/*
Function:
- send command to sensor to get/set sensor can mapping
- hasSettings() will return true once sensor info is available
Parameters:
each can channel is a (uint32_t)CanMapping.
Returns: none
*/
virtual void commandGetCanMapping(void) = 0;
virtual void commandSetCanMapping(uint32_t map[16]) = 0;
/*
Function:
- send command to sensor to get/set sensor can heartbeat freq
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_CAN_HEARTBEAT_005 0
#define LPMS_CAN_HEARTBEAT_010 1
#define LPMS_CAN_HEARTBEAT_020 2
#define LPMS_CAN_HEARTBEAT_050 5
#define LPMS_CAN_HEARTBEAT_100 10
Returns: none
*/
virtual void commandGetCanHeartbeat(void) = 0;
virtual void commandSetCanHeartbeat(uint32_t data) = 0;
//Filter
/*
Function:
- send command to sensor to get/set sensor filter mode
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_FILTER_GYR 0
#define LPMS_FILTER_KALMAN_GYR_ACC 1
#define LPMS_FILTER_KALMAN_GYR_ACC_MAG 2
#define LPMS_FILTER_DCM_GYR_ACC 3
#define LPMS_FILTER_DCM_GYR_ACC_MAG 4
Returns: none
*/
virtual void commandGetFilterMode(void) = 0;
virtual void commandSetFilterMode(uint32_t data) = 0;
/*
Function:
- send command to sensor to enable/disable gyro auto calibraiton
- hasSettings() will return true once sensor info is available
Parameters: bool
Returns: none
*/
virtual void commandSetGyroAutoCalibration(bool enable) = 0;
virtual void commandGetGyroAutoCalibration(void) = 0;
//Offset Mode
/*
Function:
- send command to sensor to set offset
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_OFFSET_MODE_OBJECT 0
#define LPMS_OFFSET_MODE_HEADING 1
#define LPMS_OFFSET_MODE_ALIGNMENT 2
Returns: none
*/
virtual void commandSetOffsetMode(uint32_t data) = 0;
virtual void commandResetOffsetMode(void) = 0;
//GPS
/*
Function:
- send command to sensor to save/clear gps flash
- hasSettings() will return true once sensor info is available
Parameters:
Returns: none
*/
virtual void commandSaveGPSState(void) = 0;
virtual void commandClearGPSState(void) = 0;
/*
Function:
- send command to sensor to get/set GPS transmit data.
- hasSettings() will return true once sensor info is available
Parameters:
Returns: none
*/
virtual void commandSetGpsTransmitData(uint32_t data, uint32_t data1) = 0;
virtual void commandGetGpsTransmitData(void) = 0;
// Uart
/*
Function:
- send command to sensor to get/set uart baudrate
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_UART_BAUDRATE_xxxxx
Returns: none
*/
virtual void commandSetUartBaudRate(uint32_t data) = 0;
virtual void commandGetUartBaudRate(void) = 0;
/*
Function:
- send command to sensor to get/set uart data format
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_UART_DATA_FORMAT_LPBUS 0
#define LPMS_UART_DATA_FORMAT_ASCII 1
Returns: none
*/
virtual void commandSetUartDataFormat(uint32_t data) = 0;
virtual void commandGetUartDataFormat(void) = 0;
/*
Function:
- send command to sensor to get/set uart data precision
- hasSettings() will return true once sensor info is available
Parameters:
#define LPMS_UART_DATA_PRECISION_FIXED_POINT 0
#define LPMS_UART_DATA_PRECISION_FLOATING_POINT 1
Returns: none
*/
virtual void commandSetUartDataPrecision(uint32_t data) = 0;
virtual void commandGetUartDataPrecision(void) = 0;
/*
Function:
- send command to sensor directly bypassing internal command queue
Parameters:
- cmd: command register
- length: length of data to send
- data: pointer to data buffer
Returns: none
*/
virtual void sendCommand(uint16_t cmd, uint16_t length, uint8_t* data) = 0;
/////////////////////////////////////////////
// Sensor interface
/////////////////////////////////////////////
// General
/*
Function: Specify sensor to streaming or command mode after connect
Parameters:
- SENSOR_MODE_COMMAND: start up in command mode
- SENSOR_MODE_STREAMING: start up in streaming mode
Returns: none
*/
virtual void setStartupSensorMode(int mode) = 0;
/*
Function: set auto reconnection status
Parameters:
- true: enable
- false: disable
Returns: none
*/
virtual void setAutoReconnectStatus(bool b) = 0;
/*
Function: get enable auto reconnection status
Parameters: none
Returns:
- true: enable
- false: disable
*/
virtual bool getAutoReconnectStatus(void) = 0;
/*
Function: get status of sensor
Parameters:none
Returns: as defined in STATUS_XXX
#define STATUS_DISCONNECTED 0
#define STATUS_CONNECTING 1
#define STATUS_CONNECTED 2
#define STATUS_CONNECTION_ERROR 3
#define STATUS_DATA_TIMEOUT 4
#define STATUS_UPDATING 5
*/
virtual int getStatus(void) = 0;
/*
Function: get frequency of incoming data
Parameters:none
Returns: data frequency in Hz
*/
virtual float getDataFrequency(void) = 0;
// info
/*
Function: has new sensor info. consume once
Parameters:none
Returns:
- true: new sensor info available
- false: no new sensor info available
*/
virtual bool hasInfo(void) = 0;
/*
Function: get sensor info
Parameters: reference IG1Info
Returns: none
*/
virtual void getInfo(IG1InfoI &info) = 0;
// settings
/*
Function: has new sensor settings. consume once
Parameters:none
Returns:
- true: new sensor settings available
- false: no new sensor settings available
*/
virtual bool hasSettings(void) = 0;
/*
Function: get sensor settings
Parameters: reference IG1Settings
Returns: none
*/
virtual void getSettings(IG1SettingsI &settings) = 0;
// response from sensor
/*
Function: has feedback from sensor after command sent
Parameters : none
Returns : number of feedback in queue
*/
virtual int hasResponse(void) = 0;
/*
Function: get feedback from internal response queue
Parameters : string reference
Returns :
- true : feedback valid and available
- false : no feedback available
*/
virtual bool getResponse(std::string &s) = 0;
// Imu data
/*
Function: has new imu data
Parameters:none
Returns: number of imu data available in queue
*/
virtual int hasImuData(void) = 0;
/*
Function: get imu data from queue
Parameters : IG1ImuData reference
Returns :
- true : data pop from queue
- false : queue is empty, return latest imu data
*/
virtual bool getImuData(IG1ImuDataI &sd) = 0;
// gps data
/*
Function: has new gps data
Parameters:none
Returns: number of gps data available in queue
*/
virtual int hasGpsData() = 0;
/*
Function: get gps data from queue
Parameters : IG1ImuData reference
Returns :
- true : data pop from queue
- false : queue is empty, return latest gps data
*/
virtual bool getGpsData(IG1GpsDataI &data) = 0;
// Sensor data
/*
Function: is acc raw data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isAccRawEnabled(void) = 0;
/*
Function: is calibrated acc data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isAccCalibratedEnabled(void) = 0;
/*
Function: is gyro raw (high accuracy) data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isGyroIRawEnabled(void) = 0;
/*
Function: is bias calibrated gyro (high accuracy) data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isGyroIBiasCalibratedEnabled(void) = 0;
/*
Function: is alignment + bias calibrated gyro (high accuracy) data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isGyroIAlignCalibratedEnabled(void) = 0;
/*
Function: is gyro raw data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isGyroIIRawEnabled(void) = 0;
/*
Function: is bias calibrated gyro data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isGyroIIBiasCalibratedEnabled(void) = 0;
/*
Function: is alignment + bias calibrated gyro data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isGyroIIAlignCalibratedEnabled(void) = 0;
/*
Function: is mag raw data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isMagRawEnabled(void) = 0;
/*
Function: is calibrated mag data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isMagCalibratedEnabled(void) = 0;
/*
Function: is angular velocity data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isAngularVelocityEnabled(void) = 0;
/*
Function: is quaternion data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isQuaternionEnabled(void) = 0;
/*
Function: is euler data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isEulerEnabled(void) = 0;
/*
Function: is linear acceleration data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isLinearAccelerationEnabled(void) = 0;
/*
Function: is pressure data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isPressureEnabled(void) = 0;
/*
Function: is altitude data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isAltitudeEnabled(void) = 0;
/*
Function: is temperature data enabled
Parameters: void
Returns:
- true: enabled
- false: disabled
*/
virtual bool isTemperatureEnabled(void) = 0;
/**
* Get current data queue size (default: 10)
*/
virtual int getSensorDataQueueSize(void) = 0;
// Error
virtual std::string getLastErrMsg() = 0;
/*
Function: Set library verbosity output
Parameters:
- VERBOSE_NONE
- VERBOSE_INFO
- VERBOSE_DEBUG
Returns: void
*/
virtual void setVerbose(int verboseLevel) = 0;
};
#ifdef _WIN32
#ifdef DLL_EXPORT
#define LPMS_IG1_API __declspec(dllexport)
#else
#define LPMS_IG1_API __declspec(dllimport)
#endif
extern "C" LPMS_IG1_API IG1I* APIENTRY IG1Factory();
#else
#define LPMS_IG1_API
extern "C" LPMS_IG1_API IG1I* IG1Factory();
#endif
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/LpmsIG1Registers.h
+403
View File
@@ -0,0 +1,403 @@
#ifndef LPMSIG1_REGISTERS_H
#define LPMSIG1_REGISTERS_H
/////////////////////////////////////////////////////////////////////
// Essentials - should not change over time
/////////////////////////////////////////////////////////////////////
// Commands start address
#define COMMAND_START_ADDRESS 0
// Acknowledged and Not-acknowledged identifier
#define REPLY_ACK (COMMAND_START_ADDRESS + 0)
#define REPLY_NACK (COMMAND_START_ADDRESS + 1)
// Firmware update and in-application-programmer upload
#define UPDATE_FIRMWARE (COMMAND_START_ADDRESS + 2)
#define UPDATE_IAP (COMMAND_START_ADDRESS + 3)
// Register value save and reset
#define WRITE_REGISTERS (COMMAND_START_ADDRESS + 4)
#define RESTORE_FACTORY_VALUE (COMMAND_START_ADDRESS + 5)
// Mode switching
#define GOTO_COMMAND_MODE (COMMAND_START_ADDRESS + 6)
#define GOTO_STREAM_MODE (COMMAND_START_ADDRESS + 7)
// Sensor status
#define GET_SENSOR_STATUS (COMMAND_START_ADDRESS + 8) // streaming/command
// Data
#define GET_IMU_DATA (COMMAND_START_ADDRESS + 9) // Todo
#define GET_GPS_DATA (COMMAND_START_ADDRESS + 10) // Todo
/////////////////////////////////////////////////////////////////////
// Sensor Info
/////////////////////////////////////////////////////////////////////
#define GET_SENSOR_MODEL (COMMAND_START_ADDRESS + 20)
#define GET_FIRMWARE_INFO (COMMAND_START_ADDRESS + 21)
#define GET_SERIAL_NUMBER (COMMAND_START_ADDRESS + 22)
#define GET_FILTER_VERSION (COMMAND_START_ADDRESS + 23)
#define GET_IAP_CHECKSTATUS (COMMAND_START_ADDRESS + 24)
/////////////////////////////////////////////////////////////////////
// General
/////////////////////////////////////////////////////////////////////
// Transmit data
#define SET_IMU_TRANSMIT_DATA (COMMAND_START_ADDRESS + 30)
#define GET_IMU_TRANSMIT_DATA (COMMAND_START_ADDRESS + 31)
// IMU ID setting
#define SET_IMU_ID (COMMAND_START_ADDRESS + 32)
#define GET_IMU_ID (COMMAND_START_ADDRESS + 33)
// Stream frequency
#define SET_STREAM_FREQ (COMMAND_START_ADDRESS + 34)
#define GET_STREAM_FREQ (COMMAND_START_ADDRESS + 35)
// Deg/Rad output
#define SET_DEGRAD_OUTPUT (COMMAND_START_ADDRESS + 36)
#define GET_DEGRAD_OUTPUT (COMMAND_START_ADDRESS + 37)
// Reference setting and offset reset
#define SET_ORIENTATION_OFFSET (COMMAND_START_ADDRESS + 38)
#define RESET_ORIENTATION_OFFSET (COMMAND_START_ADDRESS + 39)
/////////////////////////////////////////////////////////////////////
// Accelerometer
/////////////////////////////////////////////////////////////////////
// Accelerometer settings
#define SET_ACC_RANGE (COMMAND_START_ADDRESS + 50)
#define GET_ACC_RANGE (COMMAND_START_ADDRESS + 51)
/////////////////////////////////////////////////////////////////////
// Gyro
/////////////////////////////////////////////////////////////////////
// Gyroscope settings
#define SET_GYR_RANGE (COMMAND_START_ADDRESS + 60)
#define GET_GYR_RANGE (COMMAND_START_ADDRESS + 61)
#define START_GYR_CALIBRATION (COMMAND_START_ADDRESS + 62)
#define SET_ENABLE_GYR_AUTOCALIBRATION (COMMAND_START_ADDRESS + 64)
#define GET_ENABLE_GYR_AUTOCALIBRATION (COMMAND_START_ADDRESS + 65)
#define SET_GYR_THRESHOLD (COMMAND_START_ADDRESS + 66)
#define GET_GYR_THRESHOLD (COMMAND_START_ADDRESS + 67)
/////////////////////////////////////////////////////////////////////
// Magnetometer
/////////////////////////////////////////////////////////////////////
// Magnetometer settings
#define SET_MAG_RANGE (COMMAND_START_ADDRESS + 70)
#define GET_MAG_RANGE (COMMAND_START_ADDRESS + 71)
#define SET_HARD_IRON_OFFSET (COMMAND_START_ADDRESS + 72)
#define GET_HARD_IRON_OFFSET (COMMAND_START_ADDRESS + 73)
#define SET_SOFT_IRON_MATRIX (COMMAND_START_ADDRESS + 74)
#define GET_SOFT_IRON_MATRIX (COMMAND_START_ADDRESS + 75)
#define SET_FIELD_ESTIMATE (COMMAND_START_ADDRESS + 76)
#define GET_FIELD_ESTIMATE (COMMAND_START_ADDRESS + 77)
#define SET_MAG_ALIGNMENT_MATRIX (COMMAND_START_ADDRESS + 78)
#define GET_MAG_ALIGNMENT_MATRIX (COMMAND_START_ADDRESS + 79)
#define SET_MAG_ALIGNMENT_BIAS (COMMAND_START_ADDRESS + 80)
#define GET_MAG_ALIGNMENT_BIAS (COMMAND_START_ADDRESS + 81)
#define SET_MAG_REFERENCE (COMMAND_START_ADDRESS + 82)
#define GET_MAG_REFERENCE (COMMAND_START_ADDRESS + 83)
#define START_MAG_CALIBRATION (COMMAND_START_ADDRESS + 84)
#define STOP_MAG_CALIBRATION (COMMAND_START_ADDRESS + 85)
#define SET_MAG_CALIBRATION_TIMEOUT (COMMAND_START_ADDRESS + 86)
#define GET_MAG_CALIBRATION_TIMEOUT (COMMAND_START_ADDRESS + 87)
#define SET_ENABLE_MAG_AUTOCALIBRATION (COMMAND_START_ADDRESS + 88) // Not implemented yet
#define GET_ENABLE_MAG_AUTOCALIBRATION (COMMAND_START_ADDRESS + 89) // Not implemented yet
/////////////////////////////////////////////////////////////////////
// Filter
/////////////////////////////////////////////////////////////////////
// Filter settings
#define SET_FILTER_MODE (COMMAND_START_ADDRESS + 90)
#define GET_FILTER_MODE (COMMAND_START_ADDRESS + 91)
/////////////////////////////////////////////////////////////////////
// CAN
/////////////////////////////////////////////////////////////////////
// CAN settings
#define SET_CAN_START_ID (COMMAND_START_ADDRESS + 110)
#define GET_CAN_START_ID (COMMAND_START_ADDRESS + 111)
#define SET_CAN_BAUDRATE (COMMAND_START_ADDRESS + 112)
#define GET_CAN_BAUDRATE (COMMAND_START_ADDRESS + 113)
#define SET_CAN_DATA_PRECISION (COMMAND_START_ADDRESS + 114)
#define GET_CAN_DATA_PRECISION (COMMAND_START_ADDRESS + 115)
#define SET_CAN_MODE (COMMAND_START_ADDRESS + 116)
#define GET_CAN_MODE (COMMAND_START_ADDRESS + 117)
#define SET_CAN_MAPPING (COMMAND_START_ADDRESS + 118)
#define GET_CAN_MAPPING (COMMAND_START_ADDRESS + 119)
#define SET_CAN_HEARTBEAT (COMMAND_START_ADDRESS + 120)
#define GET_CAN_HEARTBEAT (COMMAND_START_ADDRESS + 121)
/////////////////////////////////////////////////////////////////////
// UART/RS232
/////////////////////////////////////////////////////////////////////
// UART
#define SET_UART_BAUDRATE (COMMAND_START_ADDRESS + 130)
#define GET_UART_BAUDRATE (COMMAND_START_ADDRESS + 131)
#define SET_UART_FORMAT (COMMAND_START_ADDRESS + 132)
#define GET_UART_FORMAT (COMMAND_START_ADDRESS + 133)
#define SET_UART_ASCII_CHARACTER (COMMAND_START_ADDRESS + 134)
#define GET_UART_ASCII_CHARACTER (COMMAND_START_ADDRESS + 135)
#define SET_LPBUS_DATA_PRECISION (COMMAND_START_ADDRESS + 136)
#define GET_LPBUS_DATA_PRECISION (COMMAND_START_ADDRESS + 137)
/////////////////////////////////////////////////////////////////////
// Sync
/////////////////////////////////////////////////////////////////////
// Software Sync
#define START_SYNC (COMMAND_START_ADDRESS + 150) // Todo
#define STOP_SYNC (COMMAND_START_ADDRESS + 151) // Todo
// Timestamp manipulation
#define SET_TIMESTAMP (COMMAND_START_ADDRESS + 152) // Todo
/////////////////////////////////////////////////////////////////////
// GPS
/////////////////////////////////////////////////////////////////////
#define SET_GPS_TRANSMIT_DATA (COMMAND_START_ADDRESS + 160)
#define GET_GPS_TRANSMIT_DATA (COMMAND_START_ADDRESS + 161)
#define SAVE_GPS_STATE (COMMAND_START_ADDRESS + 162)
#define CLEAR_GPS_STATE (COMMAND_START_ADDRESS + 163)
#define SET_GPS_UDR (COMMAND_START_ADDRESS + 164)
#define GET_GPS_UDR (COMMAND_START_ADDRESS + 165)
/////////////////////////////////////////////////////////////////////
// Transmit data Register
/////////////////////////////////////////////////////////////////////
#define TDR_ACC_RAW_OUTPUT_ENABLED (uint32_t)(0x00000001 << 0)
#define TDR_ACC_CALIBRATED_OUTPUT_ENABLED (uint32_t)(0x00000001 << 1)
#define TDR_GYR0_RAW_OUTPUT_ENABLED (uint32_t)(0x00000001 << 2)
#define TDR_GYR1_RAW_OUTPUT_ENABLED (uint32_t)(0x00000001 << 3)
#define TDR_GYR0_BIAS_CALIBRATED_OUTPUT_ENABLED (uint32_t)(0x00000001 << 4)
#define TDR_GYR1_BIAS_CALIBRATED_OUTPUT_ENABLED (uint32_t)(0x00000001 << 5)
#define TDR_GYR0_ALIGN_CALIBRATED_OUTPUT_ENABLED (uint32_t)(0x00000001 << 6)
#define TDR_GYR1_ALIGN_CALIBRATED_OUTPUT_ENABLED (uint32_t)(0x00000001 << 7)
#define TDR_MAG_RAW_OUTPUT_ENABLED (uint32_t)(0x00000001 << 8)
#define TDR_MAG_CALIBRATED_OUTPUT_ENABLED (uint32_t)(0x00000001 << 9)
#define TDR_ANGULAR_VELOCITY_OUTPUT_ENABLED (uint32_t)(0x00000001 << 10)
#define TDR_QUAT_OUTPUT_ENABLED (uint32_t)(0x00000001 << 11)
#define TDR_EULER_OUTPUT_ENABLED (uint32_t)(0x00000001 << 12)
#define TDR_LINACC_OUTPUT_ENABLED (uint32_t)(0x00000001 << 13)
#define TDR_PRESSURE_OUTPUT_ENABLED (uint32_t)(0x00000001 << 14)
#define TDR_ALTITUDE_OUTPUT_ENABLED (uint32_t)(0x00000001 << 15)
#define TDR_TEMPERATURE_OUTPUT_ENABLED (uint32_t)(0x00000001 << 16)
/////////////////////////////////////////////////////////////////////
// GPS TRANSMIT_DATA_CONFIG Register
/////////////////////////////////////////////////////////////////////
// GPS transmit data register0 contents
#define GPS_NAV_PVT_ITOW_ENABLE (uint32_t)(0x00000001 << 0)
#define GPS_NAV_PVT_YEAR_ENABLE (uint32_t)(0x00000001 << 1)
#define GPS_NAV_PVT_MONTH_ENABLE (uint32_t)(0x00000001 << 2)
#define GPS_NAV_PVT_DAY_ENABLE (uint32_t)(0x00000001 << 3)
#define GPS_NAV_PVT_HOUR_ENABLE (uint32_t)(0x00000001 << 4)
#define GPS_NAV_PVT_MIN_ENABLE (uint32_t)(0x00000001 << 5)
#define GPS_NAV_PVT_SEC_ENABLE (uint32_t)(0x00000001 << 6)
#define GPS_NAV_PVT_VALID_ENABLE (uint32_t)(0x00000001 << 7)
#define GPS_NAV_PVT_TACC_ENABLE (uint32_t)(0x00000001 << 8)
#define GPS_NAV_PVT_NANO_ENABLE (uint32_t)(0x00000001 << 9)
#define GPS_NAV_PVT_FIXTYPE_ENABLE (uint32_t)(0x00000001 << 10)
#define GPS_NAV_PVT_FLAGS_ENABLE (uint32_t)(0x00000001 << 11)
#define GPS_NAV_PVT_FLAGS2_ENABLE (uint32_t)(0x00000001 << 12)
#define GPS_NAV_PVT_NUMSV_ENABLE (uint32_t)(0x00000001 << 13)
#define GPS_NAV_PVT_LONGITUDE_ENABLE (uint32_t)(0x00000001 << 14)
#define GPS_NAV_PVT_LATITUDE_ENABLE (uint32_t)(0x00000001 << 15)
#define GPS_NAV_PVT_HEIGHT_ENABLE (uint32_t)(0x00000001 << 16)
#define GPS_NAV_PVT_HMSL_ENABLE (uint32_t)(0x00000001 << 17)
#define GPS_NAV_PVT_HACC_ENABLE (uint32_t)(0x00000001 << 18)
#define GPS_NAV_PVT_VACC_ENABLE (uint32_t)(0x00000001 << 19)
#define GPS_NAV_PVT_VELN_ENABLE (uint32_t)(0x00000001 << 20)
#define GPS_NAV_PVT_VELE_ENABLE (uint32_t)(0x00000001 << 21)
#define GPS_NAV_PVT_VELD_ENABLE (uint32_t)(0x00000001 << 22)
#define GPS_NAV_PVT_GSPEED_ENABLE (uint32_t)(0x00000001 << 23)
#define GPS_NAV_PVT_HEADMOT_ENABLE (uint32_t)(0x00000001 << 24)
#define GPS_NAV_PVT_SACC_ENABLE (uint32_t)(0x00000001 << 25)
#define GPS_NAV_PVT_HEADACC_ENABLE (uint32_t)(0x00000001 << 26)
#define GPS_NAV_PVT_PDOP_ENABLE (uint32_t)(0x00000001 << 27)
#define GPS_NAV_PVT_HEADVEH_ENABLE (uint32_t)(0x00000001 << 28)
// GPS transmit data register1 contents
#define GPS_NAV_ATT_ITOW_ENABLE (uint32_t)(0x00000001 << 0)
#define GPS_NAV_ATT_VERSION_ENABLE (uint32_t)(0x00000001 << 1)
#define GPS_NAV_ATT_ROLL_ENABLE (uint32_t)(0x00000001 << 2)
#define GPS_NAV_ATT_PITCH_ENABLE (uint32_t)(0x00000001 << 3)
#define GPS_NAV_ATT_HEADING_ENABLE (uint32_t)(0x00000001 << 4)
#define GPS_NAV_ATT_ACCROLL_ENABLE (uint32_t)(0x00000001 << 5)
#define GPS_NAV_ATT_ACCPITCH_ENABLE (uint32_t)(0x00000001 << 6)
#define GPS_NAV_ATT_ACCHEADING_ENABLE (uint32_t)(0x00000001 << 7)
#define GPS_ESF_STATUS_ITOW_ENABLE (uint32_t)(0x00000001 << 8)
#define GPS_ESF_STATUS_VERSION_ENABLE (uint32_t)(0x00000001 << 9)
#define GPS_ESF_STATUS_INITSTATUS1_ENABLE (uint32_t)(0x00000001 << 10)
#define GPS_ESF_STATUS_INITSTATUS2_ENABLE (uint32_t)(0x00000001 << 11)
#define GPS_ESF_STATUS_FUSIONMODE_ENABLE (uint32_t)(0x00000001 << 12)
#define GPS_ESF_STATUS_NUMSENS_ENABLE (uint32_t)(0x00000001 << 13)
#define GPS_ESF_STATUS_SENSSTATUS_ENABLE (uint32_t)(0x00000001 << 14)
#define GPS_UDR_STATUS_ENABLE (uint32_t)(0x00000001 << 15)
// Data stream frequency
#define DATA_STREAM_FREQ_5HZ 5
#define DATA_STREAM_FREQ_10HZ 10
#define DATA_STREAM_FREQ_50HZ 50
#define DATA_STREAM_FREQ_100HZ 100
#define DATA_STREAM_FREQ_250HZ 250
#define DATA_STREAM_FREQ_500HZ 500
#define DATA_STREAM_FREQ_1000HZ 1000
//Gyro Range
#define GYR_RANGE_400DPS 400
#define GYR_RANGE_1000DPS 1000
#define GYR_RANGE_2000DPS 2000
//Acc Range
#define ACC_RANGE_2G 2
#define ACC_RANGE_4G 4
#define ACC_RANGE_8G 8
#define ACC_RANGE_16G 16
//Mag Range
#define MAG_RANGE_2GUASS 2
#define MAG_RANGE_8GUASS 8
//Filter Mode
#define LPMS_FILTER_GYR 0
#define LPMS_FILTER_KALMAN_GYR_ACC 1
#define LPMS_FILTER_KALMAN_GYR_ACC_MAG 2
#define LPMS_FILTER_DCM_GYR_ACC 3
#define LPMS_FILTER_DCM_GYR_ACC_MAG 4
#define LPMS_OFFSET_MODE_OBJECT 0
#define LPMS_OFFSET_MODE_HEADING 1
#define LPMS_OFFSET_MODE_ALIGNMENT 2
// UART Settings
#define LPMS_UART_DATA_PRECISION_FIXED_POINT 0
#define LPMS_UART_DATA_PRECISION_FLOATING_POINT 1
#define LPMS_UART_BAUDRATE_9600 9600
#define LPMS_UART_BAUDRATE_19200 19200
#define LPMS_UART_BAUDRATE_38400 38400
#define LPMS_UART_BAUDRATE_57600 57600
#define LPMS_UART_BAUDRATE_115200 115200
#define LPMS_UART_BAUDRATE_230400 230400
#define LPMS_UART_BAUDRATE_256000 256000
#define LPMS_UART_BAUDRATE_460800 460800
#define LPMS_UART_BAUDRATE_921600 921600
#define LPMS_UART_DATA_FORMAT_LPBUS 0
#define LPMS_UART_DATA_FORMAT_ASCII 1
// CAN bus baudrate values
#define LPMS_CAN_BAUDRATE_125K 125
#define LPMS_CAN_BAUDRATE_250K 250
#define LPMS_CAN_BAUDRATE_500K 500
#define LPMS_CAN_BAUDRATE_800K 800
#define LPMS_CAN_BAUDRATE_1M 1000
#define LPMS_CAN_HEARTBEAT_005 0
#define LPMS_CAN_HEARTBEAT_010 1
#define LPMS_CAN_HEARTBEAT_020 2
#define LPMS_CAN_HEARTBEAT_050 5
#define LPMS_CAN_HEARTBEAT_100 10
#define LPMS_CAN_MODE_CANOPEN 0
#define LPMS_CAN_MODE_SEQUENTIAL 1
#define LPMS_CAN_DATA_PRECISION_FIXED_POINT 0
#define LPMS_CAN_DATA_PRECISION_FLOATING_POINT 1
// CAN Mapping
enum CanMapping {
LPMS_CAN_MAPPING_DISABLE = 0,
LPMS_CAN_MAPPING_ACC_RAW_X = 1,
LPMS_CAN_MAPPING_ACC_RAW_Y,
LPMS_CAN_MAPPING_ACC_RAW_Z,
LPMS_CAN_MAPPING_ACC_CALIBRATED_X,
LPMS_CAN_MAPPING_ACC_CALIBRATED_Y,
LPMS_CAN_MAPPING_ACC_CALIBRATED_Z,
LPMS_CAN_MAPPING_GYR0_RAW_X,
LPMS_CAN_MAPPING_GYR0_RAW_Y,
LPMS_CAN_MAPPING_GYR0_RAW_Z,
LPMS_CAN_MAPPING_GYR1_RAW_X,
LPMS_CAN_MAPPING_GYR1_RAW_Y,
LPMS_CAN_MAPPING_GYR1_RAW_Z,
LPMS_CAN_MAPPING_GYR0_BIAS_CALIBRATED_X,
LPMS_CAN_MAPPING_GYR0_BIAS_CALIBRATED_Y,
LPMS_CAN_MAPPING_GYR0_BIAS_CALIBRATED_Z,
LPMS_CAN_MAPPING_GYR1_BIAS_CALIBRATED_X,
LPMS_CAN_MAPPING_GYR1_BIAS_CALIBRATED_Y,
LPMS_CAN_MAPPING_GYR1_BIAS_CALIBRATED_Z,
LPMS_CAN_MAPPING_GYR0_ALIGN_CALIBRATED_X,
LPMS_CAN_MAPPING_GYR0_ALIGN_CALIBRATED_Y,
LPMS_CAN_MAPPING_GYR0_ALIGN_CALIBRATED_Z,
LPMS_CAN_MAPPING_GYR1_ALIGN_CALIBRATED_X,
LPMS_CAN_MAPPING_GYR1_ALIGN_CALIBRATED_Y,
LPMS_CAN_MAPPING_GYR1_ALIGN_CALIBRATED_Z,
LPMS_CAN_MAPPING_MAG_RAW_X,
LPMS_CAN_MAPPING_MAG_RAW_Y,
LPMS_CAN_MAPPING_MAG_RAW_Z,
LPMS_CAN_MAPPING_MAG_CALIBRATED_X,
LPMS_CAN_MAPPING_MAG_CALIBRATED_Y,
LPMS_CAN_MAPPING_MAG_CALIBRATED_Z,
LPMS_CAN_MAPPING_ANGULAR_VELOCITY_X,
LPMS_CAN_MAPPING_ANGULAR_VELOCITY_Y,
LPMS_CAN_MAPPING_ANGULAR_VELOCITY_Z,
LPMS_CAN_MAPPING_QUAT_W,
LPMS_CAN_MAPPING_QUAT_X,
LPMS_CAN_MAPPING_QUAT_Y,
LPMS_CAN_MAPPING_QUAT_Z,
LPMS_CAN_MAPPING_EULER_X,
LPMS_CAN_MAPPING_EULER_Y,
LPMS_CAN_MAPPING_EULER_Z,
LPMS_CAN_MAPPING_LINACC_X,
LPMS_CAN_MAPPING_LINACC_Y,
LPMS_CAN_MAPPING_LINACC_Z,
LPMS_CAN_MAPPING_PRESSURE,
LPMS_CAN_MAPPING_TEMPERATURE,
LPMS_CAN_MAPPING_MAX
};
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/MicroMeasure.cpp
+95
View File
@@ -0,0 +1,95 @@
/***********************************************************************
** Copyright (C) 2019 LP-Research
** All rights reserved.
** Contact: LP-Research (info@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.
***********************************************************************/
#include "MicroMeasure.h"
#include <sys/time.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <assert.h>
static const unsigned usec_per_sec = 1000000;
static const unsigned usec_per_msec = 1000;
bool QueryPerformanceFrequency(long long *frequency)
{
*frequency = (long long)usec_per_sec;
return true;
}
bool QueryPerformanceCounter(long long *performance_count)
{
struct timeval time;
assert(performance_count != NULL);
gettimeofday(&time, NULL);
*performance_count = time.tv_usec + time.tv_sec * (long long)usec_per_sec;
return true;
}
MicroMeasure::MicroMeasure()
{
long long ticksPerSecond;
QueryPerformanceFrequency(&ticksPerSecond);
tpm = ticksPerSecond;
start_time = 0LL;
}
void MicroMeasure::reset(void)
{
long long tick;
QueryPerformanceCounter(&tick);
start_time = tick;
}
long long MicroMeasure::measure(void)
{
long long tick;
QueryPerformanceCounter(&tick);
return (((tick - start_time) * 1000LL * 1000LL) / tpm);
}
void MicroMeasure::sleep(long long s_t)
{
reset();
while (measure() < s_t)
{
}
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/MicroMeasure.h
+48
View File
@@ -0,0 +1,48 @@
/***********************************************************************
** Copyright (C) 2019 LP-Research
** All rights reserved.
** Contact: LP-Research (info@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 MICRO_MEASURE
#define MICRO_MEASURE
class MicroMeasure
{
public:
MicroMeasure();
void reset(void);
long long measure(void);
void sleep(long long s_t);
public:
long long start_time;
long long tpm;
};
#endif
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/MicroMeasureWindows.cpp
+71
View File
@@ -0,0 +1,71 @@
/***********************************************************************
** Copyright (C) 2013 LP-Research
** All rights reserved.
** Contact: LP-Research (info@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.
***********************************************************************/
#include <windows.h>
#include "MicroMeasure.h"
MicroMeasure::MicroMeasure()
{
LARGE_INTEGER ticksPerSecond;
QueryPerformanceFrequency(&ticksPerSecond);
tpm = ticksPerSecond.QuadPart;
start_time = 0;
}
void MicroMeasure::reset(void)
{
LARGE_INTEGER tick;
QueryPerformanceCounter(&tick);
start_time = tick.QuadPart;
}
long long MicroMeasure::measure(void)
{
LARGE_INTEGER tick;
QueryPerformanceCounter(&tick);
return (((tick.QuadPart - start_time) * 1000 * 1000) / tpm);
}
void MicroMeasure::sleep(long long s_t)
{
reset();
while (measure() < s_t)
{
}
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/README.md
+196
View File
@@ -0,0 +1,196 @@
# LPMS-IG1/BE/NAV3 Series OpenSource Lib
## Usage
### 1.Compiling Library programs
```bash
$ cd ~
$ git clone https://bitbucket.org/lpresearch/lpmsig1opensourcelib
$ cd lpmsig1opensourcelib
$ mkdir build
$ cd build
$ cmake ..
$ make
$ make package
$ sudo dpkg -i libLpmsIG1_OpenSource-x.y.z-Linux.deb
```
### 2.Compiling Sample programs
```bash
$ cd linux_example
$ mkdir build
$ cd build
$ cmake ..
$ make
$ ./LpmsIG1SimpleExample
```
You might require running command using sudo if current user is not in dialout group. See [Troubleshooting](#troubleshooting)
### 3.Compiling ROS Example programs
```bash
# Create ROS workspace.
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
# We suggest to create a symbolic link to ros_example folder instead of copying
$ ln -s ~/lpmsig1opensourcelib/ros_example lpms_ig1_ros_example
# Compiling ROS example programs
$ cd ~/catkin_ws
$ source ./devel/setup.bash
$ catkin_make
```
Open a new terminal window and run roscore
```bash
$ roscore
```
Connect `LPMS-IG1/BE/NAV3` sensor to PC.
Now you can run lpms_ig1 node on your other terminal windows.
The following are some example commands to launch ros lpms_ig1 node. Please change the parameters appropriately according to your sensor settings:
*IG1*:
```bash
$ rosrun lpms_ig1 lpms_ig1_node _port:=LPMSIG1000001 _baudrate:=921600
```
*IG1-RS485*:
```bash
$ rosrun lpms_ig1 lpms_ig1_rs485_node _port:=/dev/ttyTHS2 _baudrate:=115200 _rs485ControlPin:=388 _rs485ControlPinToggleWaitMs:=2
```
*BE*:
```bash
$ rosrun lpms_ig1 lpms_be1_node _port:=/dev/ttyUSB0 _baudrate:=115200
```
*NAV3*:
```bash
$ rosrun lpms_ig1 lpms_nav3_node _port:=/dev/ttyUSB0 _baudrate:=115200
```
Please refer to [Troubleshooting](#troubleshooting) section is error occurs.
You can print out the imu data by subscribing to `/imu/data` topic
```bash
#Show imu message.
$ rostopic echo /imu/data
#Plot imu message.
$ rosrun rqt_plot rqt_plot
```
Alternatively, you can use the sample launch file (lpmsig1.launch) start data acquisition and data plotting:
*IG1*:
```
roslaunch lpms_ig1 lpmsig1.launch
```
*IG1-RS485*:
```
roslaunch lpms_ig1 lpmsig1_rs485.launch
```
*BE1*:
```
roslaunch lpms_ig1 lpmsbe1.launch
```
*NAV3*:
```
roslaunch lpms_ig1 lpmsnav3.launch
```
## Troubleshooting
### Serial connection error
If prompted with the following information：
```bash
$ rosrun lpms_ig1 lpms_ig1_node
#Error opening [IG1] COM:/dev/ttyUSB0 disconnected
```
this error might be due to the fact that the current user does not have sufficient permission to access the device.
To allow access to sensors connected via USB, you need to ensure that the user running the ROS sensor node has access to the /dev/ttyUSB devices. You can do this by adding the user to the dialout group. After this call, you should logout and login with this user to ensure the changed permissions are in effect.
```bash
$ sudo adduser <username> dialout
```
## ROS Package Summary
### 1. Supported Hardware
This driver interfaces with LPMS-IG1 IMU sensor from LP-Research Inc.
### 2.1 lpms_ig1_node / lpms_ig1_rs485_node / lpms_be1_node / lpms_nav3_node
lpms_ig1_node is a driver for the LPMS-IG1 Inertial Measurement Unit. It publishes orientation, angular velocity, linear acceleration and magnetometer data (covariances are not yet supported), and complies with the [Sensor message](https://wiki.ros.org/sensor_msgs) for [IMU API](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html) and [MagneticField](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/MagneticField.html) API.
Similarly lpms_ig1_rs485_node is a driver for the LPMS-IG1-RS485 Inertial Measurement Unit.
lpms_be1_node is a driver for the LPMS-BE1/BE2 Inertial Measurement Unit.
lpms_nav3_node is a driver for the LPMS-NAV3 Inertial Measurement Unit.
#### 2.1.1 Published Topics
/imu/data ([sensor_msgs/Imu](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html))
: Inertial data from the IMU. Includes calibrated acceleration, calibrated angular rates and orientation. The orientation is always unit quaternion.
/imu/mag ([sensor_msgs/MagneticField](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/MagneticField.html)) *IG1 Series only*
: Magnetometer reading from the sensor.
/imu/is_autocalibration_active ([std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html), default: True)
: Latched topic indicating if the gyro autocalibration feature is active
#### 2.1.2 Services
/imu/calibrate_gyroscope ([std_srvs/Empty](http://docs.ros.org/api/std_srvs/html/srv/Empty.html))
: This service activates the IMU internal gyro bias estimation function. Please make sure the IMU sensor is placed on a stable platform with minimal vibrations before calling the service. Please make sure the sensor is stationary for at least 4 seconds. The service call returns a success response once the calibration procedure is completed.
/imu/reset_heading ([std_srvs/Empty](http://docs.ros.org/api/std_srvs/html/srv/Empty.html))
: This service will reset the heading (yaw) angle of the sensor to zero.
/imu/enable_gyro_autocalibration ([std_srvs/SetBool](http://docs.ros.org/melodic/api/std_srvs/html/srv/SetBool.html))
: Turn on/off autocalibration function in the IMU. The status of autocalibration can be obtained by subscribing to the /imu/is_autocalibration_active topic. A message will published to /imu/is_autocalibration_active for each call to /imu/autocalibrate.
/imu/enable_auto_reconnect ([std_srvs/SetBool](http://docs.ros.org/melodic/api/std_srvs/html/srv/SetBool.html))
: Turn on/off auto reconnect function in the library. When auto reconnect is enabled, the library will reestablish connection to the sensor if it detects no data from the sensor for more than 5 seconds. With auto reconnect enabled, the library will automatically put the sensor into streaming mode. We recommend to disable auto reconnect if user plans to interact with sensor via polling method.
/imu/get_imu_data ([std_srvs/Empty](http://docs.ros.org/api/std_srvs/html/srv/Empty.html)) *IG1-RS485 only*
: Poll imu sensor for latest sensor data. Latest sensor data will be publish to /imu/data topic once the driver received a new data from the sensor.
/imu/set_streaming_mode ([std_srvs/Empty](http://docs.ros.org/api/std_srvs/html/srv/Empty.html)) *IG1-RS485 only*
: Put sensor into streaming mode
/imu/set_command_mode ([std_srvs/Empty](http://docs.ros.org/api/std_srvs/html/srv/Empty.html)) *IG1-RS485 only*
: Put sensor into command mode
#### 2.1.3 Parameters
~port (string, default: /dev/ttyUSB0)
: The port the IMU is connected to.
~baudrate (int, default IG1: 921600, IG1-RS485: 115200, BE1: 115200)
: Baudrate for the IMU sensor.
~autoreconnect (bool, default True)
: Enable/disable autoreconnect in library
~frame_id (string, default: imu)
: The frame in which imu readings will be returned.
~data_process_rate (int, default: 200)
: Data processing rate of the internal loop. This rate has to be equal or larger than the data streaming frequency of the sensor to prevent internal data queue overflow.
~rs485ControlPin (int, default: -1 (disabled)) *IG1-RS485 only*
: RS485 send/receive control pin. The driver will automatically toggle this pin to HIGH when sending message and set the pin to LOW once data is sent.
~rs485ControlPinToggleWaitMs (int, default(ms): 1 ) *IG1-RS485 only*
: Delay (in millisecond) to wait before rs485ControlPin is toggled before/after sending data.
For further information on `LP-Research`, please visit our website:
* http://www.lp-research.com
* http://www.alubi.cn
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/SUMMARY.txt
+5
View File
@@ -0,0 +1,5 @@
Library for communicating and interfacing with LP-Research LpmsIG1 sensors. This library contains classes that allow a user to integrate LPMS devices into their own applications.
This library provides following features:
* Low level configuration of Lpms sensor through LpProtocol
* Easy-to-use reader and writer.
For more infomation on how to use this library, please visit http://www.lp-research.com
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/SensorData.h
+466
View File
@@ -0,0 +1,466 @@
/***********************************************************************
** 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
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/SensorDataI.h
+429
View File
@@ -0,0 +1,429 @@
/***********************************************************************
** 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
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/SerialPort.cpp
+366
View File
@@ -0,0 +1,366 @@
#include "SerialPort.h"
#include <algorithm>
using namespace std;
Serial::Serial():
portNo(0),
connected(false),
usbMode(MODE_USB_EXPRESS)
{
}
Serial::~Serial()
{
//Check if we are connected before trying to disconnect
close();
}
bool Serial::open(int portno, int baudrate)
{
if (isConnected())
CloseHandle(this->hSerial);
//if (usbMode == MODE_USB_EXPRESS)
// return open("0001");
portNo = portno;
stringstream ss;
ss << "\\\\.\\COM" << portNo;
string portName = ss.str();
//We're not yet connected
this->connected = false;
//Try to connect to the given port throuh CreateFile
this->hSerial = CreateFile(portName.c_str(),
GENERIC_READ | GENERIC_WRITE,
0,
0,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL);
//Check if the connection was successfull
if (this->hSerial == INVALID_HANDLE_VALUE)
{
printf("[Serial] ERROR: Invalid handle value\n");
CloseHandle(this->hSerial);
return false;
}
else
{
char mode_str[128];
switch (baudrate)
{
case BAUDRATE_4800:
strcpy(mode_str, "baud=4800");
break;
case BAUDRATE_9600:
strcpy(mode_str, "baud=9600");
break;
case BAUDRATE_19200:
strcpy(mode_str, "baud=19200");
break;
case BAUDRATE_38400:
strcpy(mode_str, "baud=38400");
break;
case BAUDRATE_57600:
strcpy(mode_str, "baud=57600");
break;
case BAUDRATE_115200:
strcpy(mode_str, "baud=115200");
break;
case BAUDRATE_230400:
strcpy(mode_str, "baud=230400");
break;
case BAUDRATE_256000:
strcpy(mode_str, "baud=256000");
break;
case BAUDRATE_460800:
strcpy(mode_str, "baud=460800");
break;
case BAUDRATE_921600:
strcpy(mode_str, "baud=921600");
break;
}
strcat(mode_str, " data=8");
strcat(mode_str, " parity=n");
strcat(mode_str, " stop=1");
strcat(mode_str, " dtr=on rts=on");
DCB port_settings;
memset(&port_settings, 0, sizeof(port_settings)); /* clear the new struct */
port_settings.DCBlength = sizeof(port_settings);
if (!BuildCommDCBA(mode_str, &port_settings))
{
printf("unable to set comport dcb settings\n");
CloseHandle(this->hSerial);
return false;
}
if (!BuildCommDCBA(mode_str, &port_settings))
{
printf("unable to set comport dcb settings\n");
CloseHandle(this->hSerial);
return false;
}
if (!SetCommState(this->hSerial, &port_settings))
{
printf("unable to set comport cfg settings\n");
CloseHandle(this->hSerial);
return false;
}
COMMTIMEOUTS Cptimeouts;
Cptimeouts.ReadIntervalTimeout = MAXDWORD;
Cptimeouts.ReadTotalTimeoutMultiplier = 0;
Cptimeouts.ReadTotalTimeoutConstant = 0;
Cptimeouts.WriteTotalTimeoutMultiplier = 100;
Cptimeouts.WriteTotalTimeoutConstant = 100;
if (!SetCommTimeouts(this->hSerial, &Cptimeouts))
{
printf("unable to set comport time-out settings\n");
CloseHandle(this->hSerial);
return false;
}
}
this->connected = true;
return true;
}
bool Serial::open(std::string deviceId, int baudrate)
{
bool f;
transform(deviceId.begin(), deviceId.end(), deviceId.begin(), ::toupper);
this->idNumber = deviceId;
std::cout << "[LpmsU] Lpms::connect deviceID is : " << deviceId << std::endl;
f = false;
SI_STATUS siStatus;
DWORD devercNum = 0;
SI_DEVICE_STRING DeviceString;
this->idNumber = deviceId;
DWORD numDevs;
if (SI_GetNumDevices(&numDevs) != SI_SUCCESS)
return false;
devercNum = numDevs;
for (DWORD d = 0; d < numDevs; d++)
{
siStatus = SI_GetProductString(d, DeviceString, SI_RETURN_SERIAL_NUMBER);
if (siStatus == SI_SUCCESS)
{
string deviceString = string(DeviceString);
transform(deviceString.begin(), deviceString.end(), deviceString.begin(), ::toupper);
if (deviceId == deviceString)
{
devercNum = d;
break;
}
}
}
siStatus = SI_Open(devercNum, &cyHandle);
siStatus = SI_SetBaudRate(cyHandle, baudrate);
siStatus = SI_SetFlowControl(cyHandle, SI_HANDSHAKE_LINE, SI_FIRMWARE_CONTROLLED, SI_HELD_INACTIVE, SI_STATUS_INPUT, SI_STATUS_INPUT, 0);
if (siStatus == SI_SUCCESS) {
std::cout << "[LpmsU] SI_Open device is successul : " << deviceId << std::endl;
this->connected = true;
SI_FlushBuffers(cyHandle, TRUE, TRUE);
}
else {
std::cout << "[LpmsU] SI_Open device is failed : " << deviceId << std::endl;
this->connected = false;
}
return this->connected ;
}
void Serial::setMode(int mode)
{
usbMode = mode;
}
int Serial::getMode(void)
{
return usbMode;
}
bool Serial::close()
{
if (this->connected)
{
//We're no longer connected
this->connected = false;
//Close the serial handler
if (usbMode == MODE_USB_EXPRESS)
{
SI_Close(cyHandle);
cout << "[LpmsU] Connection to " << idNumber << " closed." << endl;
}
else
{
return CloseHandle(this->hSerial);
}
}
return true;
}
int Serial::readData(unsigned char *buffer, unsigned int nbChar)
{
if (!isConnected())
return 0;
if (usbMode == MODE_USB_EXPRESS)
{
SI_STATUS siStatus;
PBYTE ModemStatus;
#ifdef _WIN32
unsigned long eventDWord;
unsigned long txBytes;
unsigned long rxBytes;
unsigned long qStatus;
#else
unsigned int eventDWord;
unsigned int txBytes;
unsigned int rxBytes;
#endif
bool f = true;
unsigned long bytesReceived = 0;
if (this->connected == false) return 0;
siStatus = SI_CheckRXQueue(cyHandle, &rxBytes, &qStatus);
if (siStatus != SI_SUCCESS)
{
std::cout << "USB read error\n";
return false;
}
#ifdef _WIN32
siStatus = SI_Read(cyHandle, buffer, rxBytes, &bytesReceived);
#else
siStatus = SI_Read(cyHandle, buffer, rxBytes, (unsigned int *)bytesReceived);
#endif
return bytesReceived;
}
else
{
//Number of bytes we'll have read
DWORD bytesRead;
//Number of bytes we'll really ask to read
unsigned int toRead;
//int n;
//ReadFile(this->hSerial, buffer, nbChar, (LPDWORD)((void *)&n), NULL);
//return n;
//Use the ClearCommError function to get status info on the Serial port
ClearCommError(this->hSerial, &this->errors, &this->status);
//Check if there is something to read
if (this->status.cbInQue>0)
{
//If there is we check if there is enough data to read the required number
//of characters, if not we'll read only the available characters to prevent
//locking of the application.
if (this->status.cbInQue>nbChar)
{
toRead = nbChar;
}
else
{
toRead = this->status.cbInQue;
}
//Try to read the require number of chars, and return the number of read bytes on success
if (ReadFile(this->hSerial, buffer, toRead, &bytesRead, NULL))
{
return bytesRead;
}
}
//If nothing has been read, or that an error was detected return -1
}
return 0;
}
bool Serial::writeData(const char *buffer, unsigned int nbChar)
{
if (!isConnected())
{
if (usbMode == MODE_USB_EXPRESS)
std::cout << "Sensor " << idNumber << " not connected\n";
else
std::cout << "COM "<< portNo << " not connected\n";
return false;
}
if (usbMode == MODE_USB_EXPRESS)
{
SI_STATUS siStatus;
#ifdef _WIN32
unsigned long bytesWritten;
#else
unsigned int bytesWritten;
#endif
bool f = false;
if (this->connected == false) return false;
siStatus = SI_Write(cyHandle, (unsigned char *)buffer, nbChar, &bytesWritten);
if (siStatus == SI_SUCCESS) {
f = true;
}else{
f = false;
std::cout << "Write error\n";
}
return f;
}
else
{
DWORD bytesSend;
//Try to write the buffer on the Serial port
if (!WriteFile(this->hSerial, (void *)buffer, nbChar, &bytesSend, 0))
{
std::cout << "Write error\n";
//In case it don't work get comm error and return false
ClearCommError(this->hSerial, &this->errors, &this->status);
return false;
}
else
{
return true;
}
}
}
bool Serial::isConnected()
{
//Simply return the connection status
return this->connected;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/SerialPort.h
+123
View File
@@ -0,0 +1,123 @@
#ifndef SERIALCLASS_H_INCLUDED
#define SERIALCLASS_H_INCLUDED
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <sstream>
#ifdef _WIN32
#include <windows.h>
#include "SiUSBXp.h"
#elif __linux__
#include <stdio.h> // standard input / output functions
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <asm/termbits.h>
#include <cstring>
#include <libudev.h>
#include <iterator>
#include <map>
#endif
#include "LpLog.h"
const static int INCOMING_DATA_MAX_LENGTH = 2048;
class Serial
{
const std::string TAG = "SERIAL";
public:
enum {
BAUDRATE_4800=4800,
BAUDRATE_9600=9600,
BAUDRATE_19200=19200,
BAUDRATE_38400=38400,
BAUDRATE_57600=57600,
BAUDRATE_115200=115200,
BAUDRATE_230400=230400,
BAUDRATE_256000=256000,
BAUDRATE_460800=460800,
BAUDRATE_921600=921600
};
static const int MODE_VCP = 0;
static const int MODE_USB_EXPRESS = 1;
private:
//Connection status
bool connected;
#ifdef _WIN32
//Serial comm handler
HANDLE hSerial;
//Get various information about the connection
COMSTAT status;
//Keep track of last error
DWORD errors;
int portNo;
// SILAB
HANDLE cyHandle;
std::string idNumber;
#elif __linux__
int fd;
std::string portNo;
std::map<std::string, std::string> usbDeviceMap;
#endif
int usbMode;
public:
Serial();
~Serial();
#ifdef _WIN32
bool open(int portno, int baudrate = BAUDRATE_9600);
bool open(std::string deviceId, int baudrate);
#elif __linux__
bool open(std::string portno, int baudrate = BAUDRATE_115200);
#endif
// Silab USBXpress or VCP mode, default VCP mode
void setMode(int mode = MODE_VCP);
int getMode(void);
bool close();
int readData(unsigned char *buffer, unsigned int nbChar);
#ifdef _WIN32
bool writeData(const char *buffer, unsigned int nbChar);
#elif __linux__
bool writeData(unsigned char *buffer, unsigned int nbChar);
#endif
bool isConnected();
#ifdef _WIN32
int getPortNo() { return portNo; }
#elif __linux__
std::string getPortNo() { return portNo; }
#endif
#ifdef __linux__
int set_interface_attribs (int fd, int speed, int parity);
void createUsbDeviceMap();
#endif
LpLog& log = LpLog::getInstance();
};
#endif // SERIALCLASS_H_INCLUDED
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/SerialPortLinux.cpp
+266
View File
@@ -0,0 +1,266 @@
#include "SerialPort.h"
#include "LpUtil.h"
using namespace std;
Serial::Serial():
portNo(""),
connected(false)
{
}
Serial::~Serial()
{
//Check if we are connected before trying to disconnect
close();
}
bool Serial::open(std::string portno, int baudrate)
{
if (isConnected())
close();
createUsbDeviceMap();
map<string, string>::iterator iter = usbDeviceMap.find(portno);
if (iter != usbDeviceMap.end())
{
portno = iter->second;
}
fd = ::open (portno.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
if (fd < 0)
{
log.e(TAG, "Error opening %s: Err %d %s\n", portno.c_str(), errno, strerror (errno));
return false;
}
if (set_interface_attribs (fd, baudrate, 0) == -1)
{
log.e(TAG, "Error configuring serial attributes");
return false;
}
//tcflush(fd,TCIOFLUSH);
portNo = portno;
this->connected = true;
return true;
}
bool Serial::close()
{
if (this->connected)
{
//We're no longer connected
this->connected = false;
//tcflush(fd,TCIOFLUSH);
//Close the serial handler
::close(fd);
}
return true;
}
int Serial::readData(unsigned char *buffer, unsigned int nbChar)
{
if (!isConnected())
return 0;
int bytes_avail;
ioctl(fd, FIONREAD, &bytes_avail);
if (bytes_avail > INCOMING_DATA_MAX_LENGTH) {
log.d(TAG, "Warning: Buffer overflow: %d\n", bytes_avail);
bytes_avail = INCOMING_DATA_MAX_LENGTH;
} else if (bytes_avail > nbChar)
bytes_avail = nbChar;
int n = ::read(fd, buffer, bytes_avail);//sizeof(rxBuffer)); // read up to 100 characters if ready to read
return n;
}
void Serial::setMode(int mode)
{
usbMode = mode;
}
int Serial::getMode(void)
{
return usbMode;
}
bool Serial::writeData(unsigned char *buffer, unsigned int nbChar)
{
if (!isConnected())
{
log.e(TAG, "Error: dongle not connected\n");
return false;
}
int ret;
ret = ::write(fd, buffer, nbChar); // send 7 character greeting
return true;
}
bool Serial::isConnected()
{
//Simply return the connection status
return this->connected;
}
int Serial::set_interface_attribs (int fd, int speed, int parity)
{
/*
struct termios2 tty;
if (ioctl(fd, TCGETS2, &tty) < 0)
{
return -1;
}
tty.c_cflag &= ~CBAUD;
tty.c_cflag |= BOTHER;
tty.c_ispeed = speed;
tty.c_ospeed = speed;
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
// disable IGNBRK for mismatched speed tests; otherwise receive break
// as \000 chars
tty.c_iflag &= ~IGNBRK; // disable break processing
tty.c_lflag = 0; // no signaling chars, no echo,
// no canonical processing
tty.c_oflag = 0; // no remapping, no delays
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tty.c_iflag &= ~(IXON | IXOFF | IXANY |INLCR | IGNCR | ICRNL); //enable xon
tty.c_iflag |=IXOFF;
tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
// enable reading
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
tty.c_cflag |= parity;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS;
if (ioctl(fd, TCSETS2, &tty) < 0)
{
return -1;
}
*/
struct termios2 config;
if (ioctl(fd, TCGETS2, &config) < 0)
{
return -1;
}
config.c_cflag &= ~CBAUD;
config.c_cflag |= BOTHER;
config.c_ispeed = speed;
config.c_ospeed = speed;
config.c_iflag &= ~(IGNBRK | IXANY | INLCR | IGNCR | ICRNL);
config.c_iflag &= ~IXON; // disable XON/XOFF flow control (output)
config.c_iflag &= ~IXOFF; // disable XON/XOFF flow control (input)
config.c_cflag &= ~CRTSCTS; // disable RTS flow control
config.c_lflag = 0;
config.c_oflag = 0;
config.c_cflag &= ~CSIZE;
config.c_cflag |= CS8; // 8-bit chars
config.c_cflag |= CLOCAL; // ignore modem controls
config.c_cflag |= CREAD; // enable reading
config.c_cflag &= ~(PARENB | PARODD); // disable parity
config.c_cflag &= ~CSTOPB; // one stop bit
config.c_cc[VMIN] = 0; // read doesn´t block
config.c_cc[VTIME] = 5; // 0.5 seconds read timeout
config.c_cflag |= parity;
if (ioctl(fd, TCSETS2, &config) < 0)
{
return -1;
}
/*
struct termios2 config;
if (ioctl(fd, TCGETS2, &config) < 0)
{
return -1;
}
config.c_iflag &= ~(IGNBRK | IXANY | INLCR | IGNCR | ICRNL);
config.c_iflag &= ~IXON; // disable XON/XOFF flow control (output)
config.c_iflag &= ~IXOFF; // disable XON/XOFF flow control (input)
config.c_cflag &= ~CRTSCTS; // disable RTS flow control
config.c_lflag = 0;
config.c_oflag = 0;
config.c_cflag &= ~CSIZE;
config.c_cflag |= CS8; // 8-bit chars
config.c_cflag |= CLOCAL; // ignore modem controls
config.c_cflag |= CREAD; // enable reading
config.c_cflag &= ~(PARENB | PARODD); // disable parity
config.c_cflag &= ~CSTOPB; // one stop bit
config.c_cc[VMIN] = 0; // read doesn´t block
config.c_cc[VTIME] = 5; // 0.5 seconds read timeout
if (ioctl(fd, TCSETS2, &config) < 0)
{
return -1;
}
*/
return 0;
}
void Serial::createUsbDeviceMap()
{
struct udev *udev;
struct udev_device *dev;
struct udev_enumerate *enumerate;
struct udev_list_entry *list, *node;
const char *path;
usbDeviceMap.clear();
udev = udev_new();
if (!udev)
{
printf("can not create udev");
}
enumerate = udev_enumerate_new(udev);
udev_enumerate_add_match_subsystem(enumerate, "tty");
udev_enumerate_scan_devices(enumerate);
list = udev_enumerate_get_list_entry(enumerate);
udev_list_entry_foreach(node, list)
{
path = udev_list_entry_get_name(node);
dev = udev_device_new_from_syspath(udev, path);
if (udev_device_get_property_value(dev, "ID_SERIAL_SHORT") &&
udev_device_get_property_value(dev, "DEVNAME"))
{
string serialID(udev_device_get_property_value(dev, "ID_SERIAL_SHORT"));
string devName(udev_device_get_property_value(dev, "DEVNAME"));
string vendorId(udev_device_get_property_value(dev, "ID_VENDOR_ID"));
string productId(udev_device_get_property_value(dev, "ID_MODEL_ID"));
// Check device usb is CP2102
if (vendorId=="10c4" && (productId=="ea60" || productId == "ea61" ))
usbDeviceMap.insert(pair<string, string>(serialID, devName));
}
udev_device_unref(dev);
}
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/linux_example/.gitignore
+2
View File
@@ -0,0 +1,2 @@
build/
bin/
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/linux_example/CMakeLists.txt
+68
View File
@@ -0,0 +1,68 @@
cmake_minimum_required(VERSION 2.4.6)
project (LpmsIG1Example)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0003 NEW)
cmake_policy(SET CMP0015 NEW)
endif(COMMAND cmake_policy)
set(CMAKE_BUILD_TYPE "Release")
set(BUILD_ARCHITECTURE "32-bit" CACHE STRING "")
set_property(CACHE BUILD_ARCHITECTURE PROPERTY STRINGS "32-bit" "64-bit")
set(IG1_INC "./..")
set(IG1_LIB "./lib")
include_directories("${IG1_INC}")
link_directories("${IG1_LIB}")
ADD_DEFINITIONS(-D_CRT_SECURE_NO_DEPRECATE)
if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -std=gnu++11")
endif()
set(headers
${IG1_INC}/LpMatrix.h
${IG1_INC}/LpmsIG1Registers.h
${IG1_INC}/LpmsIG1I.h
${IG1_INC}/SensorDataI.h
)
set(LpmsIG1_SimpleExample_SRCS
${IG1_INC}/LpMatrix.c
LpmsIG1_SimpleExample.cpp
)
set(LpmsIG1_RS485Example_SRCS
${IG1_INC}/LpMatrix.c
LpmsIG1_RS485Example.cpp
)
set(LpmsBEx_SimpleExample_SRCS
${IG1_INC}/LpMatrix.c
LpmsBEx_SimpleExample.cpp
)
set(LpmsNAV3_SimpleExample_SRCS
${IG1_INC}/LpMatrix.c
LpmsNAV3_SimpleExample.cpp
)
# LpmsIG1 Simple Example
add_executable(LpmsIG1_SimpleExample ${LpmsIG1_SimpleExample_SRCS} ${headers})
target_link_libraries(LpmsIG1_SimpleExample LpmsIG1_OpenSourceLib rt pthread udev)
# LpmsIG1_RS485 Example
add_executable(LpmsIG1_RS485Example ${LpmsIG1_RS485Example_SRCS} ${headers})
target_link_libraries(LpmsIG1_RS485Example LpmsIG1_OpenSourceLib rt pthread udev)
# LpmsBEx Simple Example
add_executable(LpmsBEx_SimpleExample ${LpmsBEx_SimpleExample_SRCS} ${headers})
target_link_libraries(LpmsBEx_SimpleExample LpmsIG1_OpenSourceLib rt pthread udev)
# LpmsNAV3 Simple Example
add_executable(LpmsNAV3_SimpleExample ${LpmsNAV3_SimpleExample_SRCS} ${headers})
target_link_libraries(LpmsNAV3_SimpleExample LpmsIG1_OpenSourceLib rt pthread udev)
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/linux_example/LpmsBE1_SimpleExample.cpp
+234
View File
@@ -0,0 +1,234 @@
#include <cstring>
#include <stdarg.h>
#include <thread>
#include <iostream>
#include <sstream>
#include "LpmsIG1I.h"
#include "SensorDataI.h"
#include "LpmsIG1Registers.h"
using namespace std;
string TAG("MAIN");
IG1I* sensor1;
// Start print data thread
std::thread *printThread;
static bool printThreadIsRunning = false;
void logd(std::string tag, const char* str, ...)
{
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
void printTask()
{
if (sensor1->getStatus() != STATUS_CONNECTED)
{
printThreadIsRunning = false;
logd(TAG, "Sensor is not connected. Sensor Status: %d\n", sensor1->getStatus());
logd(TAG, "printTask terminated\n");
return;
}
sensor1->commandGotoStreamingMode();
printThreadIsRunning = true;
while (sensor1->getStatus() == STATUS_CONNECTED && printThreadIsRunning)
{
IG1ImuDataI sd;
if (sensor1->hasImuData())
{
sensor1->getImuData(sd);
float freq = sensor1->getDataFrequency();
logd(TAG, "t(s): %.3f acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f Hz:%3.3f \r\n",
sd.timestamp*0.002f,
sd.accCalibrated.data[0], sd.accCalibrated.data[1], sd.accCalibrated.data[2],
sd.gyroIAlignmentCalibrated.data[0], sd.gyroIAlignmentCalibrated.data[1], sd.gyroIAlignmentCalibrated.data[2],
sd.euler.data[0], sd.euler.data[1], sd.euler.data[2],
freq);
}
this_thread::sleep_for(chrono::milliseconds(2));
}
printThreadIsRunning = false;
logd(TAG, "printTask terminated\n");
}
void printMenu()
{
cout << endl;
cout << "===================" << endl;
cout << "Main Menu" << endl;
cout << "===================" << endl;
cout << "[c] Connect sensor" << endl;
cout << "[d] Disconnect sensor" << endl;
cout << "[0] Goto command mode" << endl;
cout << "[1] Goto streaming mode" << endl;
cout << "[h] Reset sensor heading" << endl;
cout << "[i] Print sensor info" << endl;
cout << "[s] Print sensor settings" << endl;
cout << "[p] Print sensor data" << endl;
cout << "[a] Toggle enable/disable auto reconnect" << endl;
cout << "[q] quit" << endl;
cout << endl;
}
int main(int argc, char** argv)
{
string comportNo = "/dev/ttyUSB0";
int baudrate = 115200;
if (argc == 2)
{
comportNo = string(argv[1]);
}
else if (argc == 3)
{
comportNo = string(argv[1]);
baudrate = atoi(argv[2]);
}
logd(TAG, "Connecting to %s @%d\n", comportNo.c_str(), baudrate);
// Create LpmsIG1 object with corresponding comport and baudrate
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(true);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
sensor1->release();
logd(TAG, "Error connecting to sensor\n");
logd(TAG, "bye\n");
return 1;
}
do
{
logd(TAG, "Waiting for sensor to connect\r\n");
this_thread::sleep_for(chrono::milliseconds(1000));
} while (
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
);
if (sensor1->getStatus() != STATUS_CONNECTED)
{
logd(TAG, "Sensor connection error status: %d\n", sensor1->getStatus());
sensor1->release();
logd(TAG, "bye\n");
return 1;
}
logd(TAG, "Sensor connected\n");
printMenu();
bool quit = false;
while (!quit)
{
char cmd = '\0';
string input;
getline(cin, input);
if (!input.empty())
cmd = input.at(0);
switch (cmd)
{
case 'c':
logd(TAG, "Connect sensor\n");
if (!sensor1->connect(comportNo, baudrate))
logd(TAG, "Error connecting to sensor\n");
break;
case 'd':
logd(TAG, "Disconnect sensor\n");
sensor1->disconnect();
break;
case '0':
logd(TAG, "Goto command mode\n");
sensor1->commandGotoCommandMode();
break;
case '1':
logd(TAG, "Goto streaming mode\n");
sensor1->commandGotoStreamingMode();
break;
case 'h':
logd(TAG, "Reset heading\n");
// reset sensor heading
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
break;
case 'i': {
logd(TAG, "Print sensor info\n");
// Print sensor info
IG1InfoI info;
sensor1->getInfo(info);
cout << info.toString() << endl;
break;
}
case 's': {
logd(TAG, "Print sensor settings\n");
// Print sensor settings
IG1SettingsI settings;
sensor1->getSettings(settings);
cout << settings.toString() << endl;
break;
}
case 'p': {
logd(TAG, "Print sensor data\n");
// Print sensor data
if (!printThreadIsRunning)
printThread = new std::thread(printTask);
break;
}
case 'a': {
logd(TAG, "Toggle auto reconnect\n");
bool b = sensor1->getAutoReconnectStatus();
sensor1->setAutoReconnectStatus(!b);
logd(TAG, "Auto reconnect %s\n", !b? "enabled":"disabled");
break;
}
case 'q':
logd(TAG, "quit\n");
// disconnect sensor
sensor1->disconnect();
quit = true;
break;
default:
printThreadIsRunning = false;
if (printThread && printThread->joinable()) {
printThread->join();
printThread = NULL;
}
printMenu();
break;
}
this_thread::sleep_for(chrono::milliseconds(100));
}
if (printThread && printThread->joinable())
printThread->join();
// release sensor resources
sensor1->release();
logd(TAG, "Bye\n");
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/linux_example/LpmsBEx_SimpleExample.cpp
+234
View File
@@ -0,0 +1,234 @@
#include <cstring>
#include <stdarg.h>
#include <thread>
#include <iostream>
#include <sstream>
#include "LpmsIG1I.h"
#include "SensorDataI.h"
#include "LpmsIG1Registers.h"
using namespace std;
string TAG("MAIN");
IG1I* sensor1;
// Start print data thread
std::thread *printThread;
static bool printThreadIsRunning = false;
void logd(std::string tag, const char* str, ...)
{
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
void printTask()
{
if (sensor1->getStatus() != STATUS_CONNECTED)
{
printThreadIsRunning = false;
logd(TAG, "Sensor is not connected. Sensor Status: %d\n", sensor1->getStatus());
logd(TAG, "printTask terminated\n");
return;
}
sensor1->commandGotoStreamingMode();
printThreadIsRunning = true;
while (sensor1->getStatus() == STATUS_CONNECTED && printThreadIsRunning)
{
IG1ImuDataI sd;
if (sensor1->hasImuData())
{
sensor1->getImuData(sd);
float freq = sensor1->getDataFrequency();
logd(TAG, "t(s): %.3f acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f Hz:%3.3f \r\n",
sd.timestamp*0.002f,
sd.accCalibrated.data[0], sd.accCalibrated.data[1], sd.accCalibrated.data[2],
sd.gyroIAlignmentCalibrated.data[0], sd.gyroIAlignmentCalibrated.data[1], sd.gyroIAlignmentCalibrated.data[2],
sd.euler.data[0], sd.euler.data[1], sd.euler.data[2],
freq);
}
this_thread::sleep_for(chrono::milliseconds(2));
}
printThreadIsRunning = false;
logd(TAG, "printTask terminated\n");
}
void printMenu()
{
cout << endl;
cout << "===================" << endl;
cout << "Main Menu" << endl;
cout << "===================" << endl;
cout << "[c] Connect sensor" << endl;
cout << "[d] Disconnect sensor" << endl;
cout << "[0] Goto command mode" << endl;
cout << "[1] Goto streaming mode" << endl;
cout << "[h] Reset sensor heading" << endl;
cout << "[i] Print sensor info" << endl;
cout << "[s] Print sensor settings" << endl;
cout << "[p] Print sensor data" << endl;
cout << "[a] Toggle enable/disable auto reconnect" << endl;
cout << "[q] quit" << endl;
cout << endl;
}
int main(int argc, char** argv)
{
string comportNo = "/dev/ttyUSB0";
int baudrate = 115200;
if (argc == 2)
{
comportNo = string(argv[1]);
}
else if (argc == 3)
{
comportNo = string(argv[1]);
baudrate = atoi(argv[2]);
}
logd(TAG, "Connecting to %s @%d\n", comportNo.c_str(), baudrate);
// Create LpmsIG1 object with corresponding comport and baudrate
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(true);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
sensor1->release();
logd(TAG, "Error connecting to sensor\n");
logd(TAG, "bye\n");
return 1;
}
do
{
logd(TAG, "Waiting for sensor to connect\r\n");
this_thread::sleep_for(chrono::milliseconds(1000));
} while (
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
);
if (sensor1->getStatus() != STATUS_CONNECTED)
{
logd(TAG, "Sensor connection error status: %d\n", sensor1->getStatus());
sensor1->release();
logd(TAG, "bye\n");
return 1;
}
logd(TAG, "Sensor connected\n");
printMenu();
bool quit = false;
while (!quit)
{
char cmd = '\0';
string input;
getline(cin, input);
if (!input.empty())
cmd = input.at(0);
switch (cmd)
{
case 'c':
logd(TAG, "Connect sensor\n");
if (!sensor1->connect(comportNo, baudrate))
logd(TAG, "Error connecting to sensor\n");
break;
case 'd':
logd(TAG, "Disconnect sensor\n");
sensor1->disconnect();
break;
case '0':
logd(TAG, "Goto command mode\n");
sensor1->commandGotoCommandMode();
break;
case '1':
logd(TAG, "Goto streaming mode\n");
sensor1->commandGotoStreamingMode();
break;
case 'h':
logd(TAG, "Reset heading\n");
// reset sensor heading
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
break;
case 'i': {
logd(TAG, "Print sensor info\n");
// Print sensor info
IG1InfoI info;
sensor1->getInfo(info);
cout << info.toString() << endl;
break;
}
case 's': {
logd(TAG, "Print sensor settings\n");
// Print sensor settings
IG1SettingsI settings;
sensor1->getSettings(settings);
cout << settings.toString() << endl;
break;
}
case 'p': {
logd(TAG, "Print sensor data\n");
// Print sensor data
if (!printThreadIsRunning)
printThread = new std::thread(printTask);
break;
}
case 'a': {
logd(TAG, "Toggle auto reconnect\n");
bool b = sensor1->getAutoReconnectStatus();
sensor1->setAutoReconnectStatus(!b);
logd(TAG, "Auto reconnect %s\n", !b? "enabled":"disabled");
break;
}
case 'q':
logd(TAG, "quit\n");
// disconnect sensor
sensor1->disconnect();
quit = true;
break;
default:
printThreadIsRunning = false;
if (printThread && printThread->joinable()) {
printThread->join();
printThread = NULL;
}
printMenu();
break;
}
this_thread::sleep_for(chrono::milliseconds(100));
}
if (printThread && printThread->joinable())
printThread->join();
// release sensor resources
sensor1->release();
logd(TAG, "Bye\n");
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/linux_example/LpmsIG1_RS485Example.cpp
+249
View File
@@ -0,0 +1,249 @@
#include <cstring>
#include <stdarg.h>
#include <thread>
#include <iostream>
#include <sstream>
#include "LpmsIG1I.h"
#include "SensorDataI.h"
#include "LpmsIG1Registers.h"
using namespace std;
string TAG("MAIN");
IG1I* sensor1;
// Start print data thread
std::thread *printThread;
static bool printThreadIsRunning = false;
void logd(std::string tag, const char* str, ...)
{
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
void printTask()
{
if (sensor1->getStatus() != STATUS_CONNECTED)
{
printThreadIsRunning = false;
logd(TAG, "Sensor is not connected. Sensor Status: %d\n", sensor1->getStatus());
logd(TAG, "printTask terminated\n");
return;
}
sensor1->commandGotoStreamingMode();
printThreadIsRunning = true;
while (sensor1->getStatus() == STATUS_CONNECTED && printThreadIsRunning)
{
IG1ImuDataI sd;
if (sensor1->hasImuData())
{
sensor1->getImuData(sd);
float freq = sensor1->getDataFrequency();
logd(TAG, "t(s): %.3f acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f Hz:%3.3f \r\n",
sd.timestamp*0.002f,
sd.accCalibrated.data[0], sd.accCalibrated.data[1], sd.accCalibrated.data[2],
sd.gyroIAlignmentCalibrated.data[0], sd.gyroIAlignmentCalibrated.data[1], sd.gyroIAlignmentCalibrated.data[2],
sd.euler.data[0], sd.euler.data[1], sd.euler.data[2],
freq);
}
this_thread::sleep_for(chrono::milliseconds(2));
}
printThreadIsRunning = false;
logd(TAG, "printTask terminated\n");
}
void printMenu()
{
cout << endl;
cout << "===================" << endl;
cout << "Main Menu" << endl;
cout << "===================" << endl;
cout << "[c] Connect sensor" << endl;
cout << "[d] Disconnect sensor" << endl;
cout << "[0] Goto command mode" << endl;
cout << "[1] Goto streaming mode" << endl;
cout << "[h] Reset sensor heading" << endl;
cout << "[i] Print sensor info" << endl;
cout << "[s] Print sensor settings" << endl;
cout << "[p] Print sensor data" << endl;
cout << "[a] Toggle enable/disable auto reconnect" << endl;
cout << "[q] quit" << endl;
cout << endl;
}
int main(int argc, char** argv)
{
string comportNo = "/dev/ttyTHS2";
int baudrate = 115200;
if (argc == 2)
{
comportNo = string(argv[1]);
}
else if (argc == 3)
{
comportNo = string(argv[1]);
baudrate = atoi(argv[2]);
}
logd(TAG, "Connecting to %s @%d\n", comportNo.c_str(), baudrate);
// Create LpmsIG1 object with corresponding comport and baudrate
sensor1 = IG1Factory();
// Set verbosity to INFO level to output info messages
// Use VERBOSE_NONE to turn off all messages from library
// Use VERBOSE_DEBUG to output debug messages
sensor1->setVerbose(VERBOSE_INFO);
// Enable auto reconnect
sensor1->setAutoReconnectStatus(true);
// Set connection interface to RS485
sensor1->setConnectionInterface(CONNECTION_INTERFACE_RS485);
// Set GPIO pin toggle wait time to 2ms
sensor1->setControlGPIOToggleWaitMs(2);
// Use pin 388 as RS485 flow control pin
sensor1->setControlGPIOForRs485(388);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
sensor1->release();
logd(TAG, "Error connecting to sensor\n");
logd(TAG, "bye\n");
return 1;
}
do
{
logd(TAG, "Waiting for sensor to connect\r\n");
this_thread::sleep_for(chrono::milliseconds(1000));
} while (
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
);
if (sensor1->getStatus() != STATUS_CONNECTED)
{
logd(TAG, "Sensor connection error: %d\n", sensor1->getStatus());
sensor1->release();
logd(TAG, "bye\n");
return 1;
}
logd(TAG, "Sensor connected\n");
printMenu();
bool quit = false;
while (!quit)
{
char cmd = '\0';
string input;
getline(cin, input);
if (!input.empty())
cmd = input.at(0);
switch (cmd)
{
case 'c':
logd(TAG, "Connect sensor\n");
if (!sensor1->connect(comportNo, baudrate))
logd(TAG, "Error connecting to sensor\n");
break;
case 'd':
logd(TAG, "Disconnect sensor\n");
sensor1->disconnect();
break;
case '0':
logd(TAG, "Goto command mode\n");
sensor1->commandGotoCommandMode();
break;
case '1':
logd(TAG, "Goto streaming mode\n");
sensor1->commandGotoStreamingMode();
break;
case 'h':
logd(TAG, "Reset heading\n");
// reset sensor heading
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
break;
case 'i': {
logd(TAG, "Print sensor info\n");
// Print sensor info
IG1InfoI info;
sensor1->getInfo(info);
cout << info.toString() << endl;
break;
}
case 's': {
logd(TAG, "Print sensor settings\n");
// Print sensor settings
IG1SettingsI settings;
sensor1->getSettings(settings);
cout << settings.toString() << endl;
break;
}
case 'p': {
logd(TAG, "Print sensor data\n");
// Print sensor data
if (!printThreadIsRunning)
printThread = new std::thread(printTask);
break;
}
case 'a': {
logd(TAG, "Toggle auto reconnect\n");
bool b = sensor1->getAutoReconnectStatus();
sensor1->setAutoReconnectStatus(!b);
logd(TAG, "Auto reconnect %s\n", !b? "enabled":"disabled");
break;
}
case 'q':
logd(TAG, "quit\n");
// disconnect sensor
sensor1->disconnect();
quit = true;
break;
default:
printThreadIsRunning = false;
if (printThread && printThread->joinable()) {
printThread->join();
printThread = NULL;
}
printMenu();
break;
}
this_thread::sleep_for(chrono::milliseconds(100));
}
if (printThread && printThread->joinable())
printThread->join();
// release sensor resources
sensor1->release();
logd(TAG, "Bye\n");
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/linux_example/LpmsIG1_SimpleExample.cpp
+234
View File
@@ -0,0 +1,234 @@
#include <cstring>
#include <stdarg.h>
#include <thread>
#include <iostream>
#include <sstream>
#include "LpmsIG1I.h"
#include "SensorDataI.h"
#include "LpmsIG1Registers.h"
using namespace std;
string TAG("MAIN");
IG1I* sensor1;
// Start print data thread
std::thread *printThread;
static bool printThreadIsRunning = false;
void logd(std::string tag, const char* str, ...)
{
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
void printTask()
{
if (sensor1->getStatus() != STATUS_CONNECTED)
{
printThreadIsRunning = false;
logd(TAG, "Sensor is not connected. Sensor Status: %d\n", sensor1->getStatus());
logd(TAG, "printTask terminated\n");
return;
}
sensor1->commandGotoStreamingMode();
printThreadIsRunning = true;
while (sensor1->getStatus() == STATUS_CONNECTED && printThreadIsRunning)
{
IG1ImuDataI sd;
if (sensor1->hasImuData())
{
sensor1->getImuData(sd);
float freq = sensor1->getDataFrequency();
logd(TAG, "t(s): %.3f acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f Hz:%3.3f \r\n",
sd.timestamp*0.002f,
sd.accCalibrated.data[0], sd.accCalibrated.data[1], sd.accCalibrated.data[2],
sd.gyroIAlignmentCalibrated.data[0], sd.gyroIAlignmentCalibrated.data[1], sd.gyroIAlignmentCalibrated.data[2],
sd.euler.data[0], sd.euler.data[1], sd.euler.data[2],
freq);
}
this_thread::sleep_for(chrono::milliseconds(2));
}
printThreadIsRunning = false;
logd(TAG, "printTask terminated\n");
}
void printMenu()
{
cout << endl;
cout << "===================" << endl;
cout << "Main Menu" << endl;
cout << "===================" << endl;
cout << "[c] Connect sensor" << endl;
cout << "[d] Disconnect sensor" << endl;
cout << "[0] Goto command mode" << endl;
cout << "[1] Goto streaming mode" << endl;
cout << "[h] Reset sensor heading" << endl;
cout << "[i] Print sensor info" << endl;
cout << "[s] Print sensor settings" << endl;
cout << "[p] Print sensor data" << endl;
cout << "[a] Toggle enable/disable auto reconnect" << endl;
cout << "[q] quit" << endl;
cout << endl;
}
int main(int argc, char** argv)
{
string comportNo = "LPMSIG1-SENSOR-ID";
int baudrate = 921600;
if (argc == 2)
{
comportNo = string(argv[1]);
}
else if (argc == 3)
{
comportNo = string(argv[1]);
baudrate = atoi(argv[2]);
}
logd(TAG, "Connecting to %s @%d\n", comportNo.c_str(), baudrate);
// Create LpmsIG1 object with corresponding comport and baudrate
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(true);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
sensor1->release();
logd(TAG, "Error connecting to sensor\n");
logd(TAG, "bye\n");
return 1;
}
do
{
logd(TAG, "Waiting for sensor to connect\r\n");
this_thread::sleep_for(chrono::milliseconds(1000));
} while (
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
);
if (sensor1->getStatus() != STATUS_CONNECTED)
{
logd(TAG, "Sensor connection error status: %d\n", sensor1->getStatus());
sensor1->release();
logd(TAG, "bye\n");
return 1;
}
logd(TAG, "Sensor connected\n");
printMenu();
bool quit = false;
while (!quit)
{
char cmd = '\0';
string input;
getline(cin, input);
if (!input.empty())
cmd = input.at(0);
switch (cmd)
{
case 'c':
logd(TAG, "Connect sensor\n");
if (!sensor1->connect(comportNo, baudrate))
logd(TAG, "Error connecting to sensor\n");
break;
case 'd':
logd(TAG, "Disconnect sensor\n");
sensor1->disconnect();
break;
case '0':
logd(TAG, "Goto command mode\n");
sensor1->commandGotoCommandMode();
break;
case '1':
logd(TAG, "Goto streaming mode\n");
sensor1->commandGotoStreamingMode();
break;
case 'h':
logd(TAG, "Reset heading\n");
// reset sensor heading
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
break;
case 'i': {
logd(TAG, "Print sensor info\n");
// Print sensor info
IG1InfoI info;
sensor1->getInfo(info);
cout << info.toString() << endl;
break;
}
case 's': {
logd(TAG, "Print sensor settings\n");
// Print sensor settings
IG1SettingsI settings;
sensor1->getSettings(settings);
cout << settings.toString() << endl;
break;
}
case 'p': {
logd(TAG, "Print sensor data\n");
// Print sensor data
if (!printThreadIsRunning)
printThread = new std::thread(printTask);
break;
}
case 'a': {
logd(TAG, "Toggle auto reconnect\n");
bool b = sensor1->getAutoReconnectStatus();
sensor1->setAutoReconnectStatus(!b);
logd(TAG, "Auto reconnect %s\n", !b? "enabled":"disabled");
break;
}
case 'q':
logd(TAG, "quit\n");
// disconnect sensor
sensor1->disconnect();
quit = true;
break;
default:
printThreadIsRunning = false;
if (printThread && printThread->joinable()) {
printThread->join();
printThread = NULL;
}
printMenu();
break;
}
this_thread::sleep_for(chrono::milliseconds(100));
}
if (printThread && printThread->joinable())
printThread->join();
// release sensor resources
sensor1->release();
logd(TAG, "Bye\n");
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/linux_example/LpmsNAV3_SimpleExample.cpp
+234
View File
@@ -0,0 +1,234 @@
#include <cstring>
#include <stdarg.h>
#include <thread>
#include <iostream>
#include <sstream>
#include "LpmsIG1I.h"
#include "SensorDataI.h"
#include "LpmsIG1Registers.h"
using namespace std;
string TAG("MAIN");
IG1I* sensor1;
// Start print data thread
std::thread *printThread;
static bool printThreadIsRunning = false;
void logd(std::string tag, const char* str, ...)
{
va_list a_list;
va_start(a_list, str);
if (!tag.empty())
printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
vprintf(str, a_list);
va_end(a_list);
}
void printTask()
{
if (sensor1->getStatus() != STATUS_CONNECTED)
{
printThreadIsRunning = false;
logd(TAG, "Sensor is not connected. Sensor Status: %d\n", sensor1->getStatus());
logd(TAG, "printTask terminated\n");
return;
}
sensor1->commandGotoStreamingMode();
printThreadIsRunning = true;
while (sensor1->getStatus() == STATUS_CONNECTED && printThreadIsRunning)
{
IG1ImuDataI sd;
while (sensor1->hasImuData())
{
sensor1->getImuData(sd);
float freq = sensor1->getDataFrequency();
logd(TAG, "t(s): %.3f acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f Hz:%3.3f \r\n",
sd.timestamp*0.001f,
sd.accCalibrated.data[0], sd.accCalibrated.data[1], sd.accCalibrated.data[2],
sd.gyroIAlignmentCalibrated.data[0], sd.gyroIAlignmentCalibrated.data[1], sd.gyroIAlignmentCalibrated.data[2],
sd.euler.data[0], sd.euler.data[1], sd.euler.data[2],
freq);
}
this_thread::sleep_for(chrono::milliseconds(2));
}
printThreadIsRunning = false;
logd(TAG, "printTask terminated\n");
}
void printMenu()
{
cout << endl;
cout << "===================" << endl;
cout << "Main Menu" << endl;
cout << "===================" << endl;
cout << "[c] Connect sensor" << endl;
cout << "[d] Disconnect sensor" << endl;
cout << "[0] Goto command mode" << endl;
cout << "[1] Goto streaming mode" << endl;
cout << "[h] Reset sensor heading" << endl;
cout << "[i] Print sensor info" << endl;
cout << "[s] Print sensor settings" << endl;
cout << "[p] Print sensor data" << endl;
cout << "[a] Toggle enable/disable auto reconnect" << endl;
cout << "[q] quit" << endl;
cout << endl;
}
int main(int argc, char** argv)
{
string comportNo = "/dev/ttyUSB0";
int baudrate = 115200;
if (argc == 2)
{
comportNo = string(argv[1]);
}
else if (argc == 3)
{
comportNo = string(argv[1]);
baudrate = atoi(argv[2]);
}
logd(TAG, "Connecting to %s @%d\n", comportNo.c_str(), baudrate);
// Create LpmsIG1 object with corresponding comport and baudrate
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(true);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
sensor1->release();
logd(TAG, "Error connecting to sensor\n");
logd(TAG, "bye\n");
return 1;
}
do
{
logd(TAG, "Waiting for sensor to connect\r\n");
this_thread::sleep_for(chrono::milliseconds(1000));
} while (
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
);
if (sensor1->getStatus() != STATUS_CONNECTED)
{
logd(TAG, "Sensor connection error status: %d\n", sensor1->getStatus());
sensor1->release();
logd(TAG, "bye\n");
return 1;
}
logd(TAG, "Sensor connected\n");
printMenu();
bool quit = false;
while (!quit)
{
char cmd = '\0';
string input;
getline(cin, input);
if (!input.empty())
cmd = input.at(0);
switch (cmd)
{
case 'c':
logd(TAG, "Connect sensor\n");
if (!sensor1->connect(comportNo, baudrate))
logd(TAG, "Error connecting to sensor\n");
break;
case 'd':
logd(TAG, "Disconnect sensor\n");
sensor1->disconnect();
break;
case '0':
logd(TAG, "Goto command mode\n");
sensor1->commandGotoCommandMode();
break;
case '1':
logd(TAG, "Goto streaming mode\n");
sensor1->commandGotoStreamingMode();
break;
case 'h':
logd(TAG, "Reset heading\n");
// reset sensor heading
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
break;
case 'i': {
logd(TAG, "Print sensor info\n");
// Print sensor info
IG1InfoI info;
sensor1->getInfo(info);
cout << info.toString() << endl;
break;
}
case 's': {
logd(TAG, "Print sensor settings\n");
// Print sensor settings
IG1SettingsI settings;
sensor1->getSettings(settings);
cout << settings.toString() << endl;
break;
}
case 'p': {
logd(TAG, "Print sensor data\n");
// Print sensor data
if (!printThreadIsRunning)
printThread = new std::thread(printTask);
break;
}
case 'a': {
logd(TAG, "Toggle auto reconnect\n");
bool b = sensor1->getAutoReconnectStatus();
sensor1->setAutoReconnectStatus(!b);
logd(TAG, "Auto reconnect %s\n", !b? "enabled":"disabled");
break;
}
case 'q':
logd(TAG, "quit\n");
// disconnect sensor
sensor1->disconnect();
quit = true;
break;
default:
printThreadIsRunning = false;
if (printThread && printThread->joinable()) {
printThread->join();
printThread = NULL;
}
printMenu();
break;
}
this_thread::sleep_for(chrono::milliseconds(100));
}
if (printThread && printThread->joinable())
printThread->join();
// release sensor resources
sensor1->release();
logd(TAG, "Bye\n");
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/.gitignore
+34
View File
@@ -0,0 +1,34 @@
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
# Temporary files
*.user
.idea/
cmake-build-debug/
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/CMakeLists.txt
+90
View File
@@ -0,0 +1,90 @@
cmake_minimum_required(VERSION 2.8.3)
project(lpms_ig1)
set(CMAKE_CXX_FLAGS "-std=c++11")
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
)
generate_messages(
DEPENDENCIES
std_msgs
)
link_directories("${IG1_LIB}")
set(lpms_ig1_node_SRCS
src/lpms_ig1_node.cpp
)
set(lpms_ig1_rs485_node_SRCS
src/lpms_ig1_rs485_node.cpp
)
set(lpms_ig1_rs485_client_SRCS
src/lpms_ig1_rs485_client.cpp
)
set(lpms_be1_node_SRCS
src/lpms_be1_node.cpp
)
set(lpms_nav3_node_SRCS
src/lpms_nav3_node.cpp
)
## Declare a catkin package
catkin_package()
## Build
include_directories(include ${catkin_INCLUDE_DIRS})
# lpms_ig1_node
add_executable(lpms_ig1_node ${lpms_ig1_node_SRCS})
target_link_libraries(lpms_ig1_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_ig1_node ${catkin_EXPORTED_TARGETS})
# lpms_ig1_rs485_node
add_executable(lpms_ig1_rs485_node ${lpms_ig1_rs485_node_SRCS})
target_link_libraries(lpms_ig1_rs485_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_ig1_rs485_node ${catkin_EXPORTED_TARGETS})
# lpms_ig1_rs485_client
add_executable(lpms_ig1_rs485_client ${lpms_ig1_rs485_client_SRCS})
target_link_libraries(lpms_ig1_rs485_client
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_ig1_rs485_client ${catkin_EXPORTED_TARGETS})
# lpms_be1_node
add_executable(lpms_be1_node ${lpms_be1_node_SRCS})
target_link_libraries(lpms_be1_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_be1_node ${catkin_EXPORTED_TARGETS})
# lpms_NAV3_node
add_executable(lpms_nav3_node ${lpms_nav3_node_SRCS})
target_link_libraries(lpms_nav3_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_nav3_node ${catkin_EXPORTED_TARGETS})
# imudata_rad_to_deg_node
add_executable(imudata_rad_to_deg_node src/imudata_rad_to_deg_node.cpp)
target_link_libraries(imudata_rad_to_deg_node ${catkin_LIBRARIES})
add_dependencies(imudata_rad_to_deg_node ${catkin_EXPORTED_TARGETS})
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsbe1.launch
+18
View File
@@ -0,0 +1,18 @@
<launch>
<!-- IG1 Sensor node -->
<node name="lpms_be1" pkg="lpms_ig1" type="lpms_be1_node" output="screen">
<param name="port" value="0001" type="string" />
<param name="baudrate" value="115200" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsig1.launch
+18
View File
@@ -0,0 +1,18 @@
<launch>
<!-- IG1 Sensor node -->
<node name="lpms_ig1" pkg="lpms_ig1" type="lpms_ig1_node">
<param name="port" value="/dev/ttyUSB0" type="string" />
<param name="baudrate" value="921600" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsig1_rs485.launch
+20
View File
@@ -0,0 +1,20 @@
<launch>
<!-- IG1 RS484 Sensor node -->
<node name="lpms_ig1_rs485" pkg="lpms_ig1" type="lpms_ig1_rs485_node">
<param name="port" value="/dev/ttyTHS5" type="string" />
<param name="baudrate" value="115200" type="int" />
<param name="rs485ControlPin" value="388" type="int" />
<param name="rs485ControlPinToggleWaitMs" value="2" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsnav3.launch
+19
View File
@@ -0,0 +1,19 @@
<launch>
<!-- IG1 Sensor node -->
<node name="lpms_nav3" pkg="lpms_ig1" type="lpms_nav3_node" output="screen">
<param name="port" value="/dev/ttyUSB0" type="string" />
<param name="baudrate" value="115200" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/package.xml
+52
View File
@@ -0,0 +1,52 @@
<?xml version="1.0"?>
<package>
<name>lpms_ig1</name>
<version>0.0.1</version>
<description>ROS driver for lpms_ig1 sensors.</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="lxf@alubi.cn">Feng</maintainer>
<maintainer email="yap@lp-research.com">H.E. YAP</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="xxxxx">xxxxx</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<!-- build_depend>orocos_kdl</build_depend -->
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<!-- run_depend>orocos_kdl</run_depend -->
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/imudata_rad_to_deg_node.cpp
+47
View File
@@ -0,0 +1,47 @@
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>
#include <tf/transform_datatypes.h>
ros::Publisher angular_vel_deg_publisher;
ros::Publisher rpy_deg_publisher;
ros::Subscriber imudata_subscriber;
const float r2d = 57.29577951f;
void MsgCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
geometry_msgs::Vector3 angular_vel;
angular_vel.x = msg->angular_velocity.x*r2d;
angular_vel.y = msg->angular_velocity.y*r2d;
angular_vel.z = msg->angular_velocity.z*r2d;
angular_vel_deg_publisher.publish(angular_vel);
tf::Quaternion q(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
geometry_msgs::Vector3 rpy;
rpy.x = roll*r2d;
rpy.y = pitch*r2d;
rpy.z = yaw*r2d;
rpy_deg_publisher.publish(rpy);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_listener");
ros::NodeHandle n;
angular_vel_deg_publisher = n.advertise<geometry_msgs::Vector3>("angular_vel_deg", 1000);
rpy_deg_publisher = n.advertise<geometry_msgs::Vector3>("rpy_deg", 1000);
imudata_subscriber = n.subscribe("/imu/data", 1000, MsgCallback);
ROS_INFO("waiting for imu data");
ros::spin();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_be1_node.cpp
+399
View File
@@ -0,0 +1,399 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
//! Manages connection with the sensor, publishes data
/*!
\TODO: Make noncopyable!
*/
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpBE1Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
// Parameters
std::string comportNo;
int baudrate;
bool autoReconnect;
std::string frame_id;
int rate;
LpBE1Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 115200);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsBE1 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpBE1Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpBE1Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpBE1Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpBE1Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpBE1Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpBE1Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpBE1Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
//logd(TAG, "Error connecting to sensor\n");
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpBE1Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = -sd.quaternion.data[1];
imu_msg.orientation.y = -sd.quaternion.data[2];
imu_msg.orientation.z = -sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = -sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = -sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = -sd.accCalibrated.data[2]*9.81;
// Publish the messages
imu_pub.publish(imu_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpBE1Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
// Send command
cmdSetEnableAutocalibration(req.data);
ros::Duration(0.2).sleep();
cmdGetEnableAutocalibration();
ros::Duration(0.1).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
cmdGetEnableAutocalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
// Send command
cmdResetHeading();
res.success = true;
res.message = "[Success] Heading reset";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
cmdCalibrateGyroscope();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
ros::Duration(0.1).sleep();
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGotoCommandMode ()
{
IG1Command cmd;
cmd.command = GOTO_COMMAND_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGotoStreamingMode ()
{
IG1Command cmd;
cmd.command = GOTO_STREAM_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdCalibrateGyroscope()
{
IG1Command cmd;
cmd.command = START_GYR_CALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdResetHeading()
{
IG1Command cmd;
cmd.command = SET_ORIENTATION_OFFSET;
cmd.dataLength = 4;
cmd.data.i[0] = LPMS_OFFSET_MODE_HEADING;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdSetEnableAutocalibration(int status)
{
IG1Command cmd;
cmd.command = SET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 4;
cmd.data.i[0] = status;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetEnableAutocalibration()
{
IG1Command cmd;
cmd.command = GET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_be1_node");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpBE1Proxy lpBE1(nh);
lpBE1.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_ig1_node.cpp
+410
View File
@@ -0,0 +1,410 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpIG1Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher mag_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mag_msg;
// Parameters
std::string comportNo;
int baudrate;
bool autoReconnect;
std::string frame_id;
int rate;
LpIG1Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 921600);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsIG1 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
ROS_INFO("Settings");
ROS_INFO("Port: %s", comportNo.c_str());
ROS_INFO("Baudrate: %d", baudrate);
ROS_INFO("Auto reconnect: %s", autoReconnect? "Enabled":"Disabled");
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
mag_pub = nh.advertise<sensor_msgs::MagneticField>("mag",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpIG1Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpIG1Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpIG1Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpIG1Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpIG1Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpIG1Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpIG1Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpIG1Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = -sd.quaternion.data[1];
imu_msg.orientation.y = -sd.quaternion.data[2];
imu_msg.orientation.z = -sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = -sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = -sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = -sd.accCalibrated.data[2]*9.81;
/* Fill the magnetometer message */
mag_msg.header.stamp = imu_msg.header.stamp;
mag_msg.header.frame_id = frame_id;
// Units are microTesla in the LPMS library, Tesla in ROS.
mag_msg.magnetic_field.x = sd.magRaw.data[0]*1e-6;
mag_msg.magnetic_field.y = sd.magRaw.data[1]*1e-6;
mag_msg.magnetic_field.z = sd.magRaw.data[2]*1e-6;
// Publish the messages
imu_pub.publish(imu_msg);
mag_pub.publish(mag_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpIG1Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
// Send command
cmdSetEnableAutocalibration(req.data);
ros::Duration(0.2).sleep();
cmdGetEnableAutocalibration();
ros::Duration(0.1).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
cmdGetEnableAutocalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
// Send command
cmdResetHeading();
res.success = true;
res.message = "[Success] Heading reset";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
cmdCalibrateGyroscope();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
ros::Duration(0.1).sleep();
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGotoCommandMode ()
{
IG1Command cmd;
cmd.command = GOTO_COMMAND_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGotoStreamingMode ()
{
IG1Command cmd;
cmd.command = GOTO_STREAM_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdCalibrateGyroscope()
{
IG1Command cmd;
cmd.command = START_GYR_CALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdResetHeading()
{
IG1Command cmd;
cmd.command = SET_ORIENTATION_OFFSET;
cmd.dataLength = 4;
cmd.data.i[0] = LPMS_OFFSET_MODE_HEADING;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdSetEnableAutocalibration(int status)
{
IG1Command cmd;
cmd.command = SET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 4;
cmd.data.i[0] = status;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetEnableAutocalibration()
{
IG1Command cmd;
cmd.command = GET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_ig1_node");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpIG1Proxy lpIG1(nh);
lpIG1.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_ig1_rs485_client.cpp
+36
View File
@@ -0,0 +1,36 @@
#include "ros/ros.h"
#include <std_srvs/Trigger.h>
#include <cstdlib>
int main(int argc, char **argv)
{
ros::init(argc, argv, "lpms_ig1_rs485_client");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<std_srvs::TriggerRequest, std_srvs::TriggerResponse>("/imu/get_imu_data");
ros::Rate loop_rate(50);
std_srvs::TriggerRequest req;
std_srvs::TriggerResponse res;
int count = 0;
while (ros::ok())
{
if (client.call(req, res))
{
ROS_INFO("get_imu_data: %d", count++);
}
else
{
ROS_ERROR("Failed to call service get_imu_data");
return 1;
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_ig1_rs485_node.cpp
+369
View File
@@ -0,0 +1,369 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpIG1Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher mag_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mag_msg;
// Parameters
std::string comportNo;
int baudrate;
int startupMode;
bool autoReconnect;
std::string frame_id;
int rs485ControlPin;
int rs485ControlPinToggleWaitMs;
int rate;
LpIG1Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 115200);
private_nh.param("startupmode", startupMode, SENSOR_MODE_STREAMING);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param("rs485ControlPin", rs485ControlPin, -1);
private_nh.param("rs485ControlPinToggleWaitMs", rs485ControlPinToggleWaitMs, 2);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsIG1 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
sensor1->setStartupSensorMode(startupMode);
sensor1->setConnectionInterface(CONNECTION_INTERFACE_RS485);
sensor1->setControlGPIOForRs485(rs485ControlPin);
sensor1->setControlGPIOToggleWaitMs(rs485ControlPinToggleWaitMs);
ROS_INFO("Settings");
ROS_INFO("Port: %s", comportNo.c_str());
ROS_INFO("Baudrate: %d", baudrate);
ROS_INFO("Startup mode: %s", (startupMode == 0)? "Command mode":"Streaming mode");
ROS_INFO("Auto reconnect: %s", autoReconnect? "Enabled":"Disabled");
ROS_INFO("rs485ControlPin: %d", rs485ControlPin);
ROS_INFO("rs485ControlPinToggleWaitMs: %d", rs485ControlPinToggleWaitMs);
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
mag_pub = nh.advertise<sensor_msgs::MagneticField>("mag",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpIG1Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpIG1Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpIG1Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpIG1Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpIG1Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpIG1Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpIG1Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect. Sensor status: %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
//sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpIG1Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = -sd.quaternion.data[1];
imu_msg.orientation.y = -sd.quaternion.data[2];
imu_msg.orientation.z = -sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = -sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = -sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = -sd.accCalibrated.data[2]*9.81;
/* Fill the magnetometer message */
mag_msg.header.stamp = imu_msg.header.stamp;
mag_msg.header.frame_id = frame_id;
// Units are microTesla in the LPMS library, Tesla in ROS.
mag_msg.magnetic_field.x = sd.magRaw.data[0]*1e-6;
mag_msg.magnetic_field.y = sd.magRaw.data[1]*1e-6;
mag_msg.magnetic_field.z = sd.magRaw.data[2]*1e-6;
// Publish the messages
imu_pub.publish(imu_msg);
mag_pub.publish(mag_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpIG1Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
sensor1->commandSetGyroAutoCalibration(req.data);
ros::Duration(0.2).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
sensor1->commandGetGyroAutoCalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
res.success = true;
res.message = "[Success] Heading resets";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
sensor1->commandStartGyroCalibration();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
//sensor1->commandGotoStreamingMode();
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
sensor1->commandGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
sensor1->commandGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_ig1_node_rs485");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpIG1Proxy lpIG1(nh);
lpIG1.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_nav3_node.cpp
+399
View File
@@ -0,0 +1,399 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
//! Manages connection with the sensor, publishes data
/*!
\TODO: Make noncopyable!
*/
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpNAV3Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
// Parameters
std::string comportNo;
int baudrate;
bool autoReconnect;
std::string frame_id;
int rate;
LpNAV3Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 115200);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsNAV3 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpNAV3Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpNAV3Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpNAV3Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpNAV3Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpNAV3Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpNAV3Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpNAV3Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
//logd(TAG, "Error connecting to sensor\n");
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpNAV3Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = sd.quaternion.data[1];
imu_msg.orientation.y = sd.quaternion.data[2];
imu_msg.orientation.z = sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = sd.accCalibrated.data[2]*9.81;
// Publish the messages
imu_pub.publish(imu_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpNAV3Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
// Send command
cmdSetEnableAutocalibration(req.data);
ros::Duration(0.2).sleep();
cmdGetEnableAutocalibration();
ros::Duration(0.1).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
cmdGetEnableAutocalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
// Send command
cmdResetHeading();
res.success = true;
res.message = "[Success] Heading reset";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
cmdCalibrateGyroscope();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
ros::Duration(0.1).sleep();
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGotoCommandMode ()
{
IG1Command cmd;
cmd.command = GOTO_COMMAND_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGotoStreamingMode ()
{
IG1Command cmd;
cmd.command = GOTO_STREAM_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdCalibrateGyroscope()
{
IG1Command cmd;
cmd.command = START_GYR_CALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdResetHeading()
{
IG1Command cmd;
cmd.command = SET_ORIENTATION_OFFSET;
cmd.dataLength = 4;
cmd.data.i[0] = LPMS_OFFSET_MODE_HEADING;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdSetEnableAutocalibration(int status)
{
IG1Command cmd;
cmd.command = SET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 4;
cmd.data.i[0] = status;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetEnableAutocalibration()
{
IG1Command cmd;
cmd.command = GET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_be1_node");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpNAV3Proxy lpNAV3(nh);
lpNAV3.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/quat_to_euler_node.cpp
+37
View File
@@ -0,0 +1,37 @@
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>
#include <tf/transform_datatypes.h>
ros::Publisher rpy_publisher;
ros::Subscriber quat_subscriber;
const float r2d = 57.29577951f;
void MsgCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
tf::Quaternion q(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
rpy_publisher.publish(rpy);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_listener");
ros::NodeHandle n;
rpy_publisher = n.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);
quat_subscriber = n.subscribe("/imu/data", 1000, MsgCallback);
ROS_INFO("waiting for imu data");
ros::spin();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/version.rc
+32
View File
@@ -0,0 +1,32 @@
#include <winres.h>
VS_VERSION_INFO VERSIONINFO
FILEVERSION 0, 0, 3, 0
PRODUCTVERSION 0, 0, 3, 0
FILEFLAGSMASK 0x17L
#ifdef _DEBUG
FILEFLAGS 0x1L
#else
FILEFLAGS 0x0L
#endif
FILEOS 0x4L
FILETYPE 0x1L
FILESUBTYPE 0x0L
BEGIN
BLOCK "StringFileInfo"
BEGIN
BLOCK "040904b0"
BEGIN
VALUE "FileDescription", "LpmsIG1_OpenSourceLib"
VALUE "FileVersion", "0, 3, 0"
VALUE "InternalName", "LpmsIG1_OpenSourceLib"
VALUE "LegalCopyright", "Copyright (C) 2021 LP-Research Inc."
VALUE "OriginalFilename", "LpmsIG1_OpenSourceLib.dll"
VALUE "ProductName", "LpmsIG1_OpenSourceLib"
VALUE "ProductVersion", "0.3.0 (Build 20210907)"
END
END
BLOCK "VarFileInfo"
BEGIN
VALUE "Translation", 0x409, 1200
END
END