HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
Devices/Packages/wit_wt901ble_reader/.vscode/c_cpp_properties.json vendored Executable file
View File

@@ -0,0 +1,79 @@
{
"configurations": [
{
"browse": {
"databaseFilename": "${default}",
"limitSymbolsToIncludedHeaders": false
},
"includePath": [
"/home/huandt/HuanDT/projects_ws/devel/include/**",
"/opt/ros/noetic/include/**",
"/home/huandt/HuanDT/planner_ws/devel/include/**",
"/home/huandt/HuanDT/planner_ws/src/agv_define/agv_define/include/**",
"/home/huandt/HuanDT/planner_ws/src/agv_define/agv_dynparam/include/**",
"/home/huandt/HuanDT/planner_ws/src/agv_path_smoothing/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/amcl/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/base_local_planner/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/carrot_planner/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/clear_costmap_recovery/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/costmap_2d/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/costmap_converter/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/costmap_queue/include/**",
"/home/huandt/HuanDT/planner_ws/src/custom_planner/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/mir_robot/diff_drive_controller/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwa_local_planner/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwb_critics/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwb_local_planner/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwb_plugins/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/follow_waypoints/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geographic_info/geodesy/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/global_planner/include/**",
"/home/huandt/HuanDT/projects_ws/src/ros_basic/libcpp_pkg/include/**",
"/home/huandt/HuanDT/planner_ws/src/make_pathway/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/map_server/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_abstract_core/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_abstract_nav/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_costmap_core/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_costmap_nav/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_simple_nav/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_utility/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/mir_robot/mir_dwb_critics/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_slow_and_clear/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_2d_utils/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_core/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_core2/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_core_adapter/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_grid/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_grid_iterators/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/navfn/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/pluginlib/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/pose_follower/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/robot_localization/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/rotate_recovery/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/sbpl_lattice_planner/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/sbpl_recovery/include/**",
"/home/huandt/HuanDT/projects_ws/src/serial/include/**",
"/home/huandt/HuanDT/projects_ws/src/sick_safetyscanners/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/teb_local_planner/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geometry2/tf2_geometry_msgs/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geometry2/tf2_ros/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geometry2/tf2_sensor_msgs/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/twist_recovery/include/**",
"/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/unique_identifier/unique_id/include/**",
"/home/huandt/HuanDT/planner_ws/src/vda5050_api/vda5050_api/include/**",
"/home/huandt/HuanDT/planner_ws/src/vda5050_api/vda5050_connector/include/**",
"/home/huandt/HuanDT/planner_ws/src/vda5050_api/vda5050_msgs/include/**",
"/home/huandt/HuanDT/projects_ws/src/waveshare_tof_reader/include/**",
"/home/huandt/HuanDT/projects_ws/src/wit_wt901ble_reader/include/**",
"/usr/include/**"
],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14"
}
],
"version": 4
}

83
Devices/Packages/wit_wt901ble_reader/.vscode/settings.json vendored Executable file
View File

@@ -0,0 +1,83 @@
{
"python.autoComplete.extraPaths": [
"/home/huandt/HuanDT/projects_ws/devel/lib/python3/dist-packages",
"/home/huandt/HuanDT/planner_ws/devel/lib/python3/dist-packages",
"/opt/ros/noetic/lib/python3/dist-packages"
],
"python.analysis.extraPaths": [
"/home/huandt/HuanDT/projects_ws/devel/lib/python3/dist-packages",
"/home/huandt/HuanDT/planner_ws/devel/lib/python3/dist-packages",
"/opt/ros/noetic/lib/python3/dist-packages"
],
"files.associations": {
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"array": "cpp",
"atomic": "cpp",
"strstream": "cpp",
"bit": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"cinttypes": "cpp",
"codecvt": "cpp",
"complex": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"forward_list": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"hash_map": "cpp",
"hash_set": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"shared_mutex": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp"
}
}

101
Devices/Packages/wit_wt901ble_reader/CMakeLists.txt Executable file
View File

@@ -0,0 +1,101 @@
cmake_minimum_required(VERSION 3.0.2)
project(wit_wt901ble_reader)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_FLAGS "-std=c++11 -g ${CMAKE_CXX_FLAGS}")
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
serial
diagnostic_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES wit_wt901ble_reader
CATKIN_DEPENDS roscpp std_msgs serial diagnostic_msgs
# DEPENDS system_lib
)
SET(-ludev udev)
## Declare a C++ library
add_library(${PROJECT_NAME}
src/serial.c
src/wit_c_sdk.c
src/wit_wt901ble_reader.cc
)
target_include_directories(${PROJECT_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES} ${-ludev})
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
add_executable(${PROJECT_NAME}_node src/wit_wt901ble_reader_main.cc)
target_include_directories(${PROJECT_NAME}_node PUBLIC ${catkin_INCLUDE_DIRS} include)
target_link_libraries(${PROJECT_NAME}_node PUBLIC ${catkin_LIBRARIES} ${PROJECT_NAME})
add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(TARGETS ${PROJECT_NAME}_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
install(DIRECTORY
launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_waveshare_tof_reader.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

58
Devices/Packages/wit_wt901ble_reader/README.md Executable file
View File

@@ -0,0 +1,58 @@
# Requirements:
- Ubuntu 20.04.6 LTS
- ROS Noetic
- Install SDL library:
```
sudo apt-get install libsdl-image1.2-dev
sudo apt-get install libsdl-dev
```
- Install libudev-dev:
```
sudo apt install libudev-dev
```
- Install Serial package:
```
sudo apt-get install ros-noetic-serial
```
# Wiring and configurations:
- Following on wiki website: https://drive.google.com/drive/folders/1NlOFHSTYNy2bRAfaA0S25BEaXK4uvia9
# Build:
- Direct to the workspace and run command:
```
catkin_make
```
# Run:
- Configure parametters in .launch file:
```
<!-- Tốc độ baud của cổng serial -->
<arg name="baudrate" value="115200"/>
<!-- Tên cổng USB kết nối với cảm biến -->
<arg name="portname" value="/dev/USB_IMU"/>
<!-- Tên topic dữ liệu cảm biến -->
<arg name="topic_name" value="/imu" />
<!-- Frame id -->
<arg name="frame_id" value="imu" />
```
- Permit the serial port:
```
sudo chmod 777 /dev/USB_IMU
```
- Run the lannch file:
```
roslaunch wit_wt901ble_reader wit_wt901ble_reader.launch
```
# Messages of datas:
- The output message of the IMU:
```
name: custom topic name in wit_wt901ble_reader wit_wt901ble_reader.launch
<arg name="topic_name" value="/imu" />
type: sensor_msgs/Imu.msg
```

284
Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/REG.h Executable file
View File

@@ -0,0 +1,284 @@
#ifndef __AHRSREG_H
#define __AHRSREG_H
#ifdef __cplusplus
extern "C" {
#endif
#define REGSIZE 0x90
#define SAVE 0x00
#define CALSW 0x01
#define RSW 0x02
#define RRATE 0x03
#define BAUD 0x04
#define AXOFFSET 0x05
#define AYOFFSET 0x06
#define AZOFFSET 0x07
#define GXOFFSET 0x08
#define GYOFFSET 0x09
#define GZOFFSET 0x0a
#define HXOFFSET 0x0b`
#define HYOFFSET 0x0c
#define HZOFFSET 0x0d
#define D0MODE 0x0e
#define D1MODE 0x0f
#define D2MODE 0x10
#define D3MODE 0x11
#define D0PWMH 0x12
#define D1PWMH 0x13
#define D2PWMH 0x14
#define D3PWMH 0x15
#define D0PWMT 0x16
#define D1PWMT 0x17
#define D2PWMT 0x18
#define D3PWMT 0x19
#define IICADDR 0x1a
#define LEDOFF 0x1b
#define MAGRANGX 0x1c
#define MAGRANGY 0x1d
#define MAGRANGZ 0x1e
#define BANDWIDTH 0x1f
#define GYRORANGE 0x20
#define ACCRANGE 0x21
#define SLEEP 0x22
#define ORIENT 0x23
#define AXIS6 0x24
#define FILTK 0x25
#define GPSBAUD 0x26
#define READADDR 0x27
#define BWSCALE 0x28
#define MOVETHR 0x28
#define MOVESTA 0x29
#define ACCFILT 0x2A
#define GYROFILT 0x2b
#define MAGFILT 0x2c
#define POWONSEND 0x2d
#define VERSION 0x2e
#define CCBW 0x2f
#define YYMM 0x30
#define DDHH 0x31
#define MMSS 0x32
#define MS 0x33
#define AX 0x34
#define AY 0x35
#define AZ 0x36
#define GX 0x37
#define GY 0x38
#define GZ 0x39
#define HX 0x3a
#define HY 0x3b
#define HZ 0x3c
#define Roll 0x3d
#define Pitch 0x3e
#define Yaw 0x3f
#define TEMP 0x40
#define D0Status 0x41
#define D1Status 0x42
#define D2Status 0x43
#define D3Status 0x44
#define PressureL 0x45
#define PressureH 0x46
#define HeightL 0x47
#define HeightH 0x48
#define LonL 0x49
#define LonH 0x4a
#define LatL 0x4b
#define LatH 0x4c
#define GPSHeight 0x4d
#define GPSYAW 0x4e
#define GPSVL 0x4f
#define GPSVH 0x50
#define q0 0x51
#define q1 0x52
#define q2 0x53
#define q3 0x54
#define SVNUM 0x55
#define PDOP 0x56
#define HDOP 0x57
#define VDOP 0x58
#define DELAYT 0x59
#define XMIN 0x5a
#define XMAX 0x5b
#define BATVAL 0x5c
#define ALARMPIN 0x5d
#define YMIN 0x5e
#define YMAX 0x5f
#define GYROZSCALE 0x60
#define GYROCALITHR 0x61
#define ALARMLEVEL 0x62
#define GYROCALTIME 0x63
#define REFROLL 0x64
#define REFPITCH 0x65
#define REFYAW 0x66
#define GPSTYPE 0x67
#define TRIGTIME 0x68
#define KEY 0x69
#define WERROR 0x6a
#define TIMEZONE 0x6b
#define CALICNT 0x6c
#define WZCNT 0x6d
#define WZTIME 0x6e
#define WZSTATIC 0x6f
#define ACCSENSOR 0x70
#define GYROSENSOR 0x71
#define MAGSENSOR 0x72
#define PRESSENSOR 0x73
#define MODDELAY 0x74
#define ANGLEAXIS 0x75
#define XRSCALE 0x76
#define YRSCALE 0x77
#define ZRSCALE 0x78
#define XREFROLL 0x79
#define YREFPITCH 0x7a
#define ZREFYAW 0x7b
#define ANGXOFFSET 0x7c
#define ANGYOFFSET 0x7d
#define ANGZOFFSET 0x7e
#define NUMBERID1 0x7f
#define NUMBERID2 0x80
#define NUMBERID3 0x81
#define NUMBERID4 0x82
#define NUMBERID5 0x83
#define NUMBERID6 0x84
#define XA85PSCALE 0x85
#define XA85NSCALE 0x86
#define YA85PSCALE 0x87
#define YA85NSCALE 0x88
#define XA30PSCALE 0x89
#define XA30NSCALE 0x8a
#define YA30PSCALE 0x8b
#define YA30NSCALE 0x8c
#define CHIPIDL 0x8D
#define CHIPIDH 0x8E
#define REGINITFLAG REGSIZE-1
/* AXIS6 */
#define ALGRITHM9 0
#define ALGRITHM6 1
/************CALSW**************/
#define NORMAL 0x00
#define CALGYROACC 0x01
#define CALMAG 0x02
#define CALALTITUDE 0x03
#define CALANGLEZ 0x04
#define CALACCL 0x05
#define CALACCR 0x06
#define CALMAGMM 0x07
#define CALREFANGLE 0x08
#define CALMAG2STEP 0x09
//#define CALACCX 0x09
//#define ACC45PRX 0x0A
//#define ACC45NRX 0x0B
//#define CALACCY 0x0C
//#define ACC45PRY 0x0D
//#define ACC45NRY 0x0E
//#define CALREFANGLER 0x0F
//#define CALACCINIT 0x10
//#define CALREFANGLEINIT 0x11
#define CALHEXAHEDRON 0x12
/************OUTPUTHEAD**************/
#define WIT_TIME 0x50
#define WIT_ACC 0x51
#define WIT_GYRO 0x52
#define WIT_ANGLE 0x53
#define WIT_MAGNETIC 0x54
#define WIT_DPORT 0x55
#define WIT_PRESS 0x56
#define WIT_GPS 0x57
#define WIT_VELOCITY 0x58
#define WIT_QUATER 0x59
#define WIT_GSA 0x5A
#define WIT_REGVALUE 0x5F
/************RSW**************/
#define RSW_TIME 0x01
#define RSW_ACC 0x02
#define RSW_GYRO 0x04
#define RSW_ANGLE 0x08
#define RSW_MAG 0x10
#define RSW_PORT 0x20
#define RSW_PRESS 0x40
#define RSW_GPS 0x80
#define RSW_V 0x100
#define RSW_Q 0x200
#define RSW_GSA 0x400
#define RSW_MASK 0xfff
/**RRATE*****/
#define RRATE_NONE 0x0d
#define RRATE_02HZ 0x01
#define RRATE_05HZ 0x02
#define RRATE_1HZ 0x03
#define RRATE_2HZ 0x04
#define RRATE_5HZ 0x05
#define RRATE_10HZ 0x06
#define RRATE_20HZ 0x07
#define RRATE_50HZ 0x08
#define RRATE_100HZ 0x09
#define RRATE_125HZ 0x0a //only WT931
#define RRATE_200HZ 0x0b
#define RRATE_ONCE 0x0c
/* BAUD */
#define WIT_BAUD_4800 1
#define WIT_BAUD_9600 2
#define WIT_BAUD_19200 3
#define WIT_BAUD_38400 4
#define WIT_BAUD_57600 5
#define WIT_BAUD_115200 6
#define WIT_BAUD_230400 7
#define WIT_BAUD_460800 8
#define WIT_BAUD_921600 9
/*CAN BAUD*/
#define CAN_BAUD_1000000 0
#define CAN_BAUD_800000 1
#define CAN_BAUD_500000 2
#define CAN_BAUD_400000 3
#define CAN_BAUD_250000 4
#define CAN_BAUD_200000 5
#define CAN_BAUD_125000 6
#define CAN_BAUD_100000 7
#define CAN_BAUD_80000 8
#define CAN_BAUD_50000 9
#define CAN_BAUD_40000 10
#define CAN_BAUD_20000 11
#define CAN_BAUD_10000 12
#define CAN_BAUD_5000 13
#define CAN_BAUD_3000 14
/* KEY */
#define KEY_UNLOCK 0xB588
/* SAVE */
#define SAVE_PARAM 0x00
#define SAVE_SWRST 0xFF
/* ORIENT */
#define ORIENT_HERIZONE 0
#define ORIENT_VERTICLE 1
/* BANDWIDTH */
#define BANDWIDTH_256HZ 0
#define BANDWIDTH_184HZ 1
#define BANDWIDTH_94HZ 2
#define BANDWIDTH_44HZ 3
#define BANDWIDTH_21HZ 4
#define BANDWIDTH_10HZ 5
#define BANDWIDTH_5HZ 6
#ifdef __cplusplus
}
#endif
#endif

28
Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/serial.h Executable file
View File

@@ -0,0 +1,28 @@
#ifndef SERIAL_H
#define SERIAL_H
#include <linux/types.h>
#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <string.h>
#include <linux/rtc.h>
#include <termios.h>
void serial_close(int fd);
int serial_open(unsigned char* dev, unsigned int baud);
int serial_read_data(int fd, unsigned char *val, int len);
int serial_write_data(int fd, unsigned char *val, int len);
#endif

119
Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/udevadm_info.h Executable file
View File

@@ -0,0 +1,119 @@
#ifndef __UDEVADM_INFO_H_INCLUDE__
#define __UDEVADM_INFO_H_INCLUDE__
#include <ros/ros.h>
#include <unistd.h>
#include <poll.h>
#include <cstdio>
#include <cstdlib>
#include <cerrno>
#include <cstring>
#include <sys/signalfd.h>
#include <csignal>
#include <libudev.h>
#include <string>
class udevadmInfo {
public:
udevadmInfo(const char *product_name);
virtual ~udevadmInfo(void);
virtual int init();
virtual bool scanDevices(void);
const char *port;
protected:
struct udev *udev;
char *product_name_;
};
udevadmInfo::udevadmInfo(const char *product_name)
: product_name_((char *)product_name)
{
}
udevadmInfo::~udevadmInfo(void){
udev_unref(udev);
}
int udevadmInfo::init() {
udev = udev_new();
if (!udev) {
printf("Error while initialization!\n");
return EXIT_FAILURE;
}
sigset_t mask;
// Set signals we want to catch
sigemptyset(&mask);
sigaddset(&mask, SIGTERM);
sigaddset(&mask, SIGINT);
// Change the signal mask and check
if (sigprocmask(SIG_BLOCK, &mask, nullptr) < 0) {
ROS_ERROR("UDEV-Error while sigprocmask(): %s\n", std::strerror(errno));
return EXIT_FAILURE;
}
// Get a signal file descriptor
int signal_fd = signalfd(-1, &mask, 0);
// Check the signal file descriptor
if (signal_fd < 0) {
ROS_ERROR("UDEV-Error while signalfd(): %s\n", std::strerror(errno));
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
bool udevadmInfo::scanDevices(void) {
struct udev_device *device;
struct udev_enumerate *enumerate;
struct udev_list_entry *devices, *dev_list_entry;
// Create enumerate object
enumerate = udev_enumerate_new(udev);
if (!enumerate) {
ROS_ERROR("Error while creating udev enumerate\n");
return false;
}
// Scan devices
udev_enumerate_scan_devices(enumerate);
// Fill up device list
devices = udev_enumerate_get_list_entry(enumerate);
if (!devices) {
ROS_ERROR("Error while getting device list\n");
return false;
}
udev_list_entry_foreach(dev_list_entry, devices) {
const char* path = udev_list_entry_get_name(dev_list_entry);
// Get the device
device = udev_device_new_from_syspath(udev, udev_list_entry_get_name(dev_list_entry));
// Print device information
if(udev_device_get_devnode(device) != NULL && strstr(udev_device_get_sysname(device),"ttyUSB") != NULL) {
// ROS_INFO("DEVNODE=%s", udev_device_get_devnode(device));
struct udev_device* usb = udev_device_get_parent_with_subsystem_devtype(device,"usb","usb_device");
// ROS_INFO("usb = %s:%s, manufacturer = %s, product = %s, serial = %s, speed = %s, bMaxPower = %s",
// udev_device_get_sysattr_value(usb, "idVendor"),
// udev_device_get_sysattr_value(usb, "idProduct"),
// udev_device_get_sysattr_value(usb, "manufacturer"),
// udev_device_get_sysattr_value(usb, "product"),
// udev_device_get_sysattr_value(usb, "serial"),
// udev_device_get_sysattr_value(usb, "speed"),
// udev_device_get_sysattr_value(usb, "bMaxPower"));
if(strstr(udev_device_get_sysattr_value(usb, "product"), (const char *)product_name_) != NULL) {
port = udev_device_get_devnode(device);
return true;
}
}
else {
continue;
}
// Free the device
udev_device_unref(device);
}
// Free enumerate
udev_enumerate_unref(enumerate);
return false;
}
#endif

135
Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_c_sdk.h Executable file
View File

@@ -0,0 +1,135 @@
#ifndef __WIT_C_SDK_H
#define __WIT_C_SDK_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "wit_wt901ble_reader/REG.h"
#define WIT_HAL_OK (0) /**< There is no error */
#define WIT_HAL_BUSY (-1) /**< Busy */
#define WIT_HAL_TIMEOUT (-2) /**< Timed out */
#define WIT_HAL_ERROR (-3) /**< A generic error happens */
#define WIT_HAL_NOMEM (-4) /**< No memory */
#define WIT_HAL_EMPTY (-5) /**< The resource is empty */
#define WIT_HAL_INVAL (-6) /**< Invalid argument */
#define WIT_DATA_BUFF_SIZE 256
#define WIT_PROTOCOL_NORMAL 0
#define WIT_PROTOCOL_MODBUS 1
#define WIT_PROTOCOL_CAN 2
#define WIT_PROTOCOL_I2C 3
/* serial function */
typedef void (*SerialWrite)(uint8_t *p_ucData, uint32_t uiLen);
int32_t WitSerialWriteRegister(SerialWrite write_func);
void WitSerialDataIn(uint8_t ucData);
/* iic function */
/*
i2c write function example
int32_t WitI2cWrite(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen)
{
i2c_start();
i2c_send(ucAddr);
if(i2c_wait_ask() != SUCCESS)return 0;
i2c_send(ucReg);
if(i2c_wait_ask() != SUCCESS)return 0;
for(uint32_t i = 0; i < uiLen; i++)
{
i2c_send(*p_ucVal++);
if(i2c_wait_ask() != SUCCESS)return 0;
}
i2c_stop();
return 1;
}
*/
typedef int32_t (*WitI2cWrite)(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen);
/*
i2c read function example
int32_t WitI2cRead(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen)
{
i2c_start();
i2c_send(ucAddr);
if(i2c_wait_ask() != SUCCESS)return 0;
i2c_send(ucReg);
if(i2c_wait_ask() != SUCCESS)return 0;
i2c_start();
i2c_send(ucAddr+1);
for(uint32_t i = 0; i < uiLen; i++)
{
if(i+1 == uiLen)*p_ucVal++ = i2c_read(0); //last byte no ask
else *p_ucVal++ = i2c_read(1); // ask
}
i2c_stop();
return 1;
}
*/
typedef int32_t (*WitI2cRead)(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen);
int32_t WitI2cFuncRegister(WitI2cWrite write_func, WitI2cRead read_func);
/* can function */
typedef void (*CanWrite)(uint8_t ucStdId, uint8_t *p_ucData, uint32_t uiLen);
int32_t WitCanWriteRegister(CanWrite write_func);
/* Delayms function */
typedef void (*DelaymsCb)(uint16_t ucMs);
int32_t WitDelayMsRegister(DelaymsCb delayms_func);
void WitCanDataIn(uint8_t ucData[8], uint8_t ucLen);
typedef void (*RegUpdateCb)(uint32_t uiReg, uint32_t uiRegNum);
int32_t WitRegisterCallBack(RegUpdateCb update_func);
int32_t WitWriteReg(uint32_t uiReg, uint16_t usData);
int32_t WitReadReg(uint32_t uiReg, uint32_t uiReadNum);
int32_t WitInit(uint32_t uiProtocol, uint8_t ucAddr);
void WitDeInit(void);
/**
******************************************************************************
* @file wit_c_sdk.h
* @author Wit
* @version V1.0
* @date 05-May-2022
* @brief This file provides all Configure sensor function.
******************************************************************************
* @attention
*
* http://wit-motion.cn/
*
******************************************************************************
*/
int32_t WitStartAccCali(void);
int32_t WitStopAccCali(void);
int32_t WitStartMagCali(void);
int32_t WitStopMagCali(void);
int32_t WitSetUartBaud(int32_t uiBaudIndex);
int32_t WitSetBandwidth(int32_t uiBaudWidth);
int32_t WitSetOutputRate(int32_t uiRate);
int32_t WitSetContent(int32_t uiRsw);
int32_t WitSetCanBaud(int32_t uiBaudIndex);
char CheckRange(short sTemp,short sMin,short sMax);
extern int16_t sReg[REGSIZE];
#ifdef __cplusplus
}
#endif
#endif /* __WIT_C_SDK_H */

130
Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_wt901ble_reader.h Executable file
View File

@@ -0,0 +1,130 @@
#ifndef _WT901BLE_READER_H_
#define _WT901BLE_READER_H_
#pragma once
#include <iostream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <unistd.h>
#include <cstdint>
#include <cmath>
#include <serial/serial.h>
#include <chrono>
#include <sys/time.h>
#include <ctime>
#include <ros/ros.h>
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Quaternion.h"
#include <boost/thread.hpp>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <poll.h>
#include <cstdio>
#include <cstdlib>
#include <cerrno>
#include <cstring>
#include <sys/signalfd.h>
#include <csignal>
#include <libudev.h>
#include "wit_wt901ble_reader/udevadm_info.h"
using namespace std;
namespace wt901ble_reader
{
static bool is_imu_connected = false;
static bool is_imu_data_available = false;
static uint16_t lost_data_count = 0;
const uint8_t wt901ble_data_length = 20;
const uint8_t wt901ble_header_imu[2] = {0x55, 0x61};
struct wt901ble_data{
wt901ble_data()
{
aX = 0;
aY = 0;
aZ = 0;
wX = 0;
wY = 0;
wZ = 0;
Roll = 0;
Pitch = 0;
Yaw = 0;
}
double aX;
double aY;
double aZ;
double wX;
double wY;
double wZ;
double Roll;
double Pitch;
double Yaw;
};
struct UserParams{
UserParams(){
baudrate = 115200;
portname = "/dev/ttyUSB2";
topic_name = "/imu";
frame_id = "imu";
}
uint32_t baudrate;
string portname;
string topic_name;
string frame_id;
};
class wt901ble_node
{
public:
wt901ble_node(const string& nodename);
~wt901ble_node();
private:
ros::NodeHandle nh_;
ros::Publisher wt901ble_data_pub_;
ros::WallTimer get_and_publish_timer;
ros::WallTimer reconnect_timer;
serial::Serial* serial_;
UserParams* userParams;
wt901ble_data* wt901ble_data_;
bool LoadParams(UserParams* userParams_, const string& node_name);
void GetAndPublishData(const ::ros::WallTimerEvent& timer_event);
void ReconnectSerial(const ::ros::WallTimerEvent& timer_event);
size_t readN(uint8_t *buf, size_t len);
bool receiveData();
bool recvData(uint8_t *buf);
geometry_msgs::Quaternion getQuaternion(double yaw_, double pitch_, double roll_);
bool detect_port(string& port_name);
sensor_msgs::Imu imu_data;
string nodename_;
bool isConnected_;
bool isDataAvailable;
bool is_try_to_connect;
bool is_reconnected;
uint8_t data_buf[wt901ble_data_length]{0};
};
class Diagnostics
{
public:
Diagnostics(string& nodename);
~Diagnostics();
private:
ros::NodeHandle nh_;
ros::Publisher diagnostics_pub;
boost::thread* diagnostics_thread;
boost::mutex mutex;
void run_diagnostics();
void pubDiagnostics();
string nodename_;
string imu_diagnostics_topic_name_;
diagnostic_msgs::DiagnosticStatus connection;
diagnostic_msgs::DiagnosticArray prev_diagnostic;
};
}
#endif

26
Devices/Packages/wit_wt901ble_reader/launch/wit_wt901ble_reader.launch Executable file
View File

@@ -0,0 +1,26 @@
<launch>
<!-- Tốc độ baud của cổng serial -->
<arg name="baudrate" default="115200"/>
<!-- Tên cổng USB kết nối với cảm biến -->
<arg name="portname" default="/dev/USB_IMU"/>
<!-- Tên topic dữ liệu cảm biến -->
<arg name="topic_name" default="/imu" />
<!-- Frame id -->
<arg name="frame_id" default="imu" />
<!-- Tên topic diagnostics -->
<arg name="imu_diagnostics_topic_name" default="/imu_diagnostics" />
<node pkg="wit_wt901ble_reader" name="wit_wt901ble_reader_node" type="wit_wt901ble_reader_node" output="screen">
<param name="baudrate" type="int" value="$(arg baudrate)"/>
<param name="portname" type="string" value="$(arg portname)"/>
<param name="topic_name" type="string" value="$(arg topic_name)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="imu_diagnostics_topic_name" type="string" value="$(arg imu_diagnostics_topic_name)"/>
</node>
</launch>

69
Devices/Packages/wit_wt901ble_reader/package.xml Executable file
View File

@@ -0,0 +1,69 @@
<?xml version="1.0"?>
<package format="2">
<name>wit_wt901ble_reader</name>
<version>0.0.0</version>
<description>The wit_wt901ble_reader package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="huandt@todo.todo">huandt</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 multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/wit_wt901ble_reader</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>serial</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>serial</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>serial</exec_depend>
<build_depend>diagnostic_msgs</build_depend>
<exec_depend>diagnostic_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

173
Devices/Packages/wit_wt901ble_reader/src/main.c Executable file
View File

@@ -0,0 +1,173 @@
#include "wit_wt901ble_reader/serial.h"
#include "wit_wt901ble_reader/wit_c_sdk.h"
#include "wit_wt901ble_reader/REG.h"
#include <stdint.h>
#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
#define ANGLE_UPDATE 0x04
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80
static int fd, s_iCurBaud = 115200;
static volatile char s_cDataUpdate = 0;
const int c_uiBaud[] = {2400 , 4800 , 9600 , 19200 , 38400 , 57600 , 115200 , 230400 , 460800 , 921600};
static void AutoScanSensor(char* dev);
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);
int main(int argc,char* argv[]){
if(argc < 2)
{
printf("please input dev name\n");
return 0;
}
if((fd = serial_open(argv[1] , 115200)<0))
{
printf("open %s fail\n", argv[1]);
return 0;
}
else printf("open %s success\n", argv[1]);
float fAcc[3], fGyro[3], fAngle[3];
int i , ret;
char cBuff[1];
WitInit(WIT_PROTOCOL_NORMAL, 0x50);
WitRegisterCallBack(SensorDataUpdata);
printf("\r\n********************** wit-motion Normal example ************************\r\n");
AutoScanSensor(argv[1]);
while(1)
{
while(serial_read_data(fd, cBuff, 1))
{
WitSerialDataIn(cBuff[0]);
}
printf("\n");
Delayms(500);
if(s_cDataUpdate)
{
for(i = 0; i < 3; i++)
{
fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
}
if(s_cDataUpdate & ACC_UPDATE)
{
printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{
printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{
printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
s_cDataUpdate &= ~MAG_UPDATE;
}
}
}
serial_close(fd);
return 0;
}
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
int i;
for(i = 0; i < uiRegNum; i++)
{
switch(uiReg)
{
// case AX:
// case AY:
case AZ:
s_cDataUpdate |= ACC_UPDATE;
break;
// case GX:
// case GY:
case GZ:
s_cDataUpdate |= GYRO_UPDATE;
break;
// case HX:
// case HY:
case HZ:
s_cDataUpdate |= MAG_UPDATE;
break;
// case Roll:
// case Pitch:
case Yaw:
s_cDataUpdate |= ANGLE_UPDATE;
break;
default:
s_cDataUpdate |= READ_UPDATE;
break;
}
uiReg++;
}
}
static void Delayms(uint16_t ucMs)
{
usleep(ucMs*1000);
}
static void AutoScanSensor(char* dev)
{
int i, iRetry;
char cBuff[1];
for(i = 1; i < 10; i++)
{
serial_close(fd);
s_iCurBaud = c_uiBaud[i];
fd = serial_open(dev , c_uiBaud[i]);
iRetry = 2;
do
{
s_cDataUpdate = 0;
WitReadReg(AX, 3);
Delayms(200);
while(serial_read_data(fd, cBuff, 1))
{
WitSerialDataIn(cBuff[0]);
}
if(s_cDataUpdate != 0)
{
printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]);
return ;
}
iRetry--;
}while(iRetry);
}
printf("can not find sensor\r\n");
printf("please check your connection\r\n");
}

100
Devices/Packages/wit_wt901ble_reader/src/serial.c Executable file
View File

@@ -0,0 +1,100 @@
#include "wit_wt901ble_reader/serial.h"
int serial_open(unsigned char* dev, unsigned int baud)
{
int fd;
fd = open(dev, O_RDWR|O_NOCTTY);
if (fd < 0) return fd;
if(isatty(STDIN_FILENO)==0)
{
printf("standard input is not a terminal device\n");
}
else
{
printf("isatty success!\n");
}
struct termios newtio,oldtio;
if (tcgetattr( fd,&oldtio) != 0)
{
perror("SetupSerial 1");
printf("tcgetattr( fd,&oldtio) -> %d\n",tcgetattr( fd,&oldtio));
return -1;
}
bzero( &newtio, sizeof( newtio ) );
newtio.c_cflag |= CLOCAL | CREAD;
newtio.c_cflag |= CS8;
newtio.c_cflag &= ~PARENB;
/*设置波特率*/
switch( baud )
{
case 2400:
cfsetispeed(&newtio, B2400);
cfsetospeed(&newtio, B2400);
break;
case 4800:
cfsetispeed(&newtio, B4800);
cfsetospeed(&newtio, B4800);
break;
case 9600:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
case 115200:
cfsetispeed(&newtio, B115200);
cfsetospeed(&newtio, B115200);
break;
case 230400:
cfsetispeed(&newtio, B230400);
cfsetospeed(&newtio, B230400);
break;
case 460800:
cfsetispeed(&newtio, B460800);
cfsetospeed(&newtio, B460800);
break;
default:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
}
newtio.c_cflag &= ~CSTOPB;
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 0;
tcflush(fd,TCIFLUSH);
if((tcsetattr(fd,TCSANOW,&newtio))!=0)
{
perror("com set error");
return -1;
}
return fd;
}
void serial_close(int fd)
{
close(fd);
}
int serial_read_data(int fd, unsigned char *val, int len)
{
len=read(fd,val,len);
return len;
}
int serial_write_data(int fd, unsigned char *val, int len)
{
len=write(fd,val,len*sizeof(unsigned char));
return len;
}

502
Devices/Packages/wit_wt901ble_reader/src/wit_c_sdk.c Executable file
View File

@@ -0,0 +1,502 @@
#include "wit_wt901ble_reader/wit_c_sdk.h"
static SerialWrite p_WitSerialWriteFunc = NULL;
static WitI2cWrite p_WitI2cWriteFunc = NULL;
static WitI2cRead p_WitI2cReadFunc = NULL;
static CanWrite p_WitCanWriteFunc = NULL;
static RegUpdateCb p_WitRegUpdateCbFunc = NULL;
static DelaymsCb p_WitDelaymsFunc = NULL;
static uint8_t s_ucAddr = 0xff;
static uint8_t s_ucWitDataBuff[WIT_DATA_BUFF_SIZE];
static uint32_t s_uiWitDataCnt = 0, s_uiProtoclo = 0, s_uiReadRegIndex = 0;
int16_t sReg[REGSIZE];
#define FuncW 0x06
#define FuncR 0x03
static const uint8_t __auchCRCHi[256] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40
};
static const uint8_t __auchCRCLo[256] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
0x40
};
static uint16_t __CRC16(uint8_t *puchMsg, uint16_t usDataLen)
{
uint8_t uchCRCHi = 0xFF;
uint8_t uchCRCLo = 0xFF;
uint8_t uIndex;
int i = 0;
uchCRCHi = 0xFF;
uchCRCLo = 0xFF;
for (; i<usDataLen; i++)
{
uIndex = uchCRCHi ^ puchMsg[i];
uchCRCHi = uchCRCLo ^ __auchCRCHi[uIndex];
uchCRCLo = __auchCRCLo[uIndex] ;
}
return (uint16_t)(((uint16_t)uchCRCHi << 8) | (uint16_t)uchCRCLo) ;
}
static uint8_t __CaliSum(uint8_t *data, uint32_t len)
{
uint32_t i;
uint8_t ucCheck = 0;
for(i=0; i<len; i++) ucCheck += *(data + i);
return ucCheck;
}
int32_t WitSerialWriteRegister(SerialWrite Write_func)
{
if(!Write_func)return WIT_HAL_INVAL;
p_WitSerialWriteFunc = Write_func;
return WIT_HAL_OK;
}
static void CopeWitData(uint8_t ucIndex, uint16_t *p_data, uint32_t uiLen)
{
uint32_t uiReg1 = 0, uiReg2 = 0, uiReg1Len = 0, uiReg2Len = 0;
uint16_t *p_usReg1Val = p_data;
uint16_t *p_usReg2Val = p_data+3;
uiReg1Len = 4;
switch(ucIndex)
{
case WIT_ACC: uiReg1 = AX; uiReg1Len = 3; uiReg2 = TEMP; uiReg2Len = 1; break;
case WIT_ANGLE: uiReg1 = Roll; uiReg1Len = 3; uiReg2 = VERSION; uiReg2Len = 1; break;
case WIT_TIME: uiReg1 = YYMM; break;
case WIT_GYRO: uiReg1 = GX; uiLen = 3;break;
case WIT_MAGNETIC: uiReg1 = HX; uiLen = 3;break;
case WIT_DPORT: uiReg1 = D0Status; break;
case WIT_PRESS: uiReg1 = PressureL; break;
case WIT_GPS: uiReg1 = LonL; break;
case WIT_VELOCITY: uiReg1 = GPSHeight; break;
case WIT_QUATER: uiReg1 = q0; break;
case WIT_GSA: uiReg1 = SVNUM; break;
case WIT_REGVALUE: uiReg1 = s_uiReadRegIndex; break;
default:
return ;
}
if(uiLen == 3)
{
uiReg1Len = 3;
uiReg2Len = 0;
}
if(uiReg1Len)
{
memcpy(&sReg[uiReg1], p_usReg1Val, uiReg1Len<<1);
p_WitRegUpdateCbFunc(uiReg1, uiReg1Len);
}
if(uiReg2Len)
{
memcpy(&sReg[uiReg2], p_usReg2Val, uiReg2Len<<1);
p_WitRegUpdateCbFunc(uiReg2, uiReg2Len);
}
}
void WitSerialDataIn(uint8_t ucData)
{
uint16_t usCRC16, usTemp, i, usData[4];
uint8_t ucSum;
if(p_WitRegUpdateCbFunc == NULL)return ;
s_ucWitDataBuff[s_uiWitDataCnt++] = ucData;
switch(s_uiProtoclo)
{
case WIT_PROTOCOL_NORMAL:
if(s_ucWitDataBuff[0] != 0x55)
{
s_uiWitDataCnt--;
memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
return ;
}
if(s_uiWitDataCnt >= 11)
{
ucSum = __CaliSum(s_ucWitDataBuff, 10);
if(ucSum != s_ucWitDataBuff[10])
{
s_uiWitDataCnt--;
memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
return ;
}
usData[0] = ((uint16_t)s_ucWitDataBuff[3] << 8) | (uint16_t)s_ucWitDataBuff[2];
usData[1] = ((uint16_t)s_ucWitDataBuff[5] << 8) | (uint16_t)s_ucWitDataBuff[4];
usData[2] = ((uint16_t)s_ucWitDataBuff[7] << 8) | (uint16_t)s_ucWitDataBuff[6];
usData[3] = ((uint16_t)s_ucWitDataBuff[9] << 8) | (uint16_t)s_ucWitDataBuff[8];
CopeWitData(s_ucWitDataBuff[1], usData, 4);
s_uiWitDataCnt = 0;
}
break;
case WIT_PROTOCOL_MODBUS:
if(s_uiWitDataCnt > 2)
{
if(s_ucWitDataBuff[1] != FuncR)
{
s_uiWitDataCnt--;
memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
return ;
}
if(s_uiWitDataCnt < (s_ucWitDataBuff[2] + 5))return ;
usTemp = ((uint16_t)s_ucWitDataBuff[s_uiWitDataCnt-2] << 8) | s_ucWitDataBuff[s_uiWitDataCnt-1];
usCRC16 = __CRC16(s_ucWitDataBuff, s_uiWitDataCnt-2);
if(usTemp != usCRC16)
{
s_uiWitDataCnt--;
memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
return ;
}
usTemp = s_ucWitDataBuff[2] >> 1;
for(i = 0; i < usTemp; i++)
{
sReg[i+s_uiReadRegIndex] = ((uint16_t)s_ucWitDataBuff[(i<<1)+3] << 8) | s_ucWitDataBuff[(i<<1)+4];
}
p_WitRegUpdateCbFunc(s_uiReadRegIndex, usTemp);
s_uiWitDataCnt = 0;
}
break;
case WIT_PROTOCOL_CAN:
case WIT_PROTOCOL_I2C:
s_uiWitDataCnt = 0;
break;
}
if(s_uiWitDataCnt == WIT_DATA_BUFF_SIZE)s_uiWitDataCnt = 0;
}
int32_t WitI2cFuncRegister(WitI2cWrite write_func, WitI2cRead read_func)
{
if(!write_func)return WIT_HAL_INVAL;
if(!read_func)return WIT_HAL_INVAL;
p_WitI2cWriteFunc = write_func;
p_WitI2cReadFunc = read_func;
return WIT_HAL_OK;
}
int32_t WitCanWriteRegister(CanWrite Write_func)
{
if(!Write_func)return WIT_HAL_INVAL;
p_WitCanWriteFunc = Write_func;
return WIT_HAL_OK;
}
void WitCanDataIn(uint8_t ucData[8], uint8_t ucLen)
{
uint16_t usData[3];
if(p_WitRegUpdateCbFunc == NULL)return ;
if(ucLen < 8)return ;
switch(s_uiProtoclo)
{
case WIT_PROTOCOL_CAN:
if(ucData[0] != 0x55)return ;
usData[0] = ((uint16_t)ucData[3] << 8) | ucData[2];
usData[1] = ((uint16_t)ucData[5] << 8) | ucData[4];
usData[2] = ((uint16_t)ucData[7] << 8) | ucData[6];
CopeWitData(ucData[1], usData, 3);
break;
case WIT_PROTOCOL_NORMAL:
case WIT_PROTOCOL_MODBUS:
case WIT_PROTOCOL_I2C:
break;
}
}
int32_t WitRegisterCallBack(RegUpdateCb update_func)
{
if(!update_func)return WIT_HAL_INVAL;
p_WitRegUpdateCbFunc = update_func;
return WIT_HAL_OK;
}
int32_t WitWriteReg(uint32_t uiReg, uint16_t usData)
{
uint16_t usCRC;
uint8_t ucBuff[8];
if(uiReg >= REGSIZE)return WIT_HAL_INVAL;
switch(s_uiProtoclo)
{
case WIT_PROTOCOL_NORMAL:
if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
ucBuff[0] = 0xFF;
ucBuff[1] = 0xAA;
ucBuff[2] = uiReg & 0xFF;
ucBuff[3] = usData & 0xff;
ucBuff[4] = usData >> 8;
p_WitSerialWriteFunc(ucBuff, 5);
break;
case WIT_PROTOCOL_MODBUS:
if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
ucBuff[0] = s_ucAddr;
ucBuff[1] = FuncW;
ucBuff[2] = uiReg >> 8;
ucBuff[3] = uiReg & 0xFF;
ucBuff[4] = usData >> 8;
ucBuff[5] = usData & 0xff;
usCRC = __CRC16(ucBuff, 6);
ucBuff[6] = usCRC >> 8;
ucBuff[7] = usCRC & 0xff;
p_WitSerialWriteFunc(ucBuff, 8);
break;
case WIT_PROTOCOL_CAN:
if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY;
ucBuff[0] = 0xFF;
ucBuff[1] = 0xAA;
ucBuff[2] = uiReg & 0xFF;
ucBuff[3] = usData & 0xff;
ucBuff[4] = usData >> 8;
p_WitCanWriteFunc(s_ucAddr, ucBuff, 5);
break;
case WIT_PROTOCOL_I2C:
if(p_WitI2cWriteFunc == NULL)return WIT_HAL_EMPTY;
ucBuff[0] = usData & 0xff;
ucBuff[1] = usData >> 8;
if(p_WitI2cWriteFunc(s_ucAddr << 1, uiReg, ucBuff, 2) != 1)
{
//printf("i2c write fail\r\n");
}
break;
default:
return WIT_HAL_INVAL;
}
return WIT_HAL_OK;
}
int32_t WitReadReg(uint32_t uiReg, uint32_t uiReadNum)
{
uint16_t usTemp, i;
uint8_t ucBuff[8];
if((uiReg + uiReadNum) >= REGSIZE)return WIT_HAL_INVAL;
switch(s_uiProtoclo)
{
case WIT_PROTOCOL_NORMAL:
if(uiReadNum > 4)return WIT_HAL_INVAL;
if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
ucBuff[0] = 0xFF;
ucBuff[1] = 0xAA;
ucBuff[2] = 0x27;
ucBuff[3] = uiReg & 0xff;
ucBuff[4] = uiReg >> 8;
p_WitSerialWriteFunc(ucBuff, 5);
break;
case WIT_PROTOCOL_MODBUS:
if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
usTemp = uiReadNum << 1;
if((usTemp + 5) > WIT_DATA_BUFF_SIZE)return WIT_HAL_NOMEM;
ucBuff[0] = s_ucAddr;
ucBuff[1] = FuncR;
ucBuff[2] = uiReg >> 8;
ucBuff[3] = uiReg & 0xFF;
ucBuff[4] = uiReadNum >> 8;
ucBuff[5] = uiReadNum & 0xff;
usTemp = __CRC16(ucBuff, 6);
ucBuff[6] = usTemp >> 8;
ucBuff[7] = usTemp & 0xff;
p_WitSerialWriteFunc(ucBuff, 8);
break;
case WIT_PROTOCOL_CAN:
if(uiReadNum > 3)return WIT_HAL_INVAL;
if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY;
ucBuff[0] = 0xFF;
ucBuff[1] = 0xAA;
ucBuff[2] = 0x27;
ucBuff[3] = uiReg & 0xff;
ucBuff[4] = uiReg >> 8;
p_WitCanWriteFunc(s_ucAddr, ucBuff, 5);
break;
case WIT_PROTOCOL_I2C:
if(p_WitI2cReadFunc == NULL)return WIT_HAL_EMPTY;
usTemp = uiReadNum << 1;
if(WIT_DATA_BUFF_SIZE < usTemp)return WIT_HAL_NOMEM;
if(p_WitI2cReadFunc(s_ucAddr << 1, uiReg, s_ucWitDataBuff, usTemp) == 1)
{
if(p_WitRegUpdateCbFunc == NULL)return WIT_HAL_EMPTY;
for(i = 0; i < uiReadNum; i++)
{
sReg[i+uiReg] = ((uint16_t)s_ucWitDataBuff[(i<<1)+1] << 8) | s_ucWitDataBuff[i<<1];
}
p_WitRegUpdateCbFunc(uiReg, uiReadNum);
}
break;
default:
return WIT_HAL_INVAL;
}
s_uiReadRegIndex = uiReg;
return WIT_HAL_OK;
}
int32_t WitInit(uint32_t uiProtocol, uint8_t ucAddr)
{
if(uiProtocol > WIT_PROTOCOL_I2C)return WIT_HAL_INVAL;
s_uiProtoclo = uiProtocol;
s_ucAddr = ucAddr;
s_uiWitDataCnt = 0;
return WIT_HAL_OK;
}
void WitDeInit(void)
{
p_WitSerialWriteFunc = NULL;
p_WitI2cWriteFunc = NULL;
p_WitI2cReadFunc = NULL;
p_WitCanWriteFunc = NULL;
p_WitRegUpdateCbFunc = NULL;
s_ucAddr = 0xff;
s_uiWitDataCnt = 0;
s_uiProtoclo = 0;
}
int32_t WitDelayMsRegister(DelaymsCb delayms_func)
{
if(!delayms_func)return WIT_HAL_INVAL;
p_WitDelaymsFunc = delayms_func;
return WIT_HAL_OK;
}
char CheckRange(short sTemp,short sMin,short sMax)
{
if ((sTemp>=sMin)&&(sTemp<=sMax)) return 1;
else return 0;
}
/*Acceleration calibration demo*/
int32_t WitStartAccCali(void)
{
/*
First place the equipment horizontally, and then perform the following operations
*/
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;// unlock reg
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(CALSW, CALGYROACC) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
int32_t WitStopAccCali(void)
{
if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(SAVE, SAVE_PARAM) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
/*Magnetic field calibration*/
int32_t WitStartMagCali(void)
{
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(CALSW, CALMAGMM) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
int32_t WitStopMagCali(void)
{
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
/*change Band*/
int32_t WitSetUartBaud(int32_t uiBaudIndex)
{
if(!CheckRange(uiBaudIndex,WIT_BAUD_4800,WIT_BAUD_230400))
{
return WIT_HAL_INVAL;
}
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
/*change Can Band*/
int32_t WitSetCanBaud(int32_t uiBaudIndex)
{
if(!CheckRange(uiBaudIndex,CAN_BAUD_1000000,CAN_BAUD_3000))
{
return WIT_HAL_INVAL;
}
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
/*change Bandwidth*/
int32_t WitSetBandwidth(int32_t uiBaudWidth)
{
if(!CheckRange(uiBaudWidth,BANDWIDTH_256HZ,BANDWIDTH_5HZ))
{
return WIT_HAL_INVAL;
}
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(BANDWIDTH, uiBaudWidth) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
/*change output rate */
int32_t WitSetOutputRate(int32_t uiRate)
{
if(!CheckRange(uiRate,RRATE_02HZ,RRATE_NONE))
{
return WIT_HAL_INVAL;
}
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(RRATE, uiRate) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}
/*change WitSetContent */
int32_t WitSetContent(int32_t uiRsw)
{
if(!CheckRange(uiRsw,RSW_TIME,RSW_MASK))
{
return WIT_HAL_INVAL;
}
if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;
if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20);
else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
else ;
if(WitWriteReg(RSW, uiRsw) != WIT_HAL_OK) return WIT_HAL_ERROR;
return WIT_HAL_OK;
}

387
Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader.cc Executable file
View File

@@ -0,0 +1,387 @@
#include "wit_wt901ble_reader/wit_wt901ble_reader.h"
namespace wt901ble_reader
{
Diagnostics::Diagnostics(string& nodename):nodename_(nodename), nh_("~")
{
imu_diagnostics_topic_name_ = "";
if(!ros::param::get(nodename_ + "/imu_diagnostics_topic_name", imu_diagnostics_topic_name_))
{
imu_diagnostics_topic_name_ = "/imu_diagnostics";
ROS_WARN("Node: %s cannot get params", nodename_.c_str());
}
diagnostics_pub = nh_.advertise<diagnostic_msgs::DiagnosticArray>(imu_diagnostics_topic_name_, 10);
diagnostics_thread = new boost::thread(&Diagnostics::run_diagnostics, this);
}
Diagnostics::~Diagnostics()
{
diagnostics_thread->join();
delete(diagnostics_thread);
}
void Diagnostics::run_diagnostics()
{
ros::Duration(0.5).sleep();
connection.name = "connection";
connection.hardware_id = "IMU_sensors";
connection.level = connection.STALE;
connection.message = "Initialized";
pubDiagnostics();
ros::Rate r(5);
boost::lock_guard<boost::mutex> lock(mutex);
while(ros::ok())
{
ros::spinOnce();
if(!is_imu_connected)
{
connection.level = connection.ERROR;
connection.message = "Cannot connect to device";
}
else{
if(is_imu_data_available)
{
connection.level = connection.OK;
connection.message = "Connected to device, datas is received";
}
else
{
connection.level = connection.WARN;
connection.message = "Connected to device but datas is not received";
}
}
pubDiagnostics();
r.sleep();
}
}
void Diagnostics::pubDiagnostics() {
diagnostic_msgs::DiagnosticArray diagnostic;
diagnostic.status.push_back(connection);
if(diagnostic.status != prev_diagnostic.status)
{
diagnostic.header.seq = prev_diagnostic.header.seq+1;
diagnostic.header.stamp = ros::Time::now();
diagnostic.header.frame_id = "frame_id";
diagnostics_pub.publish(diagnostic);
prev_diagnostic = diagnostic;
}
}
wt901ble_node::wt901ble_node(const string& nodename):nodename_(nodename), nh_("~")
{
userParams = new UserParams();
wt901ble_data_ = new wt901ble_data();
if(LoadParams(userParams, nodename_)){
ROS_INFO("%s : Params is loaded", nodename_.c_str());
}
else ROS_ERROR("%s : Fail to load params", nodename_.c_str());
isDataAvailable = false;
wt901ble_data_pub_ = nh_.advertise<sensor_msgs::Imu>(userParams->topic_name,10,false);
isConnected_ = false;
is_imu_connected = false;
is_imu_data_available = false;
is_try_to_connect = false;
is_reconnected = false;
lost_data_count = 0;
reconnect_timer = nh_.createWallTimer(::ros::WallDuration(0.5), &wt901ble_node::ReconnectSerial, this);
get_and_publish_timer = nh_.createWallTimer(::ros::WallDuration(0.02), &wt901ble_node::GetAndPublishData, this);
reconnect_timer.stop();
if(detect_port(userParams->portname))
{
ROS_INFO("Port: %s is found",userParams->portname.c_str());
try {
serial_ = new serial::Serial(userParams->portname, userParams->baudrate, serial::Timeout::simpleTimeout(500));
}
catch(const std::exception& ex)
{
isConnected_ = false;
is_imu_connected = false;
ROS_WARN("Can not open serial port: %s", userParams->portname.c_str());
cerr << "Unhandled Exception: " << ex.what() << endl;
}
if(serial_!=NULL)
{
if(serial_-> isOpen()){
ROS_INFO("%s: Connected to serial port: %s, baudrate: %d", nodename_.c_str(), userParams->portname.c_str(), userParams->baudrate);
isConnected_ = true;
is_imu_connected = true;
}
}
get_and_publish_timer.start();
}
}
wt901ble_node::~wt901ble_node()
{
delete(serial_);
delete(userParams);
delete(wt901ble_data_);
}
void wt901ble_node::GetAndPublishData(const ::ros::WallTimerEvent& timer_event)
{
if(isConnected_)
{
if(recvData(data_buf))
{
// ROS_INFO_ONCE("%s: Start to get datas and publish", nodename_.c_str());
// size_t data_buf_size = sizeof(data_buf)/sizeof(data_buf[0]);
// int len = static_cast<int>(data_buf_size);
// ROS_INFO("len: %d, data_buf: %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x",
// len, data_buf[0], data_buf[1], data_buf[2], data_buf[3], data_buf[4], data_buf[5],
// data_buf[6], data_buf[7], data_buf[8], data_buf[9], data_buf[10], data_buf[11],
// data_buf[12], data_buf[13], data_buf[14], data_buf[15], data_buf[16], data_buf[17], data_buf[18], data_buf[19]);
int16_t aX = (data_buf[3]<<8U)|data_buf[2];
int16_t aY = (data_buf[5]<<8U)|data_buf[4];
int16_t aZ = (data_buf[7]<<8U)|data_buf[6];
int16_t wX = (data_buf[9]<<8U)|data_buf[8];
int16_t wY = (data_buf[11]<<8U)|data_buf[10];
int16_t wZ = (data_buf[13]<<8U)|data_buf[12];
int16_t Roll = (data_buf[15]<<8U)|data_buf[14];
int16_t Pitch = (data_buf[17]<<8U)|data_buf[16];
int16_t Yaw = (data_buf[19]<<8U)|data_buf[18];
wt901ble_data_->aX = ((double)(aX))/32768*156.9064;
wt901ble_data_->aY = ((double)(aY))/32768*156.9064;
wt901ble_data_->aZ = ((double)(aZ))/32768*156.9064;
wt901ble_data_->wX = ((double)(wX))/32768*2000;
wt901ble_data_->wY = ((double)(wY))/32768*2000;
wt901ble_data_->wZ = ((double)(wZ))/32768*2000;
wt901ble_data_->Roll = ((double)(Roll))/32768*180;
wt901ble_data_->Pitch = ((double)(Pitch))/32768*180;
wt901ble_data_->Yaw = ((double)(Yaw))/32768*180;
wt901ble_data_->wX = (wt901ble_data_->wX*M_PI)/180;
wt901ble_data_->wY = (wt901ble_data_->wY*M_PI)/180;
wt901ble_data_->wZ = (wt901ble_data_->wZ*M_PI)/180;
// ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f",
// wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw);
wt901ble_data_->Roll = (wt901ble_data_->Roll*M_PI)/180;
wt901ble_data_->Pitch = (wt901ble_data_->Pitch*M_PI)/180;
wt901ble_data_->Yaw = (wt901ble_data_->Yaw*M_PI)/180;
imu_data.header.frame_id = userParams->frame_id;
imu_data.header.stamp = ros::Time::now();
imu_data.orientation = getQuaternion(wt901ble_data_->Yaw, wt901ble_data_->Pitch, wt901ble_data_->Roll);
// imu_data.orientation_covariance[0] = 2.8900000000000004e-08;
// imu_data.orientation_covariance[4] = 2.8900000000000004e-08;
// imu_data.orientation_covariance[8] = 2.8900000000000004e-08;
imu_data.orientation_covariance[0] = 0.1;
imu_data.orientation_covariance[4] = 0.1;
imu_data.orientation_covariance[8] = 0.1;
imu_data.angular_velocity.x = wt901ble_data_->wX;
imu_data.angular_velocity.y = wt901ble_data_->wY;
imu_data.angular_velocity.z = wt901ble_data_->wZ;
// imu_data.angular_velocity_covariance[0] = 2.8900000000000004e-08;
// imu_data.angular_velocity_covariance[4] = 2.8900000000000004e-08;
// imu_data.angular_velocity_covariance[8] = 2.8900000000000004e-08;
imu_data.angular_velocity_covariance[0] = 8e-6;
imu_data.angular_velocity_covariance[4] = 8e-6;
imu_data.angular_velocity_covariance[8] = 3e-7;
imu_data.linear_acceleration.x = wt901ble_data_->aX;
imu_data.linear_acceleration.y = wt901ble_data_->aY;
imu_data.linear_acceleration.z = wt901ble_data_->aZ;
// imu_data.linear_acceleration_covariance[0] = 0.009604000000000001;
// imu_data.linear_acceleration_covariance[4] = 0.009604000000000001;
// imu_data.linear_acceleration_covariance[8] = 0.009604000000000001;
imu_data.linear_acceleration_covariance[0] = 5e-5;
imu_data.linear_acceleration_covariance[4] = 0.0001;
imu_data.linear_acceleration_covariance[8] = 0.00013;
wt901ble_data_pub_.publish(imu_data);
// ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f",
// wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw);
// ROS_INFO("aX: %f, aY: %f, aZ: %f, wX: %f, wY: %f, wZ: %f, Roll: %f, Pitch: %f, Yaw: %f",
// wt901ble_data_->aX, wt901ble_data_->aY, wt901ble_data_->aZ,
// wt901ble_data_->wX, wt901ble_data_->wY, wt901ble_data_->wZ,
// wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw);
is_imu_data_available = true;
lost_data_count = 0;
is_try_to_connect = false;
}
else
{
lost_data_count ++;
if(lost_data_count>30)
{
is_imu_data_available = false;
if(is_try_to_connect ==false) { is_try_to_connect=true; reconnect_timer.start();}
// ROS_WARN("IMU reader node: lost_data");
lost_data_count = 0;
}
}
}
}
void wt901ble_node::ReconnectSerial(const ::ros::WallTimerEvent& timer_event)
{
if(is_try_to_connect)
{
if(!is_reconnected)
{
if(serial_!=NULL)
{
isConnected_ = false;
is_imu_connected = false;
// boost::this_thread::sleep(boost::posix_time::milliseconds(500));
ros::Duration(0.5).sleep();
serial_->flush();
serial_->close();
delete(serial_);
serial_ = new serial::Serial(userParams->portname, userParams->baudrate, serial::Timeout::simpleTimeout(500));
}
if(serial_!=NULL)
{
if(serial_-> isOpen()){
ROS_WARN("%s: Reconnected to serial port (IMU sensor): %s, baudrate: %d", nodename_.c_str(), userParams->portname.c_str(), userParams->baudrate);
isConnected_ = true;
is_imu_connected = true;
}
}
is_reconnected = true;
reconnect_timer.stop();
}
}
}
geometry_msgs::Quaternion wt901ble_node::getQuaternion(double yaw, double pitch, double roll)
{
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
geometry_msgs::Quaternion q;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;
q.w = cr * cp * cy + sr * sp * sy;
return q;
}
bool wt901ble_node::LoadParams(UserParams* userParams_, const string& node_name){
bool result = true;
try{
int baudrate;
if(!ros::param::get(node_name + "/baudrate", baudrate)) result = false;
if(!ros::param::get(node_name + "/portname", userParams_->portname)) result = false;
if(!ros::param::get(node_name + "/topic_name", userParams_->topic_name)) result = false;
if(!ros::param::get(node_name + "/frame_id", userParams_->frame_id)) result = false;
userParams_->baudrate = static_cast<uint32_t>(baudrate);
}
catch(const std::exception& ex)
{
std::cerr << "Fail to load user params caught: " << ex.what() << std::endl;
result = false;
}
return result;
}
size_t wt901ble_node::readN(uint8_t *buf, size_t len){
size_t offset = 0;
offset = serial_->read(buf,len);
return offset;
}
bool wt901ble_node::recvData(uint8_t *buf){
bool ret = false;
uint8_t ch[2];
if(readN(ch, 2) == 2)
{
if(ch[0]==wt901ble_header_imu[0]&&
ch[1]==wt901ble_header_imu[1])
{
buf[0] = ch[0];
buf[1] = ch[1];
if(readN(&buf[2],18)==18)
{
ret = true;
}
}
}
// if (readN(buf, wt901ble_data_length) != 20) {
// // continue;
// return false;
// }
// if (buf[0] == wt901ble_header_imu[0]&&
// buf[1] == wt901ble_header_imu[1])
// {
// ret = true;
// }
return ret;
}
bool wt901ble_node::receiveData()
{
bool ret = false;
isDataAvailable = false;
uint8_t ch;
while (!ret) {
if(readN(&ch, 1) != 1){
continue;
}
if(ch == wt901ble_header_imu[0]){
data_buf[0] = ch;
if(readN(&ch, 1) == 1){
if(ch == wt901ble_header_imu[2])
{
data_buf[1] = ch;
if(readN(&data_buf[2], 18) == 18)
{
ret = true;
isDataAvailable = true;
}
}
}
}
}
return ret;
}
bool wt901ble_node::detect_port(string& port_name)
{
bool result = false;
// vector<serial::PortInfo> devices_found = serial::list_ports();
// vector<serial::PortInfo>::iterator iter = devices_found.begin();
// bool result = false;
// while( iter != devices_found.end() )
// {
// serial::PortInfo device = *iter++;
// // ROS_INFO( "(%s, %s, %s)\n", device.port.c_str(), device.description.c_str(),
// // device.hardware_id.c_str() );
// if(device.port == port_name)
// {
// // ROS_INFO("Found port: %s",device.port.c_str());
// result = true;
// break;
// }
// }
// return result;
string product = "USB Serial";
char _port[50] = "";
udevadmInfo *ludev = new udevadmInfo(product.c_str());
if(ludev->init() == EXIT_FAILURE)
return false;
if(!ludev->scanDevices()) { return false; }
else {
strcpy(_port, ludev->port);
}
ROS_INFO("Found port: %s",_port);
if(strstr(_port,"/dev/ttyUSB") != NULL) {
// std::string cmd_ = "sudo chmod 777 " + (std::string)_port;
// system(cmd_.c_str());
userParams->portname = (std::string)_port;
result = true;
}
return result;
}
}

19
Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader_main.cc Executable file
View File

@@ -0,0 +1,19 @@
#include "wit_wt901ble_reader/wit_wt901ble_reader.h"
using namespace std;
using namespace wt901ble_reader;
wt901ble_node* node;
Diagnostics* diagnostics_node;
int main(int argc, char** argv)
{
ros::init(argc, argv, "wit_wt901ble_reader_node");
ros::start();
string nodename = ros::this_node::getName();
node = new wt901ble_node(nodename);
diagnostics_node = new Diagnostics(nodename);
ros::spin();
delete(node);
delete(diagnostics_node);
return 0;
}