├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── anchor.yaml ├── anchor_new.yaml ├── anchor_old.yaml ├── relative.yaml ├── uwb0_params.yaml ├── uwb1_params.yaml └── uwb_params.yaml ├── include └── srv_return_verbal.h ├── launch ├── uwb_driver.launch ├── uwb_gazebo.launch ├── uwb_play.launch └── uwb_record.launch ├── msg ├── UwbData.msg ├── UwbRange.msg └── Uwbrange.msg ├── package.xml ├── src ├── hostInterfaceCommon.h ├── hostInterfaceRCM.h ├── hostInterfaceRN.h ├── rcm.c ├── rcm.h ├── rcmIf.c ├── rcmIf.h ├── rn.c ├── rn.h ├── rtwtypes.h ├── uwb_driver.cpp └── uwb_driver_gazebo.cpp └── srv ├── uwbModeConfig.srv └── uwbRangeComm.srv /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uwb_driver) 3 | 4 | if(UNIX) 5 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++0x") 6 | endif() 7 | 8 | ## Find catkin macros and libraries 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 10 | ## is used, also find other catkin packages 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | message_generation 16 | serial 17 | geometry_msgs 18 | tf 19 | eigen_conversions 20 | tf_conversions 21 | ) 22 | 23 | 24 | ## System dependencies are found with CMake's conventions 25 | # find_package(Boost REQUIRED COMPONENTS system) 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a run_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | add_message_files( 58 | FILES 59 | UwbRange.msg 60 | UwbData.msg 61 | ) 62 | 63 | 64 | ## Generate services in the 'srv' folder 65 | add_service_files( 66 | FILES 67 | uwbModeConfig.srv 68 | uwbRangeComm.srv 69 | ) 70 | 71 | ## Generate actions in the 'action' folder 72 | # add_action_files( 73 | # FILES 74 | # Action1.action 75 | # Action2.action 76 | # ) 77 | 78 | ## Generate added messages and services with any dependencies listed here 79 | generate_messages( 80 | DEPENDENCIES 81 | std_msgs 82 | geometry_msgs 83 | ) 84 | 85 | ################################################ 86 | ## Declare ROS dynamic reconfigure parameters ## 87 | ################################################ 88 | 89 | ## To declare and build dynamic reconfigure parameters within this 90 | ## package, follow these steps: 91 | ## * In the file package.xml: 92 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 93 | ## * In this file (CMakeLists.txt): 94 | ## * add "dynamic_reconfigure" to 95 | ## find_package(catkin REQUIRED COMPONENTS ...) 96 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 97 | ## and list every .cfg file to be processed 98 | 99 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 100 | # generate_dynamic_reconfigure_options( 101 | # cfg/DynReconf1.cfg 102 | # cfg/DynReconf2.cfg 103 | # ) 104 | 105 | ################################### 106 | ## catkin specific configuration ## 107 | ################################### 108 | ## The catkin_package macro generates cmake config files for your package 109 | ## Declare things to be passed to dependent projects 110 | ## INCLUDE_DIRS: uncomment this if you package contains header files 111 | ## LIBRARIES: libraries you create in this project that dependent projects also need 112 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 113 | ## DEPENDS: system dependencies of this project that dependent projects also need 114 | catkin_package( 115 | # INCLUDE_DIRS include 116 | # LIBRARIES uwb_driver 117 | # CATKIN_DEPENDS roscpp rospy std_msgs 118 | # DEPENDS system_lib 119 | ) 120 | 121 | ########### 122 | ## Build ## 123 | ########### 124 | 125 | ## Specify additional locations of header files 126 | ## Your package locations should be listed before other locations 127 | # include_directories(include) 128 | include_directories( 129 | ${catkin_INCLUDE_DIRS} 130 | src/ 131 | include/ 132 | ) 133 | 134 | ## Declare a C++ library 135 | # add_library(uwb_driver 136 | # src/${PROJECT_NAME}/uwb_driver.cpp 137 | # ) 138 | #additional link directory 139 | link_directories(src/lib/) 140 | 141 | 142 | SET(UWB_SRCS 143 | src/rcm.c 144 | src/rcmIf.c 145 | src/rn.c 146 | ) 147 | 148 | ## Add cmake target dependencies of the library 149 | ## as an example, code may need to be generated before libraries 150 | ## either from message generation or dynamic reconfigure 151 | # add_dependencies(uwb_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Declare a C++ executable 154 | add_executable(uwb_driver_node src/uwb_driver.cpp ${UWB_SRCS}) 155 | 156 | add_executable(uwb_driver_gazebo_node src/uwb_driver_gazebo.cpp) 157 | 158 | 159 | ## Add cmake target dependencies of the executable 160 | ## same as for the library above 161 | add_dependencies(uwb_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 162 | add_dependencies(uwb_driver_gazebo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 163 | 164 | ## Specify libraries to link a library or executable target against 165 | target_link_libraries(uwb_driver_node 166 | ${catkin_LIBRARIES} 167 | ) 168 | 169 | target_link_libraries(uwb_driver_gazebo_node 170 | ${catkin_LIBRARIES} 171 | ) 172 | 173 | #target_link_libraries(uwb_driver_sim_node 174 | # ${catkin_LIBRARIES} 175 | #) 176 | 177 | #target_link_libraries(uwb_driver_ls_sim_node 178 | # ${catkin_LIBRARIES} 179 | # libnlsTRI345.so 180 | #) 181 | 182 | ############# 183 | ## Install ## 184 | ############# 185 | 186 | # all install targets should use catkin DESTINATION variables 187 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 188 | 189 | ## Mark executable scripts (Python etc.) for installation 190 | ## in contrast to setup.py, you can choose the destination 191 | # install(PROGRAMS 192 | # scripts/my_python_script 193 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 194 | # ) 195 | 196 | ## Mark executables and/or libraries for installation 197 | # install(TARGETS uwb_driver uwb_driver_node 198 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 199 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 200 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 201 | # ) 202 | 203 | ## Mark cpp header files for installation 204 | # install(DIRECTORY include/${PROJECT_NAME}/ 205 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 206 | # FILES_MATCHING PATTERN "*.h" 207 | # PATTERN ".svn" EXCLUDE 208 | # ) 209 | 210 | ## Mark other files for installation (e.g. launch and bag files, etc.) 211 | # install(FILES 212 | # # myfile1 213 | # # myfile2 214 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 215 | # ) 216 | 217 | ############# 218 | ## Testing ## 219 | ############# 220 | 221 | ## Add gtest based cpp test target and link libraries 222 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_uwb_driver.cpp) 223 | # if(TARGET ${PROJECT_NAME}-test) 224 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 225 | # endif() 226 | 227 | ## Add folders to be run by python nosetests 228 | # catkin_add_nosetests(test) 229 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, britsknguyen 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of uwb_localization nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # uwb_driver 2 | UWB ROS driver 3 | 4 | To simulate UWB with gazebo, plese clone following repo to your ROS workspace: 5 | 6 | git clone https://github.com/wang-chen/hector_quadrotor.git 7 | 8 | Then launch the gazebo simulation launch file 9 | 10 | roslaunch uwb_driver uwb_gazebo.launch 11 | 12 | You may also interested in the localization repo for multiple sensors: 13 | 14 | https://github.com/wang-chen/localization 15 | 16 | 17 | # Acknowledgement: 18 | 19 | [Thien-Minh Nguyen](https://github.com/BritskNguyen) made great contributions to this repo, thanks! 20 | -------------------------------------------------------------------------------- /cfg/anchor.yaml: -------------------------------------------------------------------------------- 1 | uwb: 2 | 3 | # UWB's Anchor Configuration Parameters 4 | # nodesId: [100, 101, 102, 103, 201] 5 | # nodesPos: [3, -3, 0.75, 3, 3, 1.95, -3, 3, 0.5, -3, -3, 1.7, 0, 0, 1.0] 6 | 7 | nodesId: [100, 101, 102, 103, 200] 8 | # nodesPos: [3, -3, 0.75, 3, 3, 1.95, -3, 3, 0.5, -3, -3, 1.7, 0, 0, 1.0] 9 | # nodesPos: [3, -3, 0.58, 3, 3, 1.97, -3, 3, 0.54, -3, -3, 1.76, 0, 0, 1.0] # 31-38 10 | nodesPos: [3, -3, 0.53, 3, 3, 1.95, -3, 3, 0.54, -3, -3, 1.98, 0, 0, 1.0] 11 | antennaOffset: [-0.2, 0, 0, 0.2, 0, 0, 0, -0.2, 0] 12 | # comment the line of antennaOffset, if there are no antennaoffset -------------------------------------------------------------------------------- /cfg/anchor_new.yaml: -------------------------------------------------------------------------------- 1 | uwb: 2 | 3 | # UWB's Anchor Configuration Parameters 4 | # nodesId: [100, 101, 102, 103, 201] 5 | # nodesPos: [3, -3, 0.75, 3, 3, 1.95, -3, 3, 0.5, -3, -3, 1.7, 0, 0, 1.0] 6 | 7 | nodesId: [0,1,2,3,4,5,8] 8 | # nodesPos: [3, -3, 0.75, 3, 3, 1.95, -3, 3, 0.5, -3, -3, 1.7, 0, 0, 1.0] 9 | # nodesPos: [3, -3, 0.58, 3, 3, 1.97, -3, 3, 0.54, -3, -3, 1.76, 0, 0, 1.0] # 31-38 10 | 11 | nodesPos: [0, 0, 0, 12 | 0, 3.22, 1.83, 13 | 0, 3.26, 0, 14 | 0, 5.9, 0, 15 | 8.61, 3.14, 0, 16 | 8.55, 5.7, 1.83, 17 | 0, 0, 1] 18 | 19 | antennaOffset: [-0.2, 0, 0, 0.2, 0, 0, 0, -0.2, 0] 20 | 21 | # comment the line of antennaOffset, if there are no antennaoffset -------------------------------------------------------------------------------- /cfg/anchor_old.yaml: -------------------------------------------------------------------------------- 1 | uwb: 2 | 3 | # UWB's Anchor Configuration Parameters 4 | nodesId: [100, 101, 102, 103, 204] 5 | nodesPos: [3, 3, 0.65, 3, -3, 0.25, -3, -3, 0.25, -3, 3, 0.25, 0, 0, 1.0] 6 | -------------------------------------------------------------------------------- /cfg/relative.yaml: -------------------------------------------------------------------------------- 1 | uwb: 2 | 3 | # UWB's Anchor Configuration Parameters 4 | # nodesId: [100, 101, 102, 103, 201] 5 | # nodesPos: [3, -3, 0.75, 3, 3, 1.95, -3, 3, 0.5, -3, -3, 1.7, 0, 0, 1.0] 6 | 7 | nodesId: [1, 2, 0] 8 | # nodesPos: [3, -3, 0.75, 3, 3, 1.95, -3, 3, 0.5, -3, -3, 1.7, 0, 0, 1.0] 9 | # nodesPos: [3, -3, 0.58, 3, 3, 1.97, -3, 3, 0.54, -3, -3, 1.76, 0, 0, 1.0] # 31-38 10 | nodesPos: [2.1507,0,0.87,0.125,1.8683,0.87,0,0,0.87] 11 | 12 | # comment the line of antennaOffset, if there are no antennaoffset -------------------------------------------------------------------------------- /cfg/uwb0_params.yaml: -------------------------------------------------------------------------------- 1 | uwb0: 2 | 3 | # UWB's Anchor Configuration Parameters 4 | nodesId: [100, 200, 201] 5 | nodesPos: [0, 0, 1.514, 0, 0, 0, 0, 0, 0] 6 | antennaOffset: [0.1820, 0.2682, 0.0045, 0.1670, -0.2853, 0.0045, -0.3861, -0.2645, -0.0071, -0.3630, 0.2879, -0.0060] 7 | # comment the line of antennaOffset, if there are no antennaoffset 8 | commonId: 200 9 | 10 | # For calibration 11 | albegaA: [1.0, -0.445, 200] 12 | albegaB: [1.0, -0.4564, 200] -------------------------------------------------------------------------------- /cfg/uwb1_params.yaml: -------------------------------------------------------------------------------- 1 | uwb1: 2 | 3 | # UWB's Anchor Configuration Parameters 4 | nodesId: [100, 200, 201] 5 | nodesPos: [0, 0, 1.514, 0, 0, 0, 0, 0, 0] 6 | antennaOffset: [0.1820, 0.2682, 0.0045, 0.1670, -0.2853, 0.0045, -0.3861, -0.2645, -0.0071, -0.3630, 0.2879, -0.0060] 7 | # comment the line of antennaOffset, if there are no antennaoffset 8 | commonId: 200 9 | 10 | # For calibration 11 | albegaA: [1.0, -0.4477, 200] 12 | albegaB: [1.0, -0.449, 200] -------------------------------------------------------------------------------- /cfg/uwb_params.yaml: -------------------------------------------------------------------------------- 1 | uwb: 2 | 3 | # UWB's Anchor Configuration Parameters 4 | nodesId: [100, 200] 5 | nodesPos: [0, 0, 1.514, 0, 0, 0] 6 | antennaOffset: [0.1820, 0.2682, 0.0045, 0.1670, -0.2853, 0.0045, -0.3861, -0.2645, -0.0071, -0.3630, 0.2879, -0.0060] 7 | # comment the line of antennaOffset, if there are no antennaoffset -------------------------------------------------------------------------------- /include/srv_return_verbal.h: -------------------------------------------------------------------------------- 1 | //Verbal definitions for the mode commands service 2 | #ifndef P4XX_CONFIRMED 3 | #define P4XX_CONFIRMED 0 4 | #endif 5 | 6 | #ifndef P4XX_CMD_SENT 7 | #define P4XX_CMD_SENT 1 8 | #endif 9 | 10 | #ifndef P4XX_CONFIRM_TIMEOUT 11 | #define P4XX_CONFIRM_TIMEOUT 2 12 | #endif 13 | 14 | 15 | //Verbal definitions for the range-comm service 16 | #ifndef RANGE_RQST 17 | #define RANGE_RQST 0 18 | #endif 19 | 20 | #ifndef SEND_DATA 21 | #define SEND_DATA 1 22 | #endif 23 | 24 | #ifndef RANGE_RQST_DATA 25 | #define RANGE_RQST_DATA 2 26 | #endif 27 | 28 | #ifndef SET_RQST_DATA 29 | #define SET_RQST_DATA 3 30 | #endif 31 | 32 | #ifndef SET_RSPD_DATA 33 | #define SET_RSPD_DATA 4 34 | #endif 35 | 36 | 37 | -------------------------------------------------------------------------------- /launch/uwb_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /launch/uwb_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/uwb_play.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/uwb_record.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /msg/UwbData.msg: -------------------------------------------------------------------------------- 1 | Header header # ROS header 2 | uint8 source_id # Identity number of the data sending P4xx 3 | uint8 source_idx # Index number of the data sending node passed by rosparam 4 | int8 antenna # Antenna where the measurement was carried out 5 | uint32 uwb_time # ms since radio boot at the time of the range conversation 6 | uint16 data_size # number of data bytes received 7 | uint8[] data # data array, data message will come seperately from the range 8 | -------------------------------------------------------------------------------- /msg/UwbRange.msg: -------------------------------------------------------------------------------- 1 | Header header # ROS header 2 | uint8 requester_id 3 | uint8 requester_idx 4 | uint8 responder_id # Identity number of the responding P4xx 5 | uint8 responder_idx # Index number of the node in the responding array passed by rosparam 6 | uint16 requester_LED_flag # requester’s received scan: 16 for NLS 7 | uint16 responder_LED_flag # responder's received scan: 16 for NLS 8 | uint16 noise # noise 9 | uint16 vPeak # absolute maximum value in the leading edge window of the received waveform 10 | float32 distance # Distance measurement 11 | float32 distance_err # Error estimated by P4xx 12 | float32 distance_dot # Range velocity estimated by Pxx 13 | float32 distance_dot_err # Range velocity error estimated by Pxx 14 | uint8 antenna # Antenna where the measurement was carried out 15 | uint16 stopwatch_time # How long the range conversation took, in ms 16 | uint32 uwb_time # ms since radio boot at the time of the range conversation nb 17 | geometry_msgs/Point responder_location # Location of the responding node if known (explicitly declared as anchor by rosparam), otherwise 9999 indicates unknown. 18 | -------------------------------------------------------------------------------- /msg/Uwbrange.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 requester_id 3 | uint8 responder_id # Identity number of the anchor node 4 | float32 distance # Distance measurement 5 | float32 distance_err # Distance error 6 | uint8 antenna # Antenna where the measurement was carried out, 0 by default 7 | geometry_msgs/Point responder_location # Location of the responding node if known. -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uwb_driver 4 | 0.0.0 5 | The uwb_driver package 6 | 7 | 8 | 9 | 10 | britsk 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | message_runtime 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /src/hostInterfaceCommon.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////// 2 | // 3 | // hostInterface.h 4 | // 5 | // Definitions for the interface between a host computer and the embedded RCM. 6 | // 7 | // Copyright (c) 2010 Time Domain 8 | // 9 | 10 | #ifndef __rcrmHostInterfaceCommon_h 11 | #define __rcrmHostInterfaceCommon_h 12 | 13 | // Portability 14 | 15 | // The section "#ifdef _MSC_VER" was copied from http://msinttypes.googlecode.com/svn/trunk/stdint.h, 16 | // an implementation of stdint.h for Microsoft Visual C/C++ versions earlier than Visual Studio 2010. 17 | #ifdef _MSC_VER 18 | 19 | // Visual Studio 6 and Embedded Visual C++ 4 doesn't 20 | // realize that, e.g. char has the same size as __int8 21 | // so we give up on __intX for them. 22 | #if (_MSC_VER < 1300) 23 | typedef signed char rcrm_int8_t; 24 | typedef signed short rcrm_int16_t; 25 | typedef signed int rcrm_int32_t; 26 | typedef unsigned char rcrm_uint8_t; 27 | typedef unsigned short rcrm_uint16_t; 28 | typedef unsigned int rcrm_uint32_t; 29 | #else 30 | typedef signed __int8 rcrm_int8_t; 31 | typedef signed __int16 rcrm_int16_t; 32 | typedef signed __int32 rcrm_int32_t; 33 | typedef unsigned __int8 rcrm_uint8_t; 34 | typedef unsigned __int16 rcrm_uint16_t; 35 | typedef unsigned __int32 rcrm_uint32_t; 36 | #endif 37 | 38 | typedef signed __int64 rcrm_int64_t; 39 | typedef unsigned __int64 rcrm_uint64_t; 40 | 41 | #else 42 | 43 | typedef __signed char rcrm_int8_t; 44 | typedef unsigned char rcrm_uint8_t; 45 | typedef short rcrm_int16_t; 46 | typedef unsigned short rcrm_uint16_t; 47 | typedef int rcrm_int32_t; 48 | typedef unsigned int rcrm_uint32_t; 49 | typedef long long rcrm_int64_t; 50 | typedef unsigned long long rcrm_uint64_t; 51 | 52 | #endif 53 | 54 | 55 | // Socket defines 56 | #define RCRM_SOCKET_PORT_NUM 21210 57 | 58 | 59 | // Internal modes of operation - not all are supported 60 | #define RCRM_OPMODE_RCM 0 61 | #define RCRM_OPMODE_RN 4 62 | #define RCRM_OPMODE_DEFAULT RCRM_OPMODE_RCM 63 | 64 | 65 | // P400 sleep modes 66 | #define RCRM_SLEEP_MODE_ACTIVE 0 67 | #define RCRM_SLEEP_MODE_IDLE 1 68 | #define RCRM_SLEEP_MODE_STANDBY_ETH 2 // wakeup via ethernet or serial 69 | #define RCRM_SLEEP_MODE_STANDBY_SER 3 // wakeup via serial only 70 | #define RCRM_SLEEP_MODE_SLEEP 4 // wakeup via GPIO only 71 | 72 | /////////////////////////////////////// 73 | // 74 | // Message types 75 | // 76 | 77 | // REQUEST messages are sent by the host to the embedded applicaion. 78 | // CONFIRM messages are sent by the embedded application to the host in response to REQUEST messages. 79 | // INFO messages are sent automatically by the embedded application to the host when various events occur. 80 | #define RCRM_MSG_TYPE_REQUEST (0xF000) 81 | #define RCRM_MSG_TYPE_CONFIRM (0xF100) 82 | #define RCRM_MSG_TYPE_INFO (0xF200) 83 | 84 | 85 | /////////////////////////////////////// 86 | // 87 | // Host <-> Embedded conversation messages 88 | // 89 | 90 | // Get version and temperature info 91 | #define RCRM_GET_STATUS_INFO_REQUEST (RCRM_MSG_TYPE_REQUEST + 1) 92 | #define RCRM_GET_STATUS_INFO_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 1) 93 | 94 | 95 | // Reboot P400 96 | #define RCRM_REBOOT_REQUEST (RCRM_MSG_TYPE_REQUEST + 2) 97 | #define RCRM_REBOOT_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 2) 98 | 99 | // Set opmode 100 | #define RCRM_SET_OPMODE_REQUEST (RCRM_MSG_TYPE_REQUEST + 3) 101 | #define RCRM_SET_OPMODE_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 3) 102 | 103 | // Get opmode 104 | #define RCRM_GET_OPMODE_REQUEST (RCRM_MSG_TYPE_REQUEST + 4) 105 | #define RCRM_GET_OPMODE_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 4) 106 | 107 | // Set sleep mode 108 | #define RCRM_SET_SLEEP_MODE_REQUEST (RCRM_MSG_TYPE_REQUEST + 5) 109 | #define RCRM_SET_SLEEP_MODE_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 5) 110 | 111 | // Get sleep mode 112 | #define RCRM_GET_SLEEP_MODE_REQUEST (RCRM_MSG_TYPE_REQUEST + 6) 113 | #define RCRM_GET_SLEEP_MODE_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 6) 114 | 115 | // Get ambient samples 116 | #define RCRM_GET_AMBIENT_SAMPLES_REQUEST (RCRM_MSG_TYPE_REQUEST + 7) 117 | #define RCRM_GET_AMBIENT_SAMPLES_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 7) 118 | 119 | // Execute Built-In Test 120 | #define RCRM_BIT_REQUEST (RCRM_MSG_TYPE_REQUEST + 8) 121 | #define RCRM_BIT_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 8) 122 | 123 | // Get cause of last boot 124 | #define RCRM_GET_LAST_BOOT_CAUSE_REQUEST (RCRM_MSG_TYPE_REQUEST + 9) 125 | #define RCRM_GET_LAST_BOOT_CAUSE_CONFIRM (RCRM_MSG_TYPE_CONFIRM + 9) 126 | 127 | /////////////////////////////////////// 128 | // 129 | // Common INFO messages to the host 130 | // 131 | 132 | // Sent when a waveform scan is completed. 133 | // If the complete scan doesn't fit in a single packet, multiple packets are sent which can be combined to form the complete scan. 134 | #define RCRM_FULL_SCAN_INFO (RCRM_MSG_TYPE_INFO + 1) 135 | 136 | // Sent when the radio is ready to receive commands. 137 | // Sent after a power up, reboot, or wakeup from sleep event is complete. 138 | #define RCRM_READY_INFO (RCRM_MSG_TYPE_INFO + 2) 139 | 140 | 141 | 142 | /////////////////////////////////////// 143 | // 144 | // Constants and flags 145 | // 146 | 147 | 148 | // *_CONFIRM message status codes 149 | #define RCRM_CONFIRM_MSG_STATUS_SUCCESS 0 150 | #define RCRM_CONFIRM_MSG_STATUS_GENERICFAILURE 1 151 | #define RCRM_CONFIRM_MSG_STATUS_WRONGOPMODE 2 152 | #define RCRM_CONFIRM_MSG_STATUS_UNSUPPORTEDVALUE 3 153 | #define RCRM_CONFIRM_MSG_STATUS_INVALIDDURINGSLEEP 4 154 | 155 | // Definitions of the antennaMode field in various messages 156 | 157 | // Send RCM_ANTENNAMODE_DEFAULT in a message requiring antennaMode to use the default configured antenna 158 | #define RCRM_ANTENNAMODE_DEFAULT 0xff 159 | 160 | #define RCRM_ANTENNAMODE_TXA_RXA 0 161 | #define RCRM_ANTENNAMODE_TXB_RXB 1 162 | #define RCRM_ANTENNAMODE_TXA_RXB 2 163 | #define RCRM_ANTENNAMODE_TXB_RXA 3 164 | 165 | #define RCRM_ANTENNAMODE_TOGGLE_FLAG (0x80) 166 | 167 | 168 | #define RCRM_MAX_SCAN_SAMPLES (350) 169 | 170 | // TX gain levels (inverses of the actual attenuation levels) 171 | #define RCRM_TXGAIN_MIN 0 172 | #define RCRM_TXGAIN_MAX 63 173 | 174 | 175 | // Scan filtering 176 | #define RCRM_SCAN_FILTERING_RAW (1 << 0) 177 | #define RCRM_SCAN_FILTERING_FASTTIME (1 << 1) 178 | #define RCRM_SCAN_FILTERING_MOTION (1 << 2) 179 | 180 | // Setting/saving configuration Persist flags. 181 | 182 | // Update the active configuration with this new configuration info. 183 | // The active configuration is not written to flash. 184 | #define RCRM_PERSIST_NONE (0) 185 | 186 | // Update the active configuration with this new configuration info. 187 | // Write entire active configuration to flash so that it persists across reboots, 188 | // including any not-written-to-flash changes that may have occurred (i.e. earlier set-config messages with RCRM_PERSIST_NONE). 189 | #define RCRM_PERSIST_ALL (1) 190 | 191 | // Update the active configuration with this new configuration info. 192 | // Write configuration to flash with the new changes. 193 | // Earlier set config messsages with RCRM_PERSIST_NONE will NOT be written to flash when this flag is set. 194 | #define RCRM_PERSIST_THIS_MSG (2) 195 | 196 | // (Experimental/Debug at this point) 197 | // Update the active configuration with this new configuration info. 198 | // Do not write the configuration to flash at this point, 199 | // but any later set-config message with RCRM_PERSIST_ALL or RCRM_PERSIST_THIS_MSG will write these changes to flash. 200 | // Intended when a series of set-config messages will be sent, but only one actual flash write is needed. 201 | #define RCRM_PERSIST_THIS_MSG_NO_WRITE (3) 202 | 203 | /////////////////////////////////////// 204 | // 205 | // Message struct definitions 206 | // 207 | 208 | 209 | 210 | typedef struct 211 | { 212 | // set to RCRM_GET_STATUS_INFO_REQUEST 213 | rcrm_uint16_t msgType; 214 | // identifier to correlate requests with confirms 215 | rcrm_uint16_t msgId; 216 | } rcrmMsg_GetStatusInfoRequest; 217 | 218 | typedef struct 219 | { 220 | // set to RCRM_GET_STATUS_INFO_CONFIRM 221 | rcrm_uint16_t msgType; 222 | // identifier to correlate requests with confirms 223 | rcrm_uint16_t msgId; 224 | 225 | rcrm_uint8_t appVersionMajor; 226 | rcrm_uint8_t appVersionMinor; 227 | rcrm_uint16_t appVersionBuild; 228 | 229 | rcrm_uint8_t uwbKernelVersionMajor; 230 | rcrm_uint8_t uwbKernelVersionMinor; 231 | rcrm_uint16_t uwbKernelVersionBuild; 232 | 233 | rcrm_uint8_t firmwareVersion; 234 | rcrm_uint8_t firmwareYear; 235 | rcrm_uint8_t firmwareMonth; 236 | rcrm_uint8_t firmwareDay; 237 | 238 | rcrm_uint32_t serialNum; 239 | 240 | rcrm_uint8_t boardRev; 241 | rcrm_uint8_t bitResults; 242 | rcrm_uint8_t model; 243 | rcrm_uint8_t pulserConfig; 244 | 245 | // Divide this by 4 to get temperature in degrees C. 246 | rcrm_int32_t temperature; 247 | 248 | char packageVersionStr[32]; 249 | 250 | // status code 251 | rcrm_uint32_t status; 252 | } rcrmMsg_GetStatusInfoConfirm; 253 | 254 | 255 | typedef struct 256 | { 257 | // set to RCRM_REBOOT_REQUEST 258 | rcrm_uint16_t msgType; 259 | // identifier to correlate requests with confirms 260 | rcrm_uint16_t msgId; 261 | } rcrmMsg_RebootRequest; 262 | 263 | typedef struct 264 | { 265 | // set to RCRM_REBOOT_CONFIRM 266 | rcrm_uint16_t msgType; 267 | // identifier to correlate requests with confirms 268 | rcrm_uint16_t msgId; 269 | } rcrmMsg_RebootConfirm; 270 | 271 | typedef struct 272 | { 273 | // set to RCRM_SET_OPMODE_REQUEST 274 | rcrm_uint16_t msgType; 275 | // identifier to correlate requests with confirms 276 | rcrm_uint16_t msgId; 277 | 278 | // Requested operational mode of the P400. 279 | rcrm_uint32_t opMode; 280 | } rcrmMsg_SetOpmodeRequest; 281 | 282 | typedef struct 283 | { 284 | // set to RCRM_SET_OPMODE_CONFIRM 285 | rcrm_uint16_t msgType; 286 | // identifier to correlate requests with confirms 287 | rcrm_uint16_t msgId; 288 | 289 | // Opmode of the radio 290 | rcrm_uint32_t opMode; 291 | 292 | rcrm_uint32_t status; 293 | } rcrmMsg_SetOpmodeConfirm; 294 | 295 | typedef struct 296 | { 297 | // set to RCRM_GET_OPMODE_REQUEST 298 | rcrm_uint16_t msgType; 299 | // identifier to correlate requests with confirms 300 | rcrm_uint16_t msgId; 301 | } rcrmMsg_GetOpmodeRequest; 302 | 303 | typedef struct 304 | { 305 | // set to RCRM_GET_OPMODE_CONFIRM 306 | rcrm_uint16_t msgType; 307 | // identifier to correlate requests with confirms 308 | rcrm_uint16_t msgId; 309 | 310 | // Current operational mode of the P400. 311 | rcrm_uint32_t opMode; 312 | } rcrmMsg_GetOpmodeConfirm; 313 | 314 | typedef struct 315 | { 316 | // set to RCRM_SET_SLEEP_MODE_REQUEST 317 | rcrm_uint16_t msgType; 318 | // identifier to correlate requests with confirms 319 | rcrm_uint16_t msgId; 320 | 321 | // Requested sleep mode of the P400. 322 | rcrm_uint32_t sleepMode; 323 | } rcrmMsg_SetSleepModeRequest; 324 | 325 | typedef struct 326 | { 327 | // set to RCRM_SET_SLEEP_MODE_CONFIRM 328 | rcrm_uint16_t msgType; 329 | // identifier to correlate requests with confirms 330 | rcrm_uint16_t msgId; 331 | 332 | rcrm_uint32_t status; 333 | } rcrmMsg_SetSleepModeConfirm; 334 | 335 | typedef struct 336 | { 337 | // set to RCRM_GET_SLEEP_MODE_REQUEST 338 | rcrm_uint16_t msgType; 339 | // identifier to correlate requests with confirms 340 | rcrm_uint16_t msgId; 341 | } rcrmMsg_GetSleepModeRequest; 342 | 343 | typedef struct 344 | { 345 | // set to RCRM_GET_SLEEP_MODE_CONFIRM 346 | rcrm_uint16_t msgType; 347 | // identifier to correlate requests with confirms 348 | rcrm_uint16_t msgId; 349 | 350 | // Current sleep mode of the P400. 351 | rcrm_uint32_t sleepMode; 352 | } rcrmMsg_GetSleepModeConfirm; 353 | 354 | typedef struct 355 | { 356 | // set to RCRM_GET_AMBIENT_SAMPLES_REQUEST 357 | rcrm_uint16_t msgType; 358 | // identifier to correlate requests with confirms 359 | rcrm_uint16_t msgId; 360 | 361 | // PII of samples - generally, the currently selected PII should be used 362 | // The radio can collect true "samples" (as opposed to integrated ramp 363 | // values) by setting this to 0. 364 | rcrm_uint16_t integrationIndex; 365 | 366 | } rcrmMsg_GetAmbientSamplesRequest; 367 | 368 | typedef struct 369 | { 370 | // set to RCRM_GET_AMBIENT_SAMPLES_CONFIRM 371 | rcrm_uint16_t msgType; 372 | // identifier to correlate requests with confirms 373 | rcrm_uint16_t msgId; 374 | 375 | // the ambient samples 376 | rcrm_int32_t samples[RCRM_MAX_SCAN_SAMPLES]; 377 | 378 | rcrm_uint32_t status; 379 | } rcrmMsg_GetAmbientSamplesConfirm; 380 | 381 | typedef struct 382 | { 383 | // set to RCRM_BIT_REQUEST 384 | rcrm_uint16_t msgType; 385 | // identifier to correlate requests with confirms 386 | rcrm_uint16_t msgId; 387 | } rcrmMsg_BitRequest; 388 | 389 | typedef struct 390 | { 391 | // set to RCRM_BIT_CONFIRM 392 | rcrm_uint16_t msgType; 393 | // identifier to correlate requests with confirms 394 | rcrm_uint16_t msgId; 395 | 396 | // BIT status - 0 is OK, anything else is an error 397 | rcrm_uint32_t status; 398 | } rcrmMsg_BitConfirm; 399 | 400 | typedef struct 401 | { 402 | // set to RCRM_GET_LAST_BOOT_CAUSE_REQUEST 403 | rcrm_uint16_t msgType; 404 | // identifier to correlate requests with confirms 405 | rcrm_uint16_t msgId; 406 | } rcrmMsg_GetLastBootCauseRequest; 407 | 408 | typedef struct 409 | { 410 | // set to RCRM_GET_LAST_BOOT_CAUSE_CONFIRM 411 | rcrm_uint16_t msgType; 412 | // identifier to correlate requests with confirms 413 | rcrm_uint16_t msgId; 414 | 415 | // Cause of last boot 416 | // 0 - Normal powerup 417 | // 1 - Watchdog reset 418 | rcrm_uint32_t lastBootCause; 419 | } rcrmMsg_GetLastBootCauseConfirm; 420 | 421 | 422 | typedef struct 423 | { 424 | // set to RCRM_READY_INFO 425 | rcrm_uint16_t msgType; 426 | // identifier to correlate requests with info messages 427 | rcrm_uint16_t msgId; 428 | } rcrmMsg_ReadyInfo; 429 | 430 | typedef struct 431 | { 432 | // set to RCRM_FULL_SCAN_INFO 433 | rcrm_uint16_t msgType; 434 | // identifier to correlate range requests with info messages 435 | rcrm_uint16_t msgId; 436 | 437 | // ID of the transmitting radio 438 | rcrm_uint32_t sourceId; 439 | 440 | // Milliseconds since radio boot at the time the scan was completed 441 | rcrm_uint32_t timestamp; 442 | 443 | // noise 444 | rcrm_uint16_t noise; 445 | // Vpeak 446 | rcrm_uint16_t vPeak; 447 | 448 | rcrm_uint32_t reserved1; // inserted with RCM 2.0 to avoid breaking MRM 1.1 449 | 450 | // These are indices within the assembled scan. 451 | rcrm_int32_t ledIndex; 452 | rcrm_int32_t lockspotOffset; 453 | 454 | rcrm_int32_t scanStartPs; 455 | rcrm_int32_t scanStopPs; 456 | 457 | rcrm_uint16_t scanStepBins; 458 | 459 | // Raw, fast time, motion, etc. 460 | rcrm_uint8_t scanFiltering; 461 | 462 | rcrm_uint8_t reserved2; // alignment 463 | 464 | // Antenna the scan was received on 465 | rcrm_uint8_t antennaId; 466 | 467 | // The type of operation behind this scan (ranging, BSR, MSR) 468 | rcrm_uint8_t operationMode; 469 | 470 | // Number of scan samples in this message 471 | rcrm_uint16_t numSamplesInMessage; 472 | 473 | // Number of samples in the entire scan 474 | rcrm_uint32_t numSamplesTotal; 475 | 476 | // Index of the message in the scan 477 | rcrm_uint16_t messageIndex; 478 | 479 | // Total number of RCRM_FULL_SCAN_INFO messages to expect for this particular scan. 480 | rcrm_uint16_t numMessagesTotal; 481 | 482 | // Scan samples. 483 | // Note that, unlike RCRM_SCAN_INFO, this is NOT a variable-sized packet. 484 | rcrm_int32_t scan[RCRM_MAX_SCAN_SAMPLES]; 485 | } rcrmMsg_FullScanInfo; 486 | 487 | #endif // #ifdef __rcrmHostInterfaceCommon_h 488 | -------------------------------------------------------------------------------- /src/hostInterfaceRCM.h: -------------------------------------------------------------------------------- 1 | #ifndef __rcrmHostInterfaceRCM_h 2 | #define __rcrmHostInterfaceRCM_h 3 | 4 | #include "hostInterfaceCommon.h" 5 | 6 | // Maximum size of user data 7 | #define RCM_USER_DATA_LENGTH (1000) 8 | 9 | /////////////////////////////////////// 10 | // 11 | // Message types 12 | // 13 | 14 | // REQUEST messages are sent by the host to the embedded applicaion. 15 | // CONFIRM messages are sent by the embedded application to the host in response to REQUEST messages. 16 | // INFO messages are sent automatically by the embedded application to the host when various events occur. 17 | #define RCM_MSG_TYPE_REQUEST (0x0000) 18 | #define RCM_MSG_TYPE_CONFIRM (0x0100) 19 | #define RCM_MSG_TYPE_INFO (0x0200) 20 | 21 | 22 | /////////////////////////////////////// 23 | // 24 | // Host <-> Embedded conversation messages 25 | // 26 | 27 | 28 | // Set configuration 29 | #define RCM_SET_CONFIG_REQUEST (RCM_MSG_TYPE_REQUEST + 1) 30 | #define RCM_SET_CONFIG_CONFIRM (RCM_MSG_TYPE_CONFIRM + 1) 31 | 32 | 33 | // Get configuration 34 | #define RCM_GET_CONFIG_REQUEST (RCM_MSG_TYPE_REQUEST + 2) 35 | #define RCM_GET_CONFIG_CONFIRM (RCM_MSG_TYPE_CONFIRM + 2) 36 | 37 | 38 | // Initiate range request 39 | #define RCM_SEND_RANGE_REQUEST (RCM_MSG_TYPE_REQUEST + 3) 40 | #define RCM_SEND_RANGE_REQUEST_CONFIRM (RCM_MSG_TYPE_CONFIRM + 3) 41 | 42 | 43 | // Send data to another radio 44 | #define RCM_SEND_DATA_REQUEST (RCM_MSG_TYPE_REQUEST + 4) 45 | #define RCM_SEND_DATA_CONFIRM (RCM_MSG_TYPE_CONFIRM + 4) 46 | 47 | 48 | // Set the data the responder will use when responding to a range request. 49 | #define RCM_SET_RESPONSE_DATA_REQUEST (RCM_MSG_TYPE_REQUEST + 5) 50 | #define RCM_SET_RESPONSE_DATA_CONFIRM (RCM_MSG_TYPE_CONFIRM + 5) 51 | 52 | // Send range request with a code channel selection 53 | #define RCM_SEND_CHANNELIZED_RANGE_REQUEST (RCM_MSG_TYPE_REQUEST + 6) 54 | #define RCM_SEND_CHANNELIZED_RANGE_REQUEST_CONFIRM (RCM_MSG_TYPE_CONFIRM + 6) 55 | 56 | /////////////////////////////////////// 57 | // 58 | // Message struct definitions 59 | // 60 | 61 | typedef struct 62 | { 63 | // ID of the radio in the network 64 | rcrm_uint32_t nodeId; 65 | 66 | rcrm_uint16_t integrationIndex; 67 | rcrm_uint8_t antennaMode; 68 | rcrm_uint8_t codeChannel; 69 | 70 | // Two separate electrical delays in picoseconds - one for each antenna 71 | rcrm_int32_t electricalDelayPsA; 72 | rcrm_int32_t electricalDelayPsB; 73 | 74 | rcrm_uint16_t flags; 75 | 76 | // 0 = lowest power, 63 = highest power 77 | rcrm_uint8_t txGain; 78 | 79 | // set to non-zero to indicate settings should persist across radio reboots 80 | rcrm_uint8_t persistFlag; 81 | } rcmConfiguration; 82 | 83 | /////////////////////////////////////// 84 | // 85 | // RCM INFO messages to the host 86 | // 87 | 88 | // Results of an attempted range conversation 89 | #define RCM_FULL_RANGE_INFO (RCM_MSG_TYPE_INFO + 1) 90 | 91 | // Sent when data was received over the air 92 | #define RCM_DATA_INFO (RCM_MSG_TYPE_INFO + 2) 93 | 94 | // Sent when a waveform scan is completed, if the RCM_SEND_SCANINFO_PKTS flag is set. 95 | // If the complete scan doesn't fit in a single packet, only the portion of the scan centered around the leading edge is included. 96 | #define RCM_SCAN_INFO (RCM_MSG_TYPE_INFO + 3) 97 | 98 | // Overheard echoed precision range measurement. 99 | // Only sent if the RCM_FLAG_ENABLE_ECHO_LAST_RANGE flag is set in the RCM configuration. 100 | #define RCM_ECHOED_RANGE_INFO (RCM_MSG_TYPE_INFO + 4) 101 | 102 | // Smaller version of RANGE_INFO for bandwidth-limited situations. 103 | #define RCM_SMALL_RANGE_INFO (RCM_MSG_TYPE_INFO + 5) 104 | 105 | 106 | 107 | // Definitions of the rcmConfiguration::flags bitfield 108 | 109 | // RCM_SEND_SCANINFO_PKTS - when set, SCAN_INFO packets are sent whenever a scan is received. 110 | // Default is disabled/not set. 111 | #define RCM_SEND_SCANINFO_PKTS (1 << 0) 112 | 113 | // RCM_SEND_FULL_SCANINFO_PKTS - when set, FULL_SCAN_INFO packets are sent whenever a scan is received. 114 | // Default is disabled/not set. 115 | #define RCM_SEND_FULL_SCANINFO_PKTS (1 << 1) 116 | 117 | // RCM_DISABLE_FAN - (for Hammerhead Rev B) when set, turns OFF the fan. 118 | // Default is not set/fan enabled. 119 | #define RCM_DISABLE_FAN (1 << 2) 120 | 121 | // RCM_SEND_SCAN_WITH_DATA - when set, data packets cause RX to scan 122 | // Default is not set - data packets do not cause scans. 123 | #define RCM_SEND_SCAN_WITH_DATA (1 << 3) 124 | 125 | // RCM_DISABLE_CRE_RANGES - when set, non-precision (CRE + Filtered) range messages are NOT sent to the host. 126 | // Default is not set/send CRE messages. 127 | #define RCM_DISABLE_CRE_RANGES (1 << 4) 128 | 129 | // RCM_USE_SINGLE_SIDED_CCI_CHECK - when set, statistics are collected for each UWB node RCM hears and analyzed to flag cochannel interference. 130 | // Default is not set/don't use single-sided cochannel interference check. 131 | #define RCM_USE_SINGLE_SIDED_CCI_CHECK (1 << 5) 132 | 133 | // RCM_FLAG_LOS_NLOS_MISMATCH_AS_CCI - when set, a LOS/NLOS mismatch in a range conversation gets flagged as cochannel interference. 134 | // Default is not set/use ranges even when they are LOS/NLOS mismatched. 135 | #define RCM_FLAG_LOS_NLOS_MISMATCH_AS_CCI (1 << 6) 136 | 137 | // RCM_FLAG_ENABLE_ECHO_LAST_RANGE - when set, RCM will echo its last successful precision range as part of the user data in its range request flag. 138 | // This flag also enables sending to the host all overheard echoed ranges. 139 | #define RCM_FLAG_ENABLE_ECHO_LAST_RANGE (1 << 7) 140 | 141 | // RCM_FLAG_SEND_SMALL_RANGE_INFOS - when set, RCM will send RCM_SMALL_RANGE_INFOs instead of RCM_RANGE_INFOs. 142 | #define RCM_FLAG_SEND_SMALL_RANGE_INFOS (1 << 8) 143 | 144 | // Range status bitmask/codes 145 | 146 | // No bits set indicates no failures, i.e. success 147 | #define RCM_RANGE_STATUS_SUCCESS 0 148 | 149 | // Timeout is set by itself (since you can't have/know about any other failures in this case) 150 | #define RCM_RANGE_STATUS_TIMEOUT 1 151 | 152 | // Virtual Carrier Sense is also sent by itself since it's exclusive to any other failures. 153 | #define RCM_RANGE_STATUS_VCS 3 154 | 155 | // Weak Signal flags are sent by themselves when there is a signal strength mismatch (> 10 dB delta between computed SNRs), 156 | // but the range doesn't have any other error flags raised 157 | 158 | // The requester sees a weak signal from the responder (possibly responder is transmitting a weak signal) 159 | #define RCM_RANGE_STATUS_REQ_RX_WEAK_SIGNAL 5 160 | 161 | // The responder sees a weak signal from the requester (possibly requester is transmitting a weak signal) 162 | #define RCM_RANGE_STATUS_RESP_RX_WEAK_SIGNAL 7 163 | 164 | // Possible co-channel interference was detected 165 | #define RCM_RANGE_STATUS_COCHANNEL_INTERFERENCE 9 166 | 167 | // Line-of-sight/Non-line-of-sight mismatch on the two legs of the range conversation 168 | #define RCM_RANGE_STATUS_LOS_NLOS_MISMATCH 11 169 | 170 | // A ranging conversation failure (besides timeout) may be any combination of these. 171 | #define RCM_RANGE_STATUS_REQUESTERLEDFAIL (1 << 1) 172 | #define RCM_RANGE_STATUS_RESPONDERLEDFAIL (1 << 2) 173 | #define RCM_RANGE_STATUS_RANGETOOLONG (1 << 5) 174 | #define RCM_RANGE_STATUS_COARSERANGEUPDATE (1 << 6) 175 | 176 | #define RCM_RANGE_TYPE_PRECISION (1 << 0) 177 | #define RCM_RANGE_TYPE_COARSE (1 << 1) 178 | #define RCM_RANGE_TYPE_FILTERED (1 << 2) 179 | 180 | // Definitions of the antennaId field in various RCM_***_INFO messages 181 | #define RCM_ANTENNAID_A RCRM_ANTENNAMODE_TXA_RXA 182 | #define RCM_ANTENNAID_B RCRM_ANTENNAMODE_TXB_RXB 183 | 184 | /////////////////////////////////////// 185 | // 186 | // Host <-> RCM conversation messages 187 | // 188 | 189 | typedef struct 190 | { 191 | // set to RCM_SET_CONFIG_REQUEST 192 | rcrm_uint16_t msgType; 193 | // identifier to correlate requests with confirms 194 | rcrm_uint16_t msgId; 195 | 196 | rcmConfiguration config; 197 | } rcmMsg_SetConfigRequest; 198 | 199 | typedef struct 200 | { 201 | // set to RCM_SET_CONFIG_CONFIRM 202 | rcrm_uint16_t msgType; 203 | // identifier to correlate requests with confirms 204 | rcrm_uint16_t msgId; 205 | 206 | // status code 207 | rcrm_uint32_t status; 208 | } rcmMsg_SetConfigConfirm; 209 | 210 | 211 | typedef struct 212 | { 213 | // set to RCM_GET_CONFIG_REQUEST 214 | rcrm_uint16_t msgType; 215 | // identifier to correlate requests with confirms 216 | rcrm_uint16_t msgId; 217 | } rcmMsg_GetConfigRequest; 218 | 219 | typedef struct 220 | { 221 | // set to RCM_GET_CONFIG_CONFIRM 222 | rcrm_uint16_t msgType; 223 | // identifier to correlate requests with confirms 224 | rcrm_uint16_t msgId; 225 | 226 | rcmConfiguration config; 227 | 228 | // milliseconds since radio boot 229 | rcrm_uint32_t timestamp; 230 | 231 | // status code 232 | rcrm_uint32_t status; 233 | } rcmMsg_GetConfigConfirm; 234 | 235 | 236 | typedef struct 237 | { 238 | // set to RCM_SEND_RANGE_REQUEST 239 | rcrm_uint16_t msgType; 240 | // identifier to correlate requests with confirms 241 | rcrm_uint16_t msgId; 242 | 243 | // ID of the responder radio to send this range request to 244 | rcrm_uint32_t responderId; 245 | 246 | // which antenna(s) to use 247 | rcrm_uint8_t antennaMode; 248 | 249 | rcrm_uint8_t reserved; // alignment 250 | 251 | // number of bytes of data to send (up to 1000) 252 | rcrm_uint16_t dataSize; 253 | 254 | // Data for range request payload. 255 | // Only the first dataSize bytes need to actually be in the UDP packet. 256 | rcrm_uint8_t data[RCM_USER_DATA_LENGTH]; 257 | } rcmMsg_SendRangeRequest; 258 | 259 | typedef struct 260 | { 261 | // set to RCM_SEND_RANGE_REQUEST_CONFIRM 262 | rcrm_uint16_t msgType; 263 | // identifier to correlate requests with confirms 264 | rcrm_uint16_t msgId; 265 | 266 | // status code 267 | rcrm_uint32_t status; 268 | } rcmMsg_SendRangeRequestConfirm; 269 | 270 | 271 | typedef struct 272 | { 273 | // set to RCM_SEND_DATA_REQUEST 274 | rcrm_uint16_t msgType; 275 | // identifier to correlate requests with confirms 276 | rcrm_uint16_t msgId; 277 | 278 | // which antenna(s) to use 279 | rcrm_uint8_t antennaMode; 280 | 281 | rcrm_uint8_t reserved; // alignment 282 | 283 | // number of bytes of data to send (up to 1000) 284 | rcrm_uint16_t dataSize; 285 | 286 | // Data for payload. 287 | // Only the first dataSize bytes need to actually be in the UDP packet. 288 | rcrm_uint8_t data[RCM_USER_DATA_LENGTH]; 289 | } rcmMsg_SendDataRequest; 290 | 291 | typedef struct 292 | { 293 | // set to RCM_SEND_DATA_CONFIRM 294 | rcrm_uint16_t msgType; 295 | // identifier to correlate requests with confirms 296 | rcrm_uint16_t msgId; 297 | 298 | // status code 299 | rcrm_uint32_t status; 300 | } rcmMsg_SendDataConfirm; 301 | 302 | 303 | typedef struct 304 | { 305 | // set to RCM_SET_RESPONSE_DATA_REQUEST 306 | rcrm_uint16_t msgType; 307 | // identifier to correlate requests with confirms 308 | rcrm_uint16_t msgId; 309 | 310 | rcrm_uint16_t reserved; // alignment 311 | 312 | // number of bytes of data to send to a requester (up to 1000) 313 | rcrm_uint16_t dataSize; 314 | 315 | // Data for range response payload. 316 | // Only the first dataSize bytes need to actually be in the UDP packet. 317 | rcrm_uint8_t data[RCM_USER_DATA_LENGTH]; 318 | } rcmMsg_SetResponseDataRequest; 319 | 320 | typedef struct 321 | { 322 | // set to RCM_SET_RESPONSE_DATA_CONFIRM 323 | rcrm_uint16_t msgType; 324 | // identifier to correlate requests with confirms 325 | rcrm_uint16_t msgId; 326 | 327 | // status code 328 | rcrm_uint32_t status; 329 | } rcmMsg_SetResponseDataConfirm; 330 | 331 | typedef struct 332 | { 333 | // set to RCM_SEND_CHANNELIZED_RANGE_REQUEST 334 | rcrm_uint16_t msgType; 335 | // identifier to correlate requests with confirms 336 | rcrm_uint16_t msgId; 337 | 338 | // ID of the responder radio to send this range request to 339 | rcrm_uint32_t responderId; 340 | 341 | // which antenna(s) to use 342 | rcrm_uint8_t antennaMode; 343 | 344 | // which code channel to switch to for this range request/response 345 | rcrm_uint8_t codeChannel; 346 | 347 | // number of bytes of data to send (up to 1000) 348 | rcrm_uint16_t dataSize; 349 | 350 | // Data for range request payload. 351 | // Only the first dataSize bytes need to actually be in the UDP packet. 352 | rcrm_uint8_t data[RCM_USER_DATA_LENGTH]; 353 | } rcmMsg_SendChannelizedRangeRequest; 354 | 355 | typedef struct 356 | { 357 | // set to RCM_SEND_CHANNELIZED_RANGE_REQUEST_CONFIRM 358 | rcrm_uint16_t msgType; 359 | // identifier to correlate requests with confirms 360 | rcrm_uint16_t msgId; 361 | 362 | // status code 363 | rcrm_uint32_t status; 364 | } rcmMsg_SendChannelizedRangeRequestConfirm; 365 | 366 | /////////////////////////////////////// 367 | // 368 | // INFO messages to the host 369 | // 370 | 371 | typedef struct 372 | { 373 | // set to RCM_FULL_RANGE_INFO 374 | rcrm_uint16_t msgType; 375 | // identifier to correlate range requests with info messages 376 | rcrm_uint16_t msgId; 377 | 378 | // ID of the responding radio 379 | rcrm_uint32_t responderId; 380 | 381 | // status/error codes for the range conversation 382 | rcrm_uint8_t rangeStatus; 383 | // Antenna(s) on this radio used in the range conversation 384 | rcrm_uint8_t antennaMode; 385 | // how long the range conversation took, in milliseconds 386 | rcrm_uint16_t stopwatchTime; 387 | 388 | rcrm_uint32_t precisionRangeMm; 389 | rcrm_uint32_t coarseRangeMm; 390 | rcrm_uint32_t filteredRangeMm; 391 | 392 | rcrm_uint16_t precisionRangeErrEst; 393 | rcrm_uint16_t coarseRangeErrEst; 394 | rcrm_uint16_t filteredRangeErrEst; 395 | 396 | rcrm_int16_t filteredRangeVel; 397 | rcrm_uint16_t filteredRangeVelErrEst; 398 | 399 | rcrm_uint8_t rangeMeasurementType; 400 | rcrm_uint8_t reserved; 401 | 402 | rcrm_uint16_t reqLEDFlags; 403 | rcrm_uint16_t respLEDFlags; 404 | 405 | rcrm_uint16_t noise; 406 | rcrm_uint16_t vPeak; 407 | 408 | rcrm_int32_t coarseTOFInBins; 409 | 410 | // milliseconds since radio boot at the time of the range conversation 411 | rcrm_uint32_t timestamp; 412 | } rcmMsg_FullRangeInfo; 413 | 414 | 415 | 416 | typedef struct 417 | { 418 | // set to RCM_DATA_INFO 419 | rcrm_uint16_t msgType; 420 | // identifier to correlate range requests with info messages 421 | rcrm_uint16_t msgId; 422 | 423 | // ID of the sending radio 424 | rcrm_uint32_t sourceId; 425 | 426 | // noise 427 | rcrm_uint16_t noise; 428 | // Peak voltage, normalized. 429 | rcrm_uint16_t vPeak; 430 | // milliseconds since radio boot at the time the data packet was received 431 | rcrm_uint32_t timestamp; 432 | 433 | // Antenna the UWB packet was received on 434 | rcrm_uint8_t antennaId; 435 | 436 | rcrm_uint8_t reserved; // alignment 437 | 438 | // number of bytes of data received, up to 1000 439 | rcrm_uint16_t dataSize; 440 | 441 | // Data from packet payload 442 | // Only the first dataSize bytes are actually in the UDP packet. 443 | rcrm_uint8_t data[RCM_USER_DATA_LENGTH]; 444 | } rcmMsg_DataInfo; 445 | 446 | #define RCM_MAX_SCAN_SAMPLES 350 447 | 448 | typedef struct 449 | { 450 | // set to RCM_SCAN_INFO 451 | rcrm_uint16_t msgType; 452 | // identifier to correlate range requests with info messages 453 | rcrm_uint16_t msgId; 454 | 455 | // ID of the transmitting radio 456 | rcrm_uint32_t sourceId; 457 | 458 | // Antenna the UWB packet was received on 459 | rcrm_uint8_t antennaId; 460 | 461 | rcrm_uint8_t reserved; // alignment 462 | 463 | rcrm_uint16_t LEDflags; 464 | 465 | // channel quality (LED rise time; lower (faster rise) is better) 466 | rcrm_uint16_t noise; 467 | // Peak voltage, normalized. 468 | rcrm_uint16_t vPeak; 469 | 470 | // milliseconds since radio boot at the time the data packet was received 471 | rcrm_uint32_t timestamp; 472 | 473 | rcrm_int32_t ledIndex; 474 | rcrm_int32_t lockspotOffset; 475 | 476 | // Number of samples provided in the scan 477 | rcrm_uint32_t numSamples; 478 | // Scan samples. Note that only the first numSamples samples are sent in the UDP packet. 479 | rcrm_int32_t samples[RCM_MAX_SCAN_SAMPLES]; 480 | } rcrmMsg_ScanInfo; 481 | 482 | 483 | typedef struct 484 | { 485 | // set to RCM_ECHOED_RANGE_INFO 486 | rcrm_uint16_t msgType; 487 | // identifier to correlate range requests with info messages 488 | rcrm_uint16_t msgId; 489 | 490 | // ID of the requesting radio 491 | rcrm_uint32_t requesterId; 492 | 493 | // ID of the responding radio 494 | rcrm_uint32_t responderId; 495 | 496 | // Reported range in millimeters 497 | rcrm_uint32_t precisionRangeMm; 498 | 499 | // Reported estimated range error in millimeters 500 | rcrm_uint16_t precisionRangeErrEst; 501 | 502 | // Requester and responder LED flags, or'd together. 503 | rcrm_uint16_t ledFlags; 504 | 505 | // Milliseconds since the requesting radio boot at the time of the range conversation. 506 | // Used to identify duplicate ELR reports. 507 | rcrm_uint32_t timestamp; 508 | } rcmMsg_EchoedRangeInfo; 509 | 510 | 511 | #pragma pack(1) 512 | 513 | typedef struct 514 | { 515 | // set to RCM_SMALL_RANGE_INFO 516 | rcrm_uint16_t msgType; 517 | // identifier to correlate requests with confirms 518 | rcrm_uint16_t msgId; 519 | 520 | rcrm_uint32_t responderId; 521 | 522 | rcrm_uint16_t rangeCm; 523 | rcrm_uint8_t rangeErrEst; 524 | 525 | rcrm_uint8_t rangeType; 526 | 527 | rcrm_uint8_t rangeStatus; 528 | 529 | rcrm_uint8_t flags; 530 | } rcmMsg_SmallRangeInfo; 531 | 532 | #pragma pack() 533 | 534 | 535 | #endif // #ifdef __rcrmHostInterfaceRCM_h 536 | -------------------------------------------------------------------------------- /src/hostInterfaceRN.h: -------------------------------------------------------------------------------- 1 | #ifndef __rcrmHostInterfaceRN_h 2 | #define __rcrmHostInterfaceRN_h 3 | 4 | #include "hostInterfaceCommon.h" 5 | 6 | // Maximum size of user data 7 | #define RN_USER_DATA_LENGTH (128) 8 | 9 | // Maximum number of excluded range targets 10 | #define RN_MAX_EXCLUDED_TARGETS (256) 11 | 12 | // Maximum number of entries in the full neighbor database message 13 | #define RN_FULL_NEIGHBOR_DATABASE_MAX_ENTRY_COUNT 32 14 | 15 | // Maximum number of entries in the small neighbor database message 16 | #define RN_SMALL_NEIGHBOR_DATABASE_MAX_ENTRY_COUNT 80 17 | 18 | // Maximum number of slots in the TDMA slotmap 19 | #define RN_MAX_TDMA_SLOTS (32) 20 | 21 | /////////////////////////////////////// 22 | // 23 | // Message types 24 | // 25 | 26 | // REQUEST messages are sent by the host to the embedded application. 27 | // CONFIRM messages are sent by the embedded application to the host in response to REQUEST messages. 28 | // INFO messages are sent automatically by the embedded application to the host when various events occur. 29 | #define RN_MSG_TYPE_REQUEST (0x3000) 30 | #define RN_MSG_TYPE_CONFIRM (0x3100) 31 | #define RN_MSG_TYPE_INFO (0x3200) 32 | 33 | 34 | /////////////////////////////////////// 35 | // 36 | // Host <-> Embedded conversation messages 37 | // 38 | 39 | // Set configuration 40 | #define RN_SET_CONFIG_REQUEST (RN_MSG_TYPE_REQUEST + 1) 41 | #define RN_SET_CONFIG_CONFIRM (RN_MSG_TYPE_CONFIRM + 1) 42 | 43 | // Get configuration 44 | #define RN_GET_CONFIG_REQUEST (RN_MSG_TYPE_REQUEST + 2) 45 | #define RN_GET_CONFIG_CONFIRM (RN_MSG_TYPE_CONFIRM + 2) 46 | 47 | // Set user data sent out with every range request 48 | #define RN_SET_REQUEST_USER_DATA_REQUEST (RN_MSG_TYPE_REQUEST + 3) 49 | #define RN_SET_REQUEST_USER_DATA_CONFIRM (RN_MSG_TYPE_CONFIRM + 3) 50 | 51 | // Set user data sent out with every range response 52 | #define RN_SET_RESPONSE_USER_DATA_REQUEST (RN_MSG_TYPE_REQUEST + 4) 53 | #define RN_SET_RESPONSE_USER_DATA_CONFIRM (RN_MSG_TYPE_CONFIRM + 4) 54 | 55 | // Get neighbor database 56 | #define RN_GET_FULL_NEIGHBOR_DATABASE_REQUEST (RN_MSG_TYPE_REQUEST + 5) 57 | #define RN_GET_FULL_NEIGHBOR_DATABASE_CONFIRM (RN_MSG_TYPE_CONFIRM + 5) 58 | #define RN_GET_SMALL_NEIGHBOR_DATABASE_REQUEST (RN_MSG_TYPE_REQUEST + 6) 59 | #define RN_GET_SMALL_NEIGHBOR_DATABASE_CONFIRM (RN_MSG_TYPE_CONFIRM + 6) 60 | 61 | // Set excluded targets list 62 | #define RN_SET_EXCLUDED_REQUEST (RN_MSG_TYPE_REQUEST + 7) 63 | #define RN_SET_EXCLUDED_CONFIRM (RN_MSG_TYPE_CONFIRM + 7) 64 | 65 | // Get excluded targets list 66 | #define RN_GET_EXCLUDED_REQUEST (RN_MSG_TYPE_REQUEST + 8) 67 | #define RN_GET_EXCLUDED_CONFIRM (RN_MSG_TYPE_CONFIRM + 8) 68 | 69 | // Get health monitoring statistics 70 | #define RN_GET_HEALTH_STATUS_REQUEST (RN_MSG_TYPE_REQUEST + 9) 71 | #define RN_GET_HEALTH_STATUS_CONFIRM (RN_MSG_TYPE_CONFIRM + 9) 72 | 73 | // Reset neighbor database, health statistics, etc. for all nodes or an individual node 74 | #define RN_RESET_DATABASE_AND_STATS_REQUEST (RN_MSG_TYPE_REQUEST + 10) 75 | #define RN_RESET_DATABASE_AND_STATS_CONFIRM (RN_MSG_TYPE_CONFIRM + 10) 76 | 77 | // Get a copy of the user data sent out with every range request 78 | #define RN_GET_REQUEST_USER_DATA_REQUEST (RN_MSG_TYPE_REQUEST + 11) 79 | #define RN_GET_REQUEST_USER_DATA_CONFIRM (RN_MSG_TYPE_CONFIRM + 11) 80 | 81 | // Get a copy of the user data sent out with every range response 82 | #define RN_GET_RESPONSE_USER_DATA_REQUEST (RN_MSG_TYPE_REQUEST + 12) 83 | #define RN_GET_RESPONSE_USER_DATA_CONFIRM (RN_MSG_TYPE_CONFIRM + 12) 84 | 85 | // Set ALOHA configuration 86 | #define RN_SET_ALOHA_CONFIG_REQUEST (RN_MSG_TYPE_REQUEST + 13) 87 | #define RN_SET_ALOHA_CONFIG_CONFIRM (RN_MSG_TYPE_CONFIRM + 13) 88 | 89 | // Get ALOHA configuration 90 | #define RN_GET_ALOHA_CONFIG_REQUEST (RN_MSG_TYPE_REQUEST + 14) 91 | #define RN_GET_ALOHA_CONFIG_CONFIRM (RN_MSG_TYPE_CONFIRM + 14) 92 | 93 | // Get durations of OTA packets and range conversations 94 | #define RN_GET_PACKET_DURATIONS_REQUEST (RN_MSG_TYPE_REQUEST + 15) 95 | #define RN_GET_PACKET_DURATIONS_CONFIRM (RN_MSG_TYPE_CONFIRM + 15) 96 | 97 | // Set TDMA slotmap or individual slot(s) 98 | #define RN_SET_TDMA_SLOTMAP_REQUEST (RN_MSG_TYPE_REQUEST + 16) 99 | #define RN_SET_TDMA_SLOTMAP_CONFIRM (RN_MSG_TYPE_CONFIRM + 16) 100 | 101 | // Get entire TDMA slotmap 102 | #define RN_GET_TDMA_SLOTMAP_REQUEST (RN_MSG_TYPE_REQUEST + 17) 103 | #define RN_GET_TDMA_SLOTMAP_CONFIRM (RN_MSG_TYPE_CONFIRM + 17) 104 | 105 | // Get single TDMA slot info 106 | #define RN_GET_TDMA_SLOT_REQUEST (RN_MSG_TYPE_REQUEST + 18) 107 | #define RN_GET_TDMA_SLOT_CONFIRM (RN_MSG_TYPE_CONFIRM + 18) 108 | 109 | // Set TDMA configuration 110 | #define RN_SET_TDMA_CONFIG_REQUEST (RN_MSG_TYPE_REQUEST + 19) 111 | #define RN_SET_TDMA_CONFIG_CONFIRM (RN_MSG_TYPE_CONFIRM + 19) 112 | 113 | // Get TDMA configuration 114 | #define RN_GET_TDMA_CONFIG_REQUEST (RN_MSG_TYPE_REQUEST + 20) 115 | #define RN_GET_TDMA_CONFIG_CONFIRM (RN_MSG_TYPE_CONFIRM + 20) 116 | 117 | 118 | /////////////////////////////////////// 119 | // 120 | // Embedded -> Host info messages 121 | // 122 | 123 | #define RN_TIMING_INFO (RN_MSG_TYPE_INFO + 2) 124 | #define RN_FULL_NEIGHBOR_DATABASE_INFO (RN_MSG_TYPE_INFO + 3) 125 | #define RN_SMALL_NEIGHBOR_DATABASE_INFO (RN_MSG_TYPE_INFO + 4) 126 | 127 | /////////////////////////////////////// 128 | // 129 | // Flags, etc. 130 | // 131 | 132 | // Network synchronization modes 133 | #define RN_NETWORK_SYNC_MODE_ALOHA (0) 134 | #define RN_NETWORK_SYNC_MODE_TDMA (1) 135 | 136 | // Neighbor database sorting flags 137 | 138 | #define RN_NEIGHBOR_DATABASE_SORT_NODEID (0) 139 | #define RN_NEIGHBOR_DATABASE_SORT_RANGE (1) 140 | #define RN_NEIGHBOR_DATABASE_SORT_AGE (2) 141 | 142 | // Auto-send range info flags 143 | 144 | #define RN_AUTOSEND_RANGEINFO_FLAGS_MASK (3 << 0) 145 | // Send no range infos 146 | #define RN_AUTOSEND_RANGEINFO_NONE (0) 147 | // Send full range infos for successful ranges 148 | #define RN_AUTOSEND_RANGEINFO_SUCCESSFUL (1 << 0) 149 | // Send full range infos for all ranges 150 | #define RN_AUTOSEND_RANGEINFO_ALL (2 << 0) 151 | // Send small range infos for successful ranges 152 | #define RN_AUTOSEND_RANGEINFO_SMALL (3 << 0) 153 | 154 | // Auto-send neighbor database flags 155 | 156 | #define RN_AUTOSEND_NEIGHBOR_DATABASE_FLAGS_MASK (3 << 2) 157 | // Do not auto-send neighbor database 158 | #define RN_AUTOSEND_NEIGHBOR_DATABASE_NONE (0 << 2) 159 | // Auto-send full neighbor database 160 | #define RN_AUTOSEND_NEIGHBOR_DATABASE_FULL (1 << 2) 161 | // Auto-send small neighbor database 162 | #define RN_AUTOSEND_NEIGHBOR_DATABASE_SMALL (2 << 2) 163 | 164 | // Default interface selections 165 | 166 | // Wait for host interaction before sending messages; this is the default selection. 167 | #define RN_INTERFACE_NONE (0) 168 | 169 | // Send messages using the Ethernet interface. 170 | // Only supported on radios with Ethernet interfaces. 171 | // address1 is the IPv4 address of the destination. 172 | // address2 is the 16-bit port number. 173 | #define RN_INTERFACE_IP (1) 174 | 175 | // Send messages using the USB interface. 176 | #define RN_INTERFACE_USB (2) 177 | 178 | // Send messages using the serial interface. 179 | #define RN_INTERFACE_SERIAL (3) 180 | 181 | // Send messages using the CAN bus interface. 182 | // Only supported on radios with a CAN interface. 183 | #define RN_INTERFACE_CAN (4) 184 | 185 | 186 | // Neighbor database entry flags which indicate special characteristics of the neighbor or neighbor database entry 187 | 188 | // Indicates that this neighbor is in beacon mode 189 | #define RN_NEIGHBOR_DATABASE_ENTRY_FLAG_IS_BEACON_NODE (1 << 0) 190 | // Indicates that this neighbor has set its do-not-range-to-me flag 191 | #define RN_NEIGHBOR_DATABASE_ENTRY_FLAG_DO_NOT_RANGE_TO_ME (1 << 1) 192 | // Indicates that this neighbor is in the local excluded targets list 193 | #define RN_NEIGHBOR_DATABASE_ENTRY_FLAG_EXCLUDED (1 << 2) 194 | // Indicates that this neighbor has been heard, but is uncalibrated and thus at the top of the list for next range target selection 195 | #define RN_NEIGHBOR_DATABASE_ENTRY_FLAG_UNCALIBRATED (1 << 3) 196 | 197 | // RangeNet config flags 198 | 199 | // Sets a flag that tells neighbors to NOT range to this node 200 | #define RN_CONFIG_FLAG_DO_NOT_RANGE_TO_ME (1 << 1) 201 | // Selects precision vs. filtered ranges 202 | #define RN_CONFIG_FLAG_NEIGHBOR_DATABASE_FILTERED_RANGE (1 << 2) 203 | 204 | // (1 << 3) is for a deprecated flag that was never used. It is available for the next flag to be added. 205 | 206 | // Enables sending of RN_TIMING_INFO messages to the host 207 | #define RN_CONFIG_FLAG_ENABLE_TIMING_INFOS (1 << 4) 208 | 209 | 210 | // ALOHA config flags 211 | 212 | // Set this node in beacon mode. In beacon mode, only beacon packets are automatically transmitted (i.e. no automatic ranging to neighbors). 213 | #define RN_ALOHA_CONFIG_FLAG_BEACON (1 << 0) 214 | 215 | // Use Slotted ALOHA. In Slotted ALOHA, transmit timing is rounded up to slot boundaries. Not currently supported. 216 | #define RN_ALOHA_CONFIG_FLAG_SLOTTED_ALOHA (1 << 1) 217 | 218 | // Use Automatic Congestion Control (ACC). When ACC is enabled, the min and max TX intervals are automatically determined based on the number of neighboring RangeNet units. 219 | #define RN_ALOHA_CONFIG_FLAG_USE_ACC (1 << 2) 220 | 221 | 222 | // TDMA slot flags 223 | 224 | // Sleep during this slot if this node is neither the requester or potential responder. 225 | #define RN_TDMA_SLOT_FLAG_SLEEP (1 << 0) 226 | // Allocate time in this slot for sending requester user data. 227 | #define RN_TDMA_SLOT_FLAG_REQUESTER_DATA (1 << 1) 228 | // Allocate time in this slot for sending responder user data. 229 | #define RN_TDMA_SLOT_FLAG_RESPONDER_DATA (1 << 2) 230 | 231 | 232 | // RN_SET_TDMA_SLOTMAP_REQUEST flags 233 | 234 | // Modify the slotmap with the message instead of overwriting it. 235 | #define RN_TDMA_SLOTMAP_FLAG_MODIFY (1 << 0) 236 | 237 | 238 | // Database and stats reset selection flags 239 | // Used in RN_RESET_DATABASE_AND_STATS_REQUEST 240 | // May be combined 241 | 242 | // Clears all RangeNet neighbor database entries, or, if a nodeId is specified, removes that neighbor. 243 | // Note that the neighbor database will still automatically repopulate as long as RangeNet is running normally. 244 | // By implication, includes the effect of RN_RESET_FLAG_NEIGHBOR_STATS. 245 | #define RN_RESET_FLAG_DATABASE (1 << 0) 246 | 247 | // Clears all RangeNet health stats. nodeId is irrelevant for this flag. 248 | #define RN_RESET_FLAG_HEALTH_STATS (1 << 1) 249 | 250 | // Clears individual neighbor statistics (but doesn't remove from the database) for all nodes or a single specified node. 251 | #define RN_RESET_FLAG_NEIGHBOR_STATS (1 << 2) 252 | 253 | // RN_TIMING_INFO UWB packet types 254 | #define RN_TIMING_INFO_PKT_TYPE_RCM_REQUEST (1) 255 | #define RN_TIMING_INFO_PKT_TYPE_RCM_RESPONSE (2) 256 | #define RN_TIMING_INFO_PKT_TYPE_RCM_DATA (3) 257 | #define RN_TIMING_INFO_PKT_TYPE_RN_REQUEST (4) 258 | #define RN_TIMING_INFO_PKT_TYPE_RN_RESPONSE (5) 259 | #define RN_TIMING_INFO_PKT_TYPE_RN_BEACON (6) 260 | #define RN_TIMING_INFO_PKT_TYPE_RN_DATA (9) 261 | 262 | /////////////////////////////////////// 263 | // 264 | // Message struct definitions 265 | // 266 | 267 | typedef struct 268 | { 269 | // Maximum age of entries in the neighbor database. 270 | // Not hearing from a neighbor for this amount of time will cause it to be removed from the database. 271 | rcrm_uint32_t maxNeighborAgeMs; 272 | 273 | // Sets the update interval when autosending the neighbor database. 274 | rcrm_uint16_t autosendNeighborDbUpdateIntervalMs; 275 | 276 | // RangeNet behavior, etc. flags 277 | rcrm_uint16_t rnFlags; 278 | 279 | // RN_NETWORK_SYNC_MODE_ALOHA or RN_NETWORK_SYNC_MODE_TDMA 280 | rcrm_uint8_t networkSyncMode; 281 | 282 | // Specifies automatically sending RANGE_INFO, FULL_ or SMALL_NEIGHBOR_DATABASE, etc. 283 | // Bits 0-1 are RANGE_INFO flags. 284 | // Bits 2-3 are NEIGHBOR_DATABASE flags. 285 | // Bits 4-5 are neighbor database sort flags (RN_NEIGHBOR_DATABASE_SORT_RANGE, etc.) 286 | // Can be a combination of any one of RANGE_INFO, any one of NEIGHBOR_DB, and any one of the neighbor database sort flags. 287 | // Default value is RN_AUTOSEND_RANGEINFO_NONE | RN_AUTOSEND_NEIGHBOR_DATABASE_FULL | (RN_NEIGHBOR_DATABASE_SORT_NODEID << 4) 288 | rcrm_uint8_t autosendType; 289 | 290 | rcrm_uint8_t reserved; 291 | 292 | // Any one of the RN_INTERFACE_* interface selections. 293 | rcrm_uint8_t defaultIf; 294 | 295 | // Destination address fields, used by some interfaces. 296 | rcrm_uint32_t defaultIfAddr1; 297 | rcrm_uint32_t defaultIfAddr2; 298 | } rnConfiguration; 299 | 300 | typedef struct 301 | { 302 | // Minimum and maximum time from transmit to transmit. 303 | // In slotted aloha, these times will be rounded up to make the next transmission start on a slot boundary. 304 | rcrm_uint16_t minTimeBetweenTxMs; 305 | rcrm_uint16_t maxTimeBetweenTxMs; 306 | 307 | // Maximum number of bytes the host application is going to use in the user data section of range requests and responses. 308 | // Used to calculate slot times, since sending user data will, in some cases significantly, decrease network update rate due to longer packets. 309 | // Valid values are 0 bytes to RN_USER_DATA_LENGTH bytes. 310 | // Note that these are the same values as used by TDMA, i.e. writing one writes the other. 311 | rcrm_uint16_t maxRequestDataSize; 312 | rcrm_uint16_t maxResponseDataSize; 313 | 314 | // Beacon mode, etc. 315 | rcrm_uint16_t flags; 316 | 317 | // Reserved; set to 0 318 | rcrm_uint8_t accKfactor; 319 | 320 | // Reserved; set to 0 321 | rcrm_uint8_t reserved; 322 | } rnALOHAConfiguration; 323 | 324 | typedef struct 325 | { 326 | // Maximum number of bytes the host application is going to use in the user data section of range requests and responses. 327 | // Used to calculate slot times, since sending user data will, in some cases significantly, decrease network update rate due to longer packets. 328 | // Valid values are 0 bytes to RN_USER_DATA_LENGTH bytes. 329 | // Note that these are the same values as used by ALOHA, i.e. writing one writes the other. 330 | rcrm_uint16_t maxRequestDataSize; 331 | rcrm_uint16_t maxResponseDataSize; 332 | 333 | // Reserved 334 | rcrm_uint16_t reserved1; 335 | 336 | // Reserved 337 | rcrm_uint16_t reserved2; 338 | } rnTDMAConfiguration; 339 | 340 | typedef enum 341 | { 342 | // Used to indicate an unconfigured/invalid slot, 343 | // such as the slot marking the end of the slotmap if not all RN_MAX_TDMA_SLOTS are filled. 344 | rnTDMA_SlotType_None = 0, 345 | 346 | // In this type of slot, the owner sends out a range request. 347 | rnTDMA_SlotType_Range, 348 | 349 | // In this type of slot, the owner sends out a data packet (no scan data or range response) 350 | rnTDMA_SlotType_Data, 351 | } rnTDMA_SlotType; 352 | 353 | typedef struct 354 | { 355 | // rnTDMA_SlotType type of slot. 356 | // Declared as rcrm_uint8_t to explicitly make it 1 byte. 357 | rcrm_uint8_t slotType; 358 | 359 | // Index of this slot in the slotmap. 360 | rcrm_uint8_t slotNumber; 361 | 362 | // RN_TDMA_SLOT_FLAG_SLEEP, etc. 363 | rcrm_uint16_t flags; 364 | 365 | // Integration, antenna, and channel use the same values as RCM's equivalent configuration fields. 366 | rcrm_uint8_t integrationIndex; 367 | // Antenna mode: lower nibble is owner antenna mode, and upper nibble is potential responders' antenna mode. 368 | rcrm_uint8_t antennaMode; 369 | rcrm_uint8_t codeChannel; 370 | 371 | // Reserved for alignment and future growth. 372 | rcrm_uint8_t reserved1; 373 | 374 | // UWB node ID of the slot's owner. 375 | rcrm_uint32_t requesterId; 376 | 377 | // UWB node ID of the node to which the slot's owner should transmit or range. 378 | // Ignored unless slotType == rnTDMA_SlotType_Range. 379 | // If responderId == 0xFFFFFFFF (the ranging "broadcast" address), the round-robin method of choosing a target is used. 380 | // If responderId == 0 (none), then a RangeNet beacon packet is sent instead. However, this is not likely to be a useful configuration. 381 | rcrm_uint32_t responderId; 382 | 383 | // Requested slot duration, in microseconds. 384 | // If the slot as configured won't fit in this duration, an error is returned in RN_SET_TDMA_SLOTMAP_CONFIRM. 385 | // 0 (default) means use value computed by radio. 386 | rcrm_uint32_t requestedDurationMicroseconds; 387 | } rnTDMASlotDefinition; 388 | 389 | // Slot defintion along with computed duration. 390 | // Used in messages containing slot info sent from the radio to the host app. 391 | typedef struct 392 | { 393 | // The slot definition. 394 | rnTDMASlotDefinition slot; 395 | 396 | // Minimum slot duration in microseconds, computed by the radio. 397 | rcrm_uint32_t minimumDurationMicroseconds; 398 | } rnTDMASlotWithComputedDuration; 399 | 400 | /////////////////////////////////////// 401 | // 402 | // Host <-> RN conversation messages 403 | // 404 | 405 | // RangeNet general config messages 406 | 407 | typedef struct 408 | { 409 | // set to RN_SET_CONFIG_REQUEST 410 | rcrm_uint16_t msgType; 411 | // identifier to correlate requests with confirms 412 | rcrm_uint16_t msgId; 413 | 414 | rnConfiguration config; 415 | 416 | // Set to non-zero to indicate settings should persist across radio reboots 417 | // Not used in get config response. 418 | rcrm_uint8_t persistFlag; 419 | 420 | rcrm_uint8_t reserved[3]; 421 | } rnMsg_SetConfigRequest; 422 | 423 | typedef struct 424 | { 425 | // set to RN_SET_CONFIG_CONFIRM 426 | rcrm_uint16_t msgType; 427 | // identifier to correlate requests with confirms 428 | rcrm_uint16_t msgId; 429 | 430 | // status code 431 | rcrm_uint32_t status; 432 | } rnMsg_SetConfigConfirm; 433 | 434 | 435 | typedef struct 436 | { 437 | // set to RN_GET_CONFIG_REQUEST 438 | rcrm_uint16_t msgType; 439 | // identifier to correlate requests with confirms 440 | rcrm_uint16_t msgId; 441 | } rnMsg_GetConfigRequest; 442 | 443 | typedef struct 444 | { 445 | // set to RN_GET_CONFIG_CONFIRM 446 | rcrm_uint16_t msgType; 447 | // identifier to correlate requests with confirms 448 | rcrm_uint16_t msgId; 449 | 450 | rnConfiguration config; 451 | 452 | // milliseconds since radio boot 453 | rcrm_uint32_t timestamp; 454 | 455 | // status code 456 | rcrm_uint32_t status; 457 | } rnMsg_GetConfigConfirm; 458 | 459 | 460 | // ALOHA config messages 461 | 462 | typedef struct 463 | { 464 | // set to RN_SET_ALOHA_CONFIG_REQUEST 465 | rcrm_uint16_t msgType; 466 | // identifier to correlate requests with confirms 467 | rcrm_uint16_t msgId; 468 | 469 | rnALOHAConfiguration config; 470 | 471 | // Set to non-zero to indicate settings should persist across radio reboots 472 | // Not used in get config response. 473 | rcrm_uint8_t persistFlag; 474 | 475 | rcrm_uint8_t reserved[3]; 476 | } rnMsg_SetALOHAConfigRequest; 477 | 478 | typedef struct 479 | { 480 | // set to RN_SET_ALOHA_CONFIG_CONFIRM 481 | rcrm_uint16_t msgType; 482 | // identifier to correlate requests with confirms 483 | rcrm_uint16_t msgId; 484 | 485 | // status code 486 | rcrm_uint32_t status; 487 | } rnMsg_SetALOHAConfigConfirm; 488 | 489 | 490 | typedef struct 491 | { 492 | // set to RN_GET_ALOHA_CONFIG_REQUEST 493 | rcrm_uint16_t msgType; 494 | // identifier to correlate requests with confirms 495 | rcrm_uint16_t msgId; 496 | } rnMsg_GetALOHAConfigRequest; 497 | 498 | typedef struct 499 | { 500 | // set to RN_GET_ALOHA_CONFIG_CONFIRM 501 | rcrm_uint16_t msgType; 502 | // identifier to correlate requests with confirms 503 | rcrm_uint16_t msgId; 504 | 505 | rnALOHAConfiguration config; 506 | 507 | // status code 508 | rcrm_uint32_t status; 509 | } rnMsg_GetALOHAConfigConfirm; 510 | 511 | 512 | 513 | // TDMA config messages 514 | 515 | typedef struct 516 | { 517 | // set to RN_SET_TDMA_CONFIG_REQUEST 518 | rcrm_uint16_t msgType; 519 | // identifier to correlate requests with confirms 520 | rcrm_uint16_t msgId; 521 | 522 | rnTDMAConfiguration config; 523 | 524 | // Set to non-zero to indicate settings should persist across radio reboots 525 | // Not used in get config response. 526 | rcrm_uint8_t persistFlag; 527 | 528 | rcrm_uint8_t reserved[3]; 529 | } rnMsg_SetTDMAConfigRequest; 530 | 531 | typedef struct 532 | { 533 | // set to RN_SET_TDMA_CONFIG_CONFIRM 534 | rcrm_uint16_t msgType; 535 | // identifier to correlate requests with confirms 536 | rcrm_uint16_t msgId; 537 | 538 | // status code 539 | rcrm_uint32_t status; 540 | } rnMsg_SetTDMAConfigConfirm; 541 | 542 | 543 | typedef struct 544 | { 545 | // set to RN_GET_TDMA_CONFIG_REQUEST 546 | rcrm_uint16_t msgType; 547 | // identifier to correlate requests with confirms 548 | rcrm_uint16_t msgId; 549 | } rnMsg_GetTDMAConfigRequest; 550 | 551 | typedef struct 552 | { 553 | // set to RN_GET_TDMA_CONFIG_CONFIRM 554 | rcrm_uint16_t msgType; 555 | // identifier to correlate requests with confirms 556 | rcrm_uint16_t msgId; 557 | 558 | rnTDMAConfiguration config; 559 | 560 | // status code 561 | rcrm_uint32_t status; 562 | } rnMsg_GetTDMAConfigConfirm; 563 | 564 | 565 | typedef struct 566 | { 567 | // set to RN_SET_TDMA_SLOTMAP_REQUEST 568 | rcrm_uint16_t msgType; 569 | // identifier to correlate requests with confirms 570 | rcrm_uint16_t msgId; 571 | 572 | // Number of slots in this variable-length message 573 | rcrm_uint8_t numSlots; 574 | 575 | // RN_TDMA_SLOTMAP_FLAG_MODIFY, etc. 576 | rcrm_uint8_t slotmapFlags; 577 | 578 | // Reserved for aligment and future growth 579 | rcrm_uint8_t reserved; 580 | 581 | // Set to non-zero to indicate settings should persist across radio reboots 582 | rcrm_uint8_t persistFlag; 583 | 584 | // The slot definitions. 585 | // Only the first numSlots need be included in this variable-length message. 586 | rnTDMASlotDefinition slots[RN_MAX_TDMA_SLOTS]; 587 | } rnMsg_SetTDMASlotmapRequest; 588 | 589 | typedef struct 590 | { 591 | // set to RN_SET_TDMA_SLOTMAP_CONFIRM 592 | rcrm_uint16_t msgType; 593 | // identifier to correlate requests with confirms 594 | rcrm_uint16_t msgId; 595 | 596 | // status code 597 | rcrm_uint32_t status; 598 | } rnMsg_SetTDMASlotmapConfirm; 599 | 600 | 601 | typedef struct 602 | { 603 | // set to RN_GET_TDMA_SLOTMAP_REQUEST 604 | rcrm_uint16_t msgType; 605 | // identifier to correlate requests with confirms 606 | rcrm_uint16_t msgId; 607 | } rnMsg_GetTDMASlotmapRequest; 608 | 609 | typedef struct 610 | { 611 | // set to RN_GET_TDMA_SLOTMAP_CONFIRM 612 | rcrm_uint16_t msgType; 613 | // identifier to correlate requests with confirms 614 | rcrm_uint16_t msgId; 615 | 616 | // Number of slots in this variable-length packet 617 | rcrm_uint8_t numSlots; 618 | 619 | rcrm_uint8_t reserved[3]; 620 | 621 | // status code 622 | rcrm_uint32_t status; 623 | 624 | // The slot definitions, plus durations computed by the radio. 625 | // Only the first numSlots are included in this variable-length message. 626 | rnTDMASlotWithComputedDuration slots[RN_MAX_TDMA_SLOTS]; 627 | } rnMsg_GetTDMASlotmapConfirm; 628 | 629 | typedef struct 630 | { 631 | // set to RN_GET_TDMA_SLOT_REQUEST 632 | rcrm_uint16_t msgType; 633 | // identifier to correlate requests with confirms 634 | rcrm_uint16_t msgId; 635 | 636 | // Zero-based index of the slot whose info is to be returned. 637 | rcrm_uint8_t slotNumber; 638 | 639 | // Reserved for alignment and future growth; set to zero. 640 | rcrm_uint8_t reserved[3]; 641 | } rnMsg_GetTDMASlotRequest; 642 | 643 | typedef struct 644 | { 645 | // set to RN_GET_TDMA_SLOT_CONFIRM 646 | rcrm_uint16_t msgType; 647 | // identifier to correlate requests with confirms 648 | rcrm_uint16_t msgId; 649 | 650 | // status code 651 | rcrm_uint32_t status; 652 | 653 | // The slot definition, plus duration computed by the radio. 654 | rnTDMASlotWithComputedDuration slot; 655 | } rnMsg_GetTDMASlotConfirm; 656 | 657 | typedef struct 658 | { 659 | // set to RN_SET_REQUEST_USER_DATA_REQUEST 660 | rcrm_uint16_t msgType; 661 | // identifier to correlate requests with confirms 662 | rcrm_uint16_t msgId; 663 | 664 | rcrm_uint16_t reserved; // alignment 665 | 666 | // Number of bytes of data to send in range request. 667 | // Can be up to RN_USER_DATA_LENGTH or maxRequestDataSize in the configuration, whichever is smaller. 668 | rcrm_uint16_t dataSize; 669 | 670 | // User data for payload. 671 | // Only the first dataSize bytes need to actually be in the UDP packet. 672 | rcrm_uint8_t data[RN_USER_DATA_LENGTH]; 673 | } rnMsg_SetRequestDataRequest; 674 | 675 | typedef struct 676 | { 677 | // set to RN_SET_REQUEST_USER_DATA_CONFIRM 678 | rcrm_uint16_t msgType; 679 | // identifier to correlate requests with confirms 680 | rcrm_uint16_t msgId; 681 | 682 | // status code 683 | rcrm_uint32_t status; 684 | } rnMsg_SetRequestDataConfirm; 685 | 686 | typedef struct 687 | { 688 | // set to RN_SET_RESPONSE_USER_DATA_REQUEST 689 | rcrm_uint16_t msgType; 690 | // identifier to correlate requests with confirms 691 | rcrm_uint16_t msgId; 692 | 693 | rcrm_uint16_t reserved; // alignment 694 | 695 | // Number of bytes of data to send in range response. 696 | // Can be up to RN_USER_DATA_LENGTH or maxResponseDataSize in the configuration, whichever is smaller. 697 | rcrm_uint16_t dataSize; 698 | 699 | // User data for payload. 700 | // Only the first dataSize bytes need to actually be in the UDP packet. 701 | rcrm_uint8_t data[RN_USER_DATA_LENGTH]; 702 | } rnMsg_SetResponseDataRequest; 703 | 704 | typedef struct 705 | { 706 | // set to RN_SET_RESPONSE_USER_DATA_CONFIRM 707 | rcrm_uint16_t msgType; 708 | // identifier to correlate requests with confirms 709 | rcrm_uint16_t msgId; 710 | 711 | // status code 712 | rcrm_uint32_t status; 713 | } rnMsg_SetResponseDataConfirm; 714 | 715 | typedef struct 716 | { 717 | // set to RN_GET_FULL_NEIGHBOR_DATABASE_REQUEST 718 | rcrm_uint16_t msgType; 719 | // identifier to correlate requests with confirms 720 | rcrm_uint16_t msgId; 721 | 722 | rcrm_uint8_t maxNeighborEntries; 723 | rcrm_uint8_t sortType; 724 | 725 | rcrm_uint16_t reserved; 726 | } rnMsg_GetFullNeighborDatabaseRequest; 727 | 728 | typedef struct 729 | { 730 | rcrm_uint32_t nodeId; 731 | 732 | rcrm_uint8_t rangeStatus; 733 | 734 | rcrm_uint8_t antennaMode; 735 | 736 | rcrm_uint16_t stopwatchTime; 737 | 738 | rcrm_uint32_t rangeMm; 739 | rcrm_uint16_t rangeErrorEstimate; 740 | 741 | rcrm_int16_t rangeVelocity; 742 | 743 | rcrm_uint8_t rangeMeasurementType; 744 | 745 | rcrm_uint8_t flags; 746 | 747 | rcrm_uint16_t ledFlags; 748 | 749 | rcrm_uint16_t noise; 750 | rcrm_uint16_t vPeak; 751 | 752 | rcrm_uint16_t statsNumRangeAttempts; 753 | rcrm_uint16_t statsNumRangeSuccesses; 754 | 755 | rcrm_uint32_t statsAgeMs; 756 | 757 | rcrm_uint32_t rangeUpdateTimestampMs; 758 | rcrm_uint32_t lastHeardTimestampMs; 759 | rcrm_uint32_t addedToNDBTimestampMs; 760 | } rnFullNeighborDatabaseEntry_t; 761 | 762 | typedef struct 763 | { 764 | // set to RN_GET_FULL_NEIGHBOR_DATABASE_CONFIRM 765 | rcrm_uint16_t msgType; 766 | // identifier to correlate requests with confirms 767 | rcrm_uint16_t msgId; 768 | 769 | // Number of neighbors in this database message 770 | rcrm_uint8_t numNeighborEntries; 771 | 772 | // Sort type used 773 | rcrm_uint8_t sortType; 774 | 775 | rcrm_uint16_t reserved; 776 | 777 | rcrm_uint32_t timestamp; 778 | 779 | rcrm_uint32_t status; 780 | 781 | rnFullNeighborDatabaseEntry_t neighbors[RN_FULL_NEIGHBOR_DATABASE_MAX_ENTRY_COUNT]; 782 | } rnMsg_GetFullNeighborDatabaseConfirm; 783 | 784 | typedef struct 785 | { 786 | // set to RN_GET_SMALL_NEIGHBOR_DATABASE_REQUEST 787 | rcrm_uint16_t msgType; 788 | // identifier to correlate requests with confirms 789 | rcrm_uint16_t msgId; 790 | 791 | rcrm_uint8_t maxNeighborEntries; 792 | rcrm_uint8_t sortType; 793 | 794 | rcrm_uint16_t reserved; 795 | } rnMsg_GetSmallNeighborDatabaseRequest; 796 | 797 | 798 | typedef struct 799 | { 800 | rcrm_uint32_t nodeId; 801 | 802 | rcrm_uint16_t rangeCm; 803 | rcrm_uint8_t rangeErrEst; 804 | 805 | rcrm_uint8_t reserved; 806 | 807 | rcrm_uint16_t ageMs; 808 | 809 | rcrm_uint8_t rangeMeasurementType; 810 | 811 | rcrm_uint8_t flags; 812 | } rnSmallNeighborDatabaseEntry_t; 813 | 814 | typedef struct 815 | { 816 | // set to RN_GET_SMALL_NEIGHBOR_DATABASE_CONFIRM 817 | rcrm_uint16_t msgType; 818 | // identifier to correlate requests with confirms 819 | rcrm_uint16_t msgId; 820 | 821 | rcrm_uint8_t numNeighborEntries; 822 | 823 | rcrm_uint8_t sortType; 824 | 825 | rcrm_uint16_t reserved; 826 | 827 | rnSmallNeighborDatabaseEntry_t neighbors[RN_SMALL_NEIGHBOR_DATABASE_MAX_ENTRY_COUNT]; 828 | } rnMsg_GetSmallNeighborDatabaseConfirm; 829 | 830 | 831 | typedef struct 832 | { 833 | // set to RN_SET_EXCLUDED_REQUEST 834 | rcrm_uint16_t msgType; 835 | // identifier to correlate requests with confirms 836 | rcrm_uint16_t msgId; 837 | 838 | rcrm_uint8_t numTargets; 839 | 840 | rcrm_uint8_t reserved[3]; 841 | 842 | // This is variable length, with a max length of RN_MAX_EXCLUDED_TARGETS of course. 843 | // Only the first numTargets need to be in the packet. 844 | rcrm_uint32_t excludedTargets[RN_MAX_EXCLUDED_TARGETS]; 845 | } rnMsg_SetExcludedRequest; 846 | 847 | typedef struct 848 | { 849 | // set to RN_SET_EXCLUDED_CONFIRM 850 | rcrm_uint16_t msgType; 851 | // identifier to correlate requests with confirms 852 | rcrm_uint16_t msgId; 853 | 854 | rcrm_uint32_t status; 855 | } rnMsg_SetExcludedConfirm; 856 | 857 | typedef struct 858 | { 859 | // set to RN_GET_EXCLUDED_REQUEST 860 | rcrm_uint16_t msgType; 861 | // identifier to correlate requests with confirms 862 | rcrm_uint16_t msgId; 863 | } rnMsg_GetExcludedRequest; 864 | 865 | typedef struct 866 | { 867 | // set to RN_GET_EXCLUDED_CONFIRM 868 | rcrm_uint16_t msgType; 869 | // identifier to correlate requests with confirms 870 | rcrm_uint16_t msgId; 871 | 872 | rcrm_uint8_t numTargets; 873 | 874 | rcrm_uint8_t reserved[3]; 875 | 876 | // This is variable length, with a max length of RN_MAX_EXCLUDED_TARGETS of course. 877 | // Only the first numTargets need to be in the packet. 878 | rcrm_uint32_t excludedTargets[RN_MAX_EXCLUDED_TARGETS]; 879 | } rnMsg_GetExcludedConfirm; 880 | 881 | typedef struct 882 | { 883 | // set to RN_GET_HEALTH_STATUS_REQUEST 884 | rcrm_uint16_t msgType; 885 | // identifier to correlate requests with confirms 886 | rcrm_uint16_t msgId; 887 | } rnMsg_GetHealthStatusRequest; 888 | 889 | typedef struct 890 | { 891 | // set to RN_GET_HEALTH_STATUS_CONFIRM 892 | rcrm_uint16_t msgType; 893 | // identifier to correlate requests with confirms 894 | rcrm_uint16_t msgId; 895 | 896 | // Divide this by 4 to get temperature in degrees C. 897 | rcrm_int32_t temperature; 898 | 899 | // Number of neighbors in the neighbor database. 900 | rcrm_uint32_t numNeighbors; 901 | 902 | // Time over which the following counters were accumulated 903 | rcrm_uint32_t statisticsTimeMs; 904 | 905 | rcrm_uint32_t numRangeAttempts; 906 | rcrm_uint32_t numPrecisionRangeSuccesses; 907 | rcrm_uint32_t numCoarseRangeEstimates; 908 | rcrm_uint32_t numRangeTimeouts; 909 | rcrm_uint32_t numRangeVCS; 910 | rcrm_uint32_t numRangeLEDFailure; 911 | rcrm_uint32_t numRangeCCIFailure; 912 | } rnMsg_GetHealthStatusConfirm; 913 | 914 | typedef struct 915 | { 916 | // set to RN_RESET_DATABASE_AND_STATS_REQUEST 917 | rcrm_uint16_t msgType; 918 | // identifier to correlate requests with confirms 919 | rcrm_uint16_t msgId; 920 | 921 | // Flags indicating which items to reset (entire neighbor database info, just the statistics, etc.) 922 | rcrm_uint32_t resetFlags; 923 | 924 | // Node ID of the neighbor to reset, or 0 to reset for all neighbors/globally 925 | rcrm_uint32_t nodeId; 926 | } rnMsg_ResetDatabaseAndStatsRequest; 927 | 928 | typedef struct 929 | { 930 | // set to RN_RESET_DATABASE_AND_STATS_CONFIRM 931 | rcrm_uint16_t msgType; 932 | // identifier to correlate requests with confirms 933 | rcrm_uint16_t msgId; 934 | 935 | rcrm_uint32_t status; 936 | } rnMsg_ResetDatabaseAndStatsConfirm; 937 | 938 | typedef struct 939 | { 940 | // set to RN_GET_REQUEST_USER_DATA_REQUEST 941 | rcrm_uint16_t msgType; 942 | // identifier to correlate requests with confirms 943 | rcrm_uint16_t msgId; 944 | } rnMsg_GetRequestDataRequest; 945 | 946 | typedef struct 947 | { 948 | // set to RN_GET_REQUEST_USER_DATA_CONFIRM 949 | rcrm_uint16_t msgType; 950 | // identifier to correlate requests with confirms 951 | rcrm_uint16_t msgId; 952 | 953 | rcrm_uint16_t reserved; // alignment 954 | 955 | // Number of bytes of data to send in range request. 956 | // Can be up to RN_USER_DATA_LENGTH or maxRequestDataSize in the configuration, whichever is smaller. 957 | rcrm_uint16_t dataSize; 958 | 959 | // User data for payload. 960 | // Only the first dataSize bytes need to actually be in the UDP packet. 961 | rcrm_uint8_t data[RN_USER_DATA_LENGTH]; 962 | } rnMsg_GetRequestDataConfirm; 963 | 964 | typedef struct 965 | { 966 | // set to RN_GET_RESPONSE_USER_DATA_REQUEST 967 | rcrm_uint16_t msgType; 968 | // identifier to correlate requests with confirms 969 | rcrm_uint16_t msgId; 970 | } rnMsg_GetResponseDataRequest; 971 | 972 | typedef struct 973 | { 974 | // set to RN_GET_RESPONSE_USER_DATA_CONFIRM 975 | rcrm_uint16_t msgType; 976 | // identifier to correlate requests with confirms 977 | rcrm_uint16_t msgId; 978 | 979 | rcrm_uint16_t reserved; // alignment 980 | 981 | // Number of bytes of data to send in range response. 982 | // Can be up to RN_USER_DATA_LENGTH or maxResponseDataSize in the configuration, whichever is smaller. 983 | rcrm_uint16_t dataSize; 984 | 985 | // User data for payload. 986 | // Only the first dataSize bytes need to actually be in the UDP packet. 987 | rcrm_uint8_t data[RN_USER_DATA_LENGTH]; 988 | } rnMsg_GetResponseDataConfirm; 989 | 990 | 991 | typedef struct 992 | { 993 | // set to RN_GET_PACKET_DURATIONS_REQUEST 994 | rcrm_uint16_t msgType; 995 | // identifier to correlate requests with confirms 996 | rcrm_uint16_t msgId; 997 | } rnMsg_GetPacketDurationsRequest; 998 | 999 | typedef struct 1000 | { 1001 | // set to RN_GET_PACKET_DURATIONS_CONFIRM 1002 | rcrm_uint16_t msgType; 1003 | // identifier to correlate requests with confirms 1004 | rcrm_uint16_t msgId; 1005 | 1006 | rcrm_uint32_t rangeReqNoDataMicroseconds; 1007 | rcrm_uint32_t rangeRespNoDataMicroseconds; 1008 | 1009 | rcrm_uint32_t rangeReqMaxDataMicroseconds; 1010 | rcrm_uint32_t rangeRespMaxDataMicroseconds; 1011 | 1012 | rcrm_uint32_t rangeConversationNoDataMicroseconds; 1013 | rcrm_uint32_t rangeConversationMaxDataMicroseconds; 1014 | 1015 | rcrm_uint32_t dataNoDataMicroseconds; 1016 | rcrm_uint32_t dataMaxDataMicroseconds; 1017 | } rnMsg_GetPacketDurationsConfirm; 1018 | 1019 | typedef struct 1020 | { 1021 | // set to RN_TIMING_INFO 1022 | rcrm_uint16_t msgType; 1023 | // identifier to correlate requests with confirms 1024 | rcrm_uint16_t msgId; 1025 | 1026 | // Transmitter node ID, if applicable. 1027 | rcrm_uint32_t srcNodeId; 1028 | 1029 | // Responder/Destination node ID, if applicable. 1030 | rcrm_uint32_t destNodeId; 1031 | 1032 | // Milliseconds since radio boot time 1033 | rcrm_uint32_t millisecondTimestamp; 1034 | 1035 | // Microsecond counter maintained by high-accuracy clock. 1036 | rcrm_uint32_t microsecondTimestamp; 1037 | 1038 | // Network clock in microseconds 1039 | rcrm_uint32_t microsecondNetworkClock; 1040 | 1041 | // Reserved for alignment future growth 1042 | rcrm_uint8_t reserved[2]; 1043 | 1044 | // RCRM_UWBPKTTYPE_RCMRANGEREQ, etc. 1045 | rcrm_uint8_t uwbPktType; 1046 | 1047 | // Zero-based index of the slot in the superframe 1048 | rcrm_uint8_t slotIndex; 1049 | 1050 | // Superframe count 1051 | rcrm_uint32_t superframeNumber; 1052 | } rnMsg_TimingInfo; 1053 | 1054 | #endif // #ifdef __rcrmHostInterfaceRN_h 1055 | 1056 | -------------------------------------------------------------------------------- /src/rcm.c: -------------------------------------------------------------------------------- 1 | //_____________________________________________________________________________ 2 | // 3 | // Copyright 2011-2015 Time Domain Corporation 4 | // 5 | // 6 | // rcm.c 7 | // 8 | // A collection of functions to communicate with an RCM. 9 | // 10 | // 11 | //_____________________________________________________________________________ 12 | 13 | 14 | //_____________________________________________________________________________ 15 | // 16 | // #includes 17 | //_____________________________________________________________________________ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #ifdef WIN32 24 | #include "winsock2.h" 25 | #else // linux 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #endif 32 | 33 | #include "rcmIf.h" 34 | #include "rcm.h" 35 | #include "rn.h" 36 | 37 | 38 | //_____________________________________________________________________________ 39 | // 40 | // #defines 41 | //_____________________________________________________________________________ 42 | 43 | 44 | //_____________________________________________________________________________ 45 | // 46 | // typedefs 47 | //_____________________________________________________________________________ 48 | 49 | typedef union 50 | { 51 | rcmMsg_FullRangeInfo rangeInfo; 52 | rcmMsg_DataInfo dataInfo; 53 | rnMsg_GetFullNeighborDatabaseConfirm ndbInfo; 54 | } infoMsgs_t; 55 | 56 | 57 | //_____________________________________________________________________________ 58 | // 59 | // static data 60 | //_____________________________________________________________________________ 61 | 62 | int msgIdCount; 63 | 64 | 65 | //_____________________________________________________________________________ 66 | // 67 | // Private function prototypes 68 | //_____________________________________________________________________________ 69 | 70 | 71 | 72 | //_____________________________________________________________________________ 73 | // 74 | // rcmBit - execute Built-In Test 75 | //_____________________________________________________________________________ 76 | 77 | int rcmBit(int *status) 78 | { 79 | rcrmMsg_BitRequest request; 80 | rcrmMsg_BitConfirm confirm; 81 | int retVal = ERR, numBytes, waitCount; 82 | 83 | // create request message 84 | request.msgType = htons(RCRM_BIT_REQUEST); 85 | request.msgId = htons(msgIdCount++); 86 | 87 | // make sure no pending messages 88 | rcmIfFlush(); 89 | 90 | // send message to RCM 91 | rcmIfSendPacket(&request, sizeof(request)); 92 | 93 | // wait for response 94 | 95 | // P440s have a longer timeout for BIT, so be prepared to wait longer. 96 | 97 | for(waitCount=0; waitCount<6; waitCount++) 98 | { 99 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 100 | 101 | // did we get a response from the RCM? 102 | if (numBytes == sizeof(confirm)) 103 | { 104 | // Handle byte ordering 105 | confirm.msgType = ntohs(confirm.msgType); 106 | confirm.status = ntohl(confirm.status); 107 | 108 | // is this the correct message type and is status good? 109 | if (confirm.msgType == RCRM_BIT_CONFIRM) 110 | { 111 | *status = confirm.status; 112 | retVal = OK; 113 | } 114 | 115 | break; 116 | } 117 | } 118 | 119 | return retVal; 120 | } 121 | 122 | 123 | //_____________________________________________________________________________ 124 | // 125 | // rcmConfigGet - get rcm configuration from radio 126 | //_____________________________________________________________________________ 127 | 128 | int rcmConfigGet(rcmConfiguration *config) 129 | { 130 | rcmMsg_GetConfigRequest request; 131 | rcmMsg_GetConfigConfirm confirm; 132 | int retVal = ERR, numBytes; 133 | 134 | // create request message 135 | request.msgType = htons(RCM_GET_CONFIG_REQUEST); 136 | request.msgId = htons(msgIdCount++); 137 | 138 | // make sure no pending messages 139 | rcmIfFlush(); 140 | 141 | // send message to RCM 142 | rcmIfSendPacket(&request, sizeof(request)); 143 | 144 | // wait for response 145 | numBytes = rcmIfGetPacket(&confirm, sizeof(rcmMsg_GetConfigConfirm)); 146 | 147 | // did we get a response from the RCM? 148 | if (numBytes == sizeof(rcmMsg_GetConfigConfirm)) 149 | { 150 | // Handle byte ordering 151 | confirm.msgType = ntohs(confirm.msgType); 152 | confirm.msgId = ntohs(confirm.msgId); 153 | 154 | // is this the correct message type? 155 | if (confirm.msgType == RCM_GET_CONFIG_CONFIRM) 156 | { 157 | // copy config from message to caller's structure 158 | memcpy(config, &confirm.config, sizeof(*config)); 159 | // Handle byte ordering 160 | config->nodeId = ntohl(config->nodeId); 161 | config->integrationIndex = ntohs(config->integrationIndex); 162 | config->electricalDelayPsA = ntohl(config->electricalDelayPsA); 163 | config->electricalDelayPsB = ntohl(config->electricalDelayPsB); 164 | config->flags = ntohs(config->flags); 165 | 166 | // milliseconds since radio boot 167 | confirm.timestamp = ntohl(confirm.timestamp); 168 | 169 | // status code 170 | confirm.status = ntohl(confirm.status); 171 | // only return OK if status is OK 172 | if (confirm.status == OK) 173 | retVal = OK; 174 | } 175 | } 176 | return retVal; 177 | } 178 | 179 | 180 | //_____________________________________________________________________________ 181 | // 182 | // rcmConfigSet - set RCM configuration in radio 183 | //_____________________________________________________________________________ 184 | 185 | int rcmConfigSet(rcmConfiguration *config) 186 | { 187 | rcmMsg_SetConfigRequest request; 188 | rcmMsg_SetConfigConfirm confirm; 189 | int retVal = ERR, numBytes; 190 | 191 | // create request message 192 | request.msgType = htons(RCM_SET_CONFIG_REQUEST); 193 | request.msgId = htons(msgIdCount++); 194 | memcpy(&request.config, config, sizeof(*config)); 195 | 196 | // Handle byte ordering in config struct 197 | request.config.nodeId = htonl(config->nodeId); 198 | request.config.integrationIndex = htons(config->integrationIndex); 199 | request.config.electricalDelayPsA = htonl(config->electricalDelayPsA); 200 | request.config.electricalDelayPsB = htonl(config->electricalDelayPsB); 201 | request.config.flags = htons(config->flags); 202 | 203 | // make sure no pending messages 204 | rcmIfFlush(); 205 | 206 | // send message to RCM 207 | rcmIfSendPacket(&request, sizeof(request)); 208 | 209 | // wait for response 210 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 211 | 212 | // did we get a response from the RCM? 213 | if (numBytes == sizeof(confirm)) 214 | { 215 | // Handle byte ordering 216 | confirm.msgType = ntohs(confirm.msgType); 217 | confirm.status = ntohl(confirm.status); 218 | 219 | // is this the correct message type and is status good? 220 | if (confirm.msgType == RCM_SET_CONFIG_CONFIRM && 221 | confirm.status == OK) 222 | retVal = OK; 223 | } 224 | return retVal; 225 | } 226 | 227 | 228 | //_____________________________________________________________________________ 229 | // 230 | // rcmOpModeSet - set RCM operational mode 231 | //_____________________________________________________________________________ 232 | 233 | int rcmOpModeSet(int opMode) 234 | { 235 | rcrmMsg_SetOpmodeRequest request; 236 | rcrmMsg_SetOpmodeConfirm confirm; 237 | int retVal = ERR, numBytes; 238 | 239 | // create request message 240 | request.msgType = htons(RCRM_SET_OPMODE_REQUEST); 241 | request.msgId = htons(msgIdCount++); 242 | request.opMode = htonl(opMode); 243 | 244 | // make sure no pending messages 245 | rcmIfFlush(); 246 | 247 | // send message to RCM 248 | rcmIfSendPacket(&request, sizeof(request)); 249 | 250 | // wait for response 251 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 252 | 253 | // did we get a response from the RCM? 254 | if (numBytes == sizeof(confirm)) 255 | { 256 | // Handle byte ordering 257 | confirm.msgType = ntohs(confirm.msgType); 258 | confirm.status = ntohl(confirm.status); 259 | 260 | // is this the correct message type and is status good? 261 | if (confirm.msgType == RCRM_SET_OPMODE_CONFIRM && 262 | confirm.status == OK) 263 | retVal = OK; 264 | } 265 | return retVal; 266 | } 267 | 268 | 269 | //_____________________________________________________________________________ 270 | // 271 | // rcmOpModeGet - set RCM operational mode 272 | //_____________________________________________________________________________ 273 | 274 | int rcmOpModeGet(int *opMode) 275 | { 276 | rcrmMsg_GetOpmodeRequest request; 277 | rcrmMsg_GetOpmodeConfirm confirm; 278 | int retVal = ERR, numBytes; 279 | 280 | // create request message 281 | request.msgType = htons(RCRM_GET_OPMODE_REQUEST); 282 | request.msgId = htons(msgIdCount++); 283 | 284 | // make sure no pending messages 285 | rcmIfFlush(); 286 | 287 | // send message to RCM 288 | rcmIfSendPacket(&request, sizeof(request)); 289 | 290 | // wait for response 291 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 292 | 293 | // did we get a response from the RCM? 294 | if (numBytes == sizeof(confirm)) 295 | { 296 | // Handle byte ordering 297 | confirm.msgType = ntohs(confirm.msgType); 298 | 299 | // is this the correct message type and is status good? 300 | if (confirm.msgType == RCRM_GET_OPMODE_CONFIRM) { 301 | *opMode = ntohl(confirm.opMode); 302 | retVal = OK; 303 | } 304 | } 305 | return retVal; 306 | } 307 | 308 | 309 | //_____________________________________________________________________________ 310 | // 311 | // rcmSleepModeSet - set RCM sleep mode 312 | //_____________________________________________________________________________ 313 | 314 | int rcmSleepModeSet(int sleepMode) 315 | { 316 | rcrmMsg_SetSleepModeRequest request; 317 | rcrmMsg_SetSleepModeConfirm confirm; 318 | int retVal = ERR, numBytes; 319 | 320 | // create request message 321 | request.msgType = htons(RCRM_SET_SLEEP_MODE_REQUEST); 322 | request.msgId = htons(msgIdCount++); 323 | request.sleepMode = htonl(sleepMode); 324 | 325 | // make sure no pending messages 326 | rcmIfFlush(); 327 | 328 | // send message to RCM 329 | rcmIfSendPacket(&request, sizeof(request)); 330 | 331 | // wait for response 332 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 333 | 334 | // did we get a response from the RCM? 335 | if (numBytes == sizeof(confirm)) 336 | { 337 | // Handle byte ordering 338 | confirm.msgType = ntohs(confirm.msgType); 339 | confirm.status = ntohl(confirm.status); 340 | 341 | // is this the correct message type and is status good? 342 | if (confirm.msgType == RCRM_SET_SLEEP_MODE_CONFIRM && 343 | confirm.status == OK) 344 | retVal = OK; 345 | } 346 | return retVal; 347 | } 348 | 349 | 350 | //_____________________________________________________________________________ 351 | // 352 | // rcmStatusInfoGet - retrieve RCM status from radio 353 | //_____________________________________________________________________________ 354 | 355 | int rcmStatusInfoGet(rcrmMsg_GetStatusInfoConfirm *confirm) 356 | { 357 | rcrmMsg_GetStatusInfoRequest request; 358 | int retVal = ERR, numBytes; 359 | 360 | // create request message 361 | request.msgType = htons(RCRM_GET_STATUS_INFO_REQUEST); 362 | request.msgId = htons(msgIdCount++); 363 | 364 | // make sure no pending messages 365 | rcmIfFlush(); 366 | 367 | // send message to RCM 368 | rcmIfSendPacket(&request, sizeof(request)); 369 | 370 | // wait for response 371 | numBytes = rcmIfGetPacket(confirm, sizeof(rcrmMsg_GetStatusInfoConfirm)); 372 | 373 | // did we get a response from the RCM? 374 | if (numBytes == sizeof(rcrmMsg_GetStatusInfoConfirm)) 375 | { 376 | // Handle byte ordering 377 | confirm->msgType = ntohs(confirm->msgType); 378 | confirm->msgId = ntohs(confirm->msgId); 379 | 380 | // is this the correct message type? 381 | if (confirm->msgType == RCRM_GET_STATUS_INFO_CONFIRM) 382 | { 383 | // Handle byte ordering 384 | confirm->appVersionBuild = ntohs(confirm->appVersionBuild); 385 | confirm->uwbKernelVersionBuild = ntohs(confirm->uwbKernelVersionBuild); 386 | confirm->serialNum = ntohl(confirm->serialNum); 387 | confirm->temperature = ntohl(confirm->temperature); 388 | 389 | // status code 390 | confirm->status = ntohl(confirm->status); 391 | // only return OK if status is OK 392 | if (confirm->status == OK) 393 | retVal = OK; 394 | } 395 | } 396 | return retVal; 397 | } 398 | 399 | 400 | //_____________________________________________________________________________ 401 | // 402 | // rcmInfoGet - range to another RCM module 403 | //_____________________________________________________________________________ 404 | 405 | int rcmInfoGet(rcmMsg_FullRangeInfo *rangeInfo, rcmMsg_DataInfo *dataInfo, rnMsg_GetFullNeighborDatabaseConfirm *ndbInfo) 406 | { 407 | infoMsgs_t infoMsgs; 408 | int retVal = ERR, numBytes; 409 | int i; 410 | 411 | // clear out caller's info structs 412 | memset(rangeInfo, 0, sizeof(*rangeInfo)); 413 | memset(dataInfo, 0, sizeof(*dataInfo)); 414 | memset(ndbInfo, 0, sizeof(*ndbInfo)); 415 | rangeInfo->rangeStatus = RCM_RANGE_STATUS_TIMEOUT; 416 | 417 | // Collect the info messages 418 | while ((numBytes = rcmIfGetPacket(&infoMsgs, sizeof(infoMsgs))) > 0) 419 | { 420 | // Based on the configuration setup, radio should only be able to receive 421 | // DATA_INFO messages as well as the RANGE_INFO or NDB_INFO message chosen 422 | switch(ntohs(infoMsgs.rangeInfo.msgType)) 423 | { 424 | case RCM_FULL_RANGE_INFO: 425 | // copy message to caller's struct 426 | memcpy(rangeInfo, &infoMsgs.rangeInfo, sizeof(*rangeInfo)); 427 | // handle byte ordering 428 | rangeInfo->msgType = ntohs(rangeInfo->msgType); 429 | rangeInfo->msgId = ntohs(rangeInfo->msgId); 430 | rangeInfo->responderId = ntohl(rangeInfo->responderId); 431 | rangeInfo->stopwatchTime = ntohs(rangeInfo->stopwatchTime); 432 | rangeInfo->precisionRangeMm = ntohl(rangeInfo->precisionRangeMm); 433 | rangeInfo->coarseRangeMm = ntohl(rangeInfo->coarseRangeMm); 434 | rangeInfo->filteredRangeMm = ntohl(rangeInfo->filteredRangeMm); 435 | rangeInfo->precisionRangeErrEst = ntohs(rangeInfo->precisionRangeErrEst); 436 | rangeInfo->coarseRangeErrEst = ntohs(rangeInfo->coarseRangeErrEst); 437 | rangeInfo->filteredRangeErrEst = ntohs(rangeInfo->filteredRangeErrEst); 438 | rangeInfo->filteredRangeVel = ntohs(rangeInfo->filteredRangeVel); 439 | rangeInfo->filteredRangeVelErrEst = ntohs(rangeInfo->filteredRangeVelErrEst); 440 | rangeInfo->reqLEDFlags = ntohs(rangeInfo->reqLEDFlags); 441 | rangeInfo->respLEDFlags = ntohs(rangeInfo->respLEDFlags); 442 | rangeInfo->noise = ntohs(rangeInfo->noise); 443 | rangeInfo->vPeak = ntohs(rangeInfo->vPeak); 444 | rangeInfo->coarseTOFInBins = ntohl(rangeInfo->coarseTOFInBins); 445 | rangeInfo->timestamp = ntohl(rangeInfo->timestamp); 446 | 447 | retVal = RANGEINFO; 448 | break; 449 | case RCM_DATA_INFO: 450 | // copy message to caller's struct 451 | memcpy(dataInfo, &infoMsgs.dataInfo, sizeof(*dataInfo)); 452 | // handle byte ordering 453 | dataInfo->msgType = ntohs(dataInfo->msgType); 454 | dataInfo->msgId = ntohs(dataInfo->msgId); 455 | dataInfo->sourceId = ntohl(dataInfo->sourceId); 456 | dataInfo->noise = ntohs(dataInfo->noise); 457 | dataInfo->vPeak = ntohs(dataInfo->vPeak); 458 | dataInfo->timestamp = ntohl(dataInfo->timestamp); 459 | dataInfo->dataSize = ntohs(dataInfo->dataSize); 460 | 461 | retVal = DATAINFO; 462 | break; 463 | case RN_FULL_NEIGHBOR_DATABASE_INFO: 464 | case RN_GET_FULL_NEIGHBOR_DATABASE_CONFIRM: 465 | // copy message to caller's struct 466 | memcpy(ndbInfo, &infoMsgs.ndbInfo, sizeof(*ndbInfo)); 467 | // handle byte ordering 468 | ndbInfo->msgType = ntohs(ndbInfo->msgType); 469 | ndbInfo->msgId = ntohs(ndbInfo->msgId); 470 | ndbInfo->timestamp = ntohl(ndbInfo->timestamp); 471 | ndbInfo->status = ntohl(ndbInfo->sortType); 472 | for (i = 0; i < ndbInfo->numNeighborEntries; i++) 473 | { 474 | ndbInfo->neighbors[i].nodeId = ntohl(ndbInfo->neighbors[i].nodeId); 475 | ndbInfo->neighbors[i].stopwatchTime = ntohs(ndbInfo->neighbors[i].stopwatchTime); 476 | ndbInfo->neighbors[i].rangeMm = ntohl(ndbInfo->neighbors[i].rangeMm); 477 | ndbInfo->neighbors[i].rangeErrorEstimate = ntohs(ndbInfo->neighbors[i].rangeErrorEstimate); 478 | ndbInfo->neighbors[i].rangeVelocity = ntohs(ndbInfo->neighbors[i].rangeVelocity); 479 | ndbInfo->neighbors[i].ledFlags = ntohs(ndbInfo->neighbors[i].ledFlags); 480 | ndbInfo->neighbors[i].noise = ntohs(ndbInfo->neighbors[i].noise); 481 | ndbInfo->neighbors[i].vPeak = ntohs(ndbInfo->neighbors[i].vPeak); 482 | ndbInfo->neighbors[i].statsNumRangeAttempts = ntohs(ndbInfo->neighbors[i].statsNumRangeAttempts); 483 | ndbInfo->neighbors[i].statsNumRangeSuccesses = ntohs(ndbInfo->neighbors[i].statsNumRangeSuccesses); 484 | ndbInfo->neighbors[i].statsAgeMs = ntohl(ndbInfo->neighbors[i].statsAgeMs); 485 | ndbInfo->neighbors[i].rangeUpdateTimestampMs = ntohl(ndbInfo->neighbors[i].rangeUpdateTimestampMs); 486 | ndbInfo->neighbors[i].lastHeardTimestampMs = ntohl(ndbInfo->neighbors[i].lastHeardTimestampMs); 487 | ndbInfo->neighbors[i].addedToNDBTimestampMs = ntohl(ndbInfo->neighbors[i].addedToNDBTimestampMs); 488 | } 489 | retVal = FULLNDB; 490 | break; 491 | default: 492 | printf("\nReceived Unknown Message Type: 0x%X\n", ntohs(infoMsgs.rangeInfo.msgType)); 493 | break; 494 | } 495 | // Keep processing unless we got desired message 496 | if (retVal > 0) 497 | break; 498 | } 499 | return retVal; 500 | } 501 | 502 | 503 | //_____________________________________________________________________________ 504 | // 505 | // rcmDataSend - broadcast a data-only packet 506 | //_____________________________________________________________________________ 507 | 508 | int rcmDataSend(int antennaMode, int dataSize, char *data) 509 | { 510 | rcmMsg_SendDataRequest request; 511 | rcmMsg_SendDataConfirm confirm; 512 | int retVal = ERR, numBytes; 513 | 514 | // create request message 515 | request.msgType = htons(RCM_SEND_DATA_REQUEST); 516 | request.msgId = htons(msgIdCount++); 517 | request.antennaMode = antennaMode; 518 | request.dataSize = htons(dataSize); 519 | // make sure there isn't too much data 520 | if (dataSize > RCM_USER_DATA_LENGTH) 521 | dataSize = RCM_USER_DATA_LENGTH; 522 | // copy data into message 523 | memcpy(request.data, data, dataSize); 524 | 525 | // make sure no pending messages 526 | rcmIfFlush(); 527 | 528 | // send message to RCM 529 | numBytes = sizeof(request) - RCM_USER_DATA_LENGTH + dataSize; 530 | rcmIfSendPacket(&request, sizeof(request)); 531 | 532 | // wait for response 533 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 534 | 535 | // did we get a response from the RCM? 536 | if (numBytes == sizeof(confirm)) 537 | { 538 | // Handle byte ordering 539 | confirm.msgType = ntohs(confirm.msgType); 540 | confirm.msgId = ntohs(confirm.msgId); 541 | 542 | // is this the correct message type? 543 | if (confirm.msgType == RCM_SEND_DATA_CONFIRM) 544 | { 545 | // status code 546 | confirm.status = ntohl(confirm.status); 547 | // only return OK if status is OK 548 | if (confirm.status == OK) 549 | { 550 | retVal = OK; 551 | } 552 | } 553 | } 554 | return retVal; 555 | 556 | } 557 | -------------------------------------------------------------------------------- /src/rcm.h: -------------------------------------------------------------------------------- 1 | //_____________________________________________________________________________ 2 | // 3 | // Copyright 2011-2 Time Domain Corporation 4 | // 5 | // 6 | // rcm.h 7 | // 8 | // Declarations for RCM communications functions. 9 | // 10 | //_____________________________________________________________________________ 11 | 12 | #ifndef __rcm_h 13 | #define __rcm_h 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | 19 | //_____________________________________________________________________________ 20 | // 21 | // #includes 22 | //_____________________________________________________________________________ 23 | 24 | // pull in message structure declarations 25 | #include "hostInterfaceRCM.h" 26 | #include "hostInterfaceRN.h" 27 | 28 | 29 | //_____________________________________________________________________________ 30 | // 31 | // #defines 32 | //_____________________________________________________________________________ 33 | 34 | #ifndef OK 35 | #define OK 0 36 | #define ERR (-1) 37 | #endif 38 | 39 | //_____________________________________________________________________________ 40 | // 41 | // typedefs 42 | //_____________________________________________________________________________ 43 | 44 | 45 | 46 | //_____________________________________________________________________________ 47 | // 48 | // Function prototypes 49 | //_____________________________________________________________________________ 50 | 51 | 52 | // 53 | // rcmBit 54 | // 55 | // Parameters: int *status - returned BIT status 56 | // Return: OK or ERR 57 | // 58 | // Sends RCM_BIT_REQUEST to radio and waits for RCM_BIT_CONFIRM. 59 | // If confirm message is received and BIT passes, returns OK. 60 | // If confirm message is not received or BIT fails, returns ERR. 61 | // 62 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 63 | int rcmBit(int *status); 64 | 65 | 66 | // 67 | // rcmConfigGet 68 | // 69 | // Parameters: rcmConfiguration *config - pointer to structure to hold 70 | // configuration 71 | // Return: OK or ERR 72 | // 73 | // Sends RCM_GET_CONFIG_REQUEST to radio and waits for RCM_GET_CONFIG_CONFIRM. 74 | // If confirm message is received, fills in config and returns OK. 75 | // If confirm message is not received, returns ERR. 76 | // 77 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 78 | int rcmConfigGet(rcmConfiguration *config); 79 | 80 | 81 | // 82 | // rcmConfigSet 83 | // 84 | // Parameters: rcmConfiguration *config - pointer to structure containing 85 | // new configuration 86 | // Return: OK or ERR 87 | // 88 | // Sends RCM_SET_CONFIG_REQUEST to radio and waits for RCM_SET_CONFIG_CONFIRM. 89 | // If confirm message is received, returns OK. 90 | // If confirm message is not received, returns ERR. 91 | // 92 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 93 | int rcmConfigSet(rcmConfiguration *config); 94 | 95 | 96 | // 97 | // rcmOpModeSet 98 | // 99 | // Parameters: int opMode - set to RCM_OPMODE_RCM 100 | // Return: OK or ERR 101 | // 102 | // Sends RCM_SET_OPMODE_REQUEST to radio and waits for RCM_SET_OPMODE_CONFIRM. 103 | // If confirm message is received, returns OK. 104 | // If confirm message is not received, returns ERR. 105 | // 106 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 107 | int rcmOpModeSet(int opMode); 108 | 109 | 110 | // 111 | // rcmOpModeGet 112 | // 113 | // Parameters: int opMode - set to RCM_OPMODE_RCM 114 | // Return: OK or ERR 115 | // 116 | // Sends RCM_SET_OPMODE_REQUEST to radio and waits for RCM_SET_OPMODE_CONFIRM. 117 | // If confirm message is received, returns OK. 118 | // If confirm message is not received, returns ERR. 119 | // 120 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 121 | int rcmOpModeGet(int *opMode); 122 | 123 | 124 | // 125 | // rcmSleepModeSet 126 | // 127 | // Parameters: int sleepMode - desired sleep mode 128 | // Return: OK or ERR 129 | // 130 | // Sends RCM_SET_SLEEP_MODE_REQUEST to radio and waits for 131 | // RCM_SET_SLEEP_MODE_CONFIRM. 132 | // If confirm message is received, returns OK. 133 | // If confirm message is not received, returns ERR. 134 | // 135 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 136 | int rcmSleepModeSet(int sleepMode); 137 | 138 | 139 | // 140 | // rcmStatusInfoGet 141 | // 142 | // Parameters: rcmMsg_GetStatusInfoConfirm *statusInfo - pointer to structure 143 | // to receive status info message 144 | // Return: OK or ERR 145 | // 146 | // Sends RCRM_GET_STATUS_INFO_REQUEST to radio and waits for 147 | // RCRM_GET_STATUS_INFO_CONFIRM. If confirm message is received, fills in 148 | // statusInfo and returns OK. If confirm message is not received, returns 149 | // ERR. 150 | // 151 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 152 | int rcmStatusInfoGet(rcrmMsg_GetStatusInfoConfirm *statusInfo); 153 | 154 | 155 | // 156 | // rcmRangeTo 157 | // 158 | // Parameters: int destNodeId - node ID of radio to range to 159 | // int antennaMode - antenna mode to use (see API for details) 160 | // int dataSize - number of bytes of data to send in range request 161 | // char *data - data to send in range request 162 | // rcmMsg_RangeInfo *rangeInfo - pointer to struct to hold range info 163 | // rcmMsg_DataInfo *dataInfo - pointer to struct to hold data info 164 | // rcmMsg_ScanInfo *scanInfo - pointer to struct to hold scan info 165 | // rcmMsg_FullScanInfo *fullScanInfo - pointer to struct to hold full scan info 166 | // Return: OK or ERR 167 | // 168 | // Sends RCM_SEND_RANGE_REQUEST to radio and waits for RCM_SEND_RANGE_CONFIRM. 169 | // If confirm message is received, receives up to 4 info messages and stores 170 | // them in rangeInfo, dataInfo, scanInfo, and fullScanInfo. The rangeStatus field in the 171 | // rangeInfo structure will indicate whether the range was successfully 172 | // calculated. If data was received in the range response, it will be in the 173 | // dataInfo structure and the dataSize field will be non-zero. If the radio is 174 | // configured to send scans (SEND_SCAN bit is set in config flags), the 175 | // scan samples will be in the scanInfo structure and the numSamples field 176 | // will be non-zero. if the radio is configured to send full scans (SEND_FULL_SCAN 177 | // bit is set in config flags, the scan samples will be in the fullScanInfo structure 178 | // and the numSamples field will be non-zero. 179 | // If confirm message is not received, returns ERR. 180 | // 181 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 182 | int rcmInfoGet(rcmMsg_FullRangeInfo *rangeInfo, rcmMsg_DataInfo *dataInfo, rnMsg_GetFullNeighborDatabaseConfirm *ndbInfo); 183 | 184 | // 185 | // rcmDataSend 186 | // 187 | // Parameters: int antennaMode - antenna mode to use (see API for details) 188 | // int dataSize - number of bytes of data to send 189 | // char *data - data to send 190 | // Return: OK or ERR 191 | // 192 | // Sends RCM_SEND_DATA_REQUEST to radio and waits for RCM_SEND_DATA_CONFIRM. 193 | // If confirm message is received, returns OK. 194 | // If confirm message is not received, returns ERR. 195 | // 196 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 197 | int rcmDataSend(int antennaMode, int dataSize, char *data); 198 | 199 | #ifdef __cplusplus 200 | } 201 | #endif 202 | 203 | 204 | #endif 205 | -------------------------------------------------------------------------------- /src/rcmIf.c: -------------------------------------------------------------------------------- 1 | //_____________________________________________________________________________ 2 | // 3 | // Copyright 2011-2015 Time Domain Corporation 4 | // 5 | // 6 | // rcmIf.c 7 | // 8 | // Functions to communicate over Ethernet, serial, or USB to an RCM 9 | // 10 | // 11 | //_____________________________________________________________________________ 12 | 13 | 14 | //_____________________________________________________________________________ 15 | // 16 | // #includes 17 | //_____________________________________________________________________________ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #ifdef WIN32 24 | #include "winsock2.h" 25 | #else // linux 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #endif 37 | 38 | #include "hostInterfaceCommon.h" 39 | #include "rcmIf.h" 40 | 41 | 42 | //_____________________________________________________________________________ 43 | // 44 | // #defines 45 | //_____________________________________________________________________________ 46 | 47 | #define DEFAULT_TIMEOUT_MS 500 48 | 49 | //_____________________________________________________________________________ 50 | // 51 | // static data 52 | //_____________________________________________________________________________ 53 | 54 | #ifdef WIN32 55 | static HANDLE hComm; 56 | #endif 57 | static int radioFd; 58 | static struct sockaddr_in radioAddr; 59 | static rcmIfType rcmIf; 60 | static int timeoutMs = DEFAULT_TIMEOUT_MS; 61 | 62 | //_____________________________________________________________________________ 63 | // 64 | // Private function prototypes 65 | //_____________________________________________________________________________ 66 | 67 | static int rcmIfGetPacketIp(void *pkt, unsigned maxSize); 68 | static int rcmIfGetPacketSerial(void *pkt, unsigned maxSize); 69 | static int rcmIfSendPacketIp(void *pkt, unsigned size); 70 | static int rcmIfSendPacketSerial(void *pkt, unsigned size); 71 | static unsigned short crc16(void *buf, int len); 72 | 73 | 74 | //_____________________________________________________________________________ 75 | // 76 | // rcmIfInit - perform initialization 77 | //_____________________________________________________________________________ 78 | 79 | int rcmIfInit(rcmIfType ifType, char *destAddr) 80 | { 81 | unsigned radioIpAddr; 82 | 83 | switch (ifType) 84 | { 85 | case rcmIfIp: 86 | #ifdef WIN32 87 | { 88 | // Initialize Windows sockets 89 | WSADATA wsad; 90 | memset(&wsad, 0, sizeof(wsad)); 91 | WSAStartup(MAKEWORD(2, 2), &wsad); 92 | } 93 | #endif 94 | 95 | // convert from string to binary 96 | radioIpAddr = inet_addr(destAddr); 97 | 98 | // make sure IP address is valid 99 | if (radioIpAddr == INADDR_NONE) 100 | { 101 | printf("Invalid IP address.\n"); 102 | return ERR; 103 | } 104 | 105 | // create UDP socket 106 | radioFd = (int)socket(AF_INET, SOCK_DGRAM, 0); 107 | if (radioFd == -1) 108 | { 109 | printf("Unable to open socket"); 110 | return ERR; 111 | } 112 | 113 | // initialize radio address structure 114 | memset(&radioAddr, 0, sizeof(radioAddr)); 115 | radioAddr.sin_family = AF_INET; 116 | radioAddr.sin_port = htons(RCRM_SOCKET_PORT_NUM); 117 | radioAddr.sin_addr.s_addr = radioIpAddr; 118 | break; 119 | 120 | case rcmIfSerial: 121 | case rcmIfUsb: 122 | #ifdef WIN32 123 | { 124 | DCB dcbSerialParams = {0}; 125 | wchar_t wcs[100]; 126 | char comPortStr[100]; 127 | 128 | // Windows requirement for COM ports above 9 129 | // but works for all 130 | sprintf(comPortStr, "\\\\.\\%s", destAddr); 131 | mbstowcs(wcs, comPortStr, sizeof(wcs)); 132 | hComm = CreateFile(wcs, 133 | GENERIC_READ | GENERIC_WRITE, 134 | 0, 135 | 0, 136 | OPEN_EXISTING, 137 | FILE_ATTRIBUTE_NORMAL, 138 | 0); 139 | if (hComm == INVALID_HANDLE_VALUE) 140 | { 141 | printf("Can't open serial port\n"); 142 | return ERR; 143 | } 144 | 145 | dcbSerialParams.DCBlength=sizeof(dcbSerialParams); 146 | GetCommState(hComm, &dcbSerialParams); 147 | dcbSerialParams.BaudRate=CBR_115200; 148 | dcbSerialParams.ByteSize=8; 149 | dcbSerialParams.StopBits=ONESTOPBIT; 150 | dcbSerialParams.Parity=NOPARITY; 151 | SetCommState(hComm, &dcbSerialParams); 152 | } 153 | #else 154 | { 155 | char portStr[100]; 156 | struct termios term; 157 | 158 | if(ifType == rcmIfUsb) 159 | { 160 | // P400s use this naming scheme 161 | //sprintf(portStr, "/dev/serial/by-id/usb-Time_Domain_P400_Radio_%s-if00", destAddr); 162 | 163 | // P440s use this naming scheme 164 | sprintf(portStr, "/dev/serial/by-id/usb-1027_Time_Domain_P440_%s-if00", destAddr); 165 | 166 | radioFd = open(portStr, O_RDWR|O_NOCTTY|O_NONBLOCK); 167 | } 168 | else 169 | { 170 | strcpy(portStr, destAddr); 171 | } 172 | 173 | radioFd = open(portStr, O_RDWR|O_NOCTTY|O_NONBLOCK); 174 | if (radioFd < 0) 175 | { 176 | printf("Can't open serial port: %s\n", portStr); 177 | return ERR; 178 | } 179 | tcgetattr(radioFd, &term); 180 | 181 | memset(&term, 0, sizeof(term)); 182 | cfmakeraw(&term); 183 | term.c_cflag = B115200 | CS8 | CLOCAL | CREAD; 184 | term.c_iflag = IGNPAR; 185 | 186 | tcflush(radioFd, TCIFLUSH); 187 | 188 | tcsetattr(radioFd, TCSANOW, &term); 189 | } 190 | #endif 191 | break; 192 | 193 | default: 194 | printf("Unknown interface type.\n"); 195 | exit(-1); 196 | break; 197 | } 198 | rcmIf = ifType; 199 | 200 | return OK; 201 | } 202 | 203 | 204 | //_____________________________________________________________________________ 205 | // 206 | // rcmIfClose - perform cleanup 207 | //_____________________________________________________________________________ 208 | 209 | void rcmIfClose(void) 210 | { 211 | switch (rcmIf) 212 | { 213 | case rcmIfIp: 214 | #ifdef WIN32 215 | // windows cleanup code 216 | closesocket(radioFd); 217 | WSACleanup(); 218 | #else 219 | // Linux cleanup code 220 | close(radioFd); 221 | #endif 222 | break; 223 | 224 | case rcmIfSerial: 225 | case rcmIfUsb: 226 | #ifdef WIN32 227 | CloseHandle(hComm); 228 | #else 229 | close(radioFd); 230 | #endif 231 | break; 232 | 233 | default: 234 | printf("Unknown interface type.\n"); 235 | exit(-1); 236 | break; 237 | } 238 | } 239 | 240 | 241 | //_____________________________________________________________________________ 242 | // 243 | // rcmIfGetPacket - return RCM packet 244 | // 245 | // Returns number of bytes received, ERR if timeout 246 | //_____________________________________________________________________________ 247 | 248 | int rcmIfGetPacket(void *pkt, unsigned maxSize) 249 | { 250 | if (rcmIf == rcmIfIp) 251 | return rcmIfGetPacketIp(pkt, maxSize); 252 | else 253 | return rcmIfGetPacketSerial(pkt, maxSize); 254 | } 255 | 256 | 257 | //_____________________________________________________________________________ 258 | // 259 | // rcmIfSendPacket - send packet out RCM interface 260 | // 261 | // Number of bytes sent, ERR on error. 262 | //_____________________________________________________________________________ 263 | 264 | int rcmIfSendPacket(void *pkt, unsigned size) 265 | { 266 | if (rcmIf == rcmIfIp) 267 | return rcmIfSendPacketIp(pkt, size); 268 | else 269 | return rcmIfSendPacketSerial(pkt, size); 270 | } 271 | 272 | 273 | //_____________________________________________________________________________ 274 | // 275 | // rcmIfFlush - flush any pending messages 276 | // 277 | //_____________________________________________________________________________ 278 | 279 | void rcmIfFlush(void) 280 | { 281 | int tmp, i = timeoutMs; 282 | 283 | timeoutMs = 0; 284 | while (rcmIfGetPacket(&tmp, sizeof(tmp)) > 0) 285 | ; 286 | timeoutMs = i; 287 | } 288 | 289 | 290 | //_____________________________________________________________________________ 291 | // 292 | // Private functions 293 | //_____________________________________________________________________________ 294 | 295 | 296 | //_____________________________________________________________________________ 297 | // 298 | // rcmIfGetPacketIp - read a packet from RCM using UDP 299 | // 300 | //_____________________________________________________________________________ 301 | 302 | int rcmIfGetPacketIp(void *pkt, unsigned maxSize) 303 | { 304 | fd_set fds; 305 | struct timeval tv; 306 | 307 | // basic select call setup 308 | FD_ZERO(&fds); 309 | FD_SET(radioFd, &fds); 310 | 311 | // Set up timeout 312 | tv.tv_sec = timeoutMs / 1000; 313 | tv.tv_usec = (timeoutMs * 1000) % 1000000; 314 | 315 | if (select(radioFd + 1, &fds, NULL, NULL, &tv) > 0) 316 | { 317 | // copy packet into buffer 318 | return recvfrom(radioFd, (char *)pkt, maxSize, 0, NULL, NULL); 319 | } 320 | 321 | // Timeout 322 | return ERR; 323 | } 324 | 325 | 326 | //_____________________________________________________________________________ 327 | // 328 | // rcmIfSendPacketIp - send a packet to the RCM using UDP 329 | // 330 | //_____________________________________________________________________________ 331 | 332 | int rcmIfSendPacketIp(void *pkt, unsigned size) 333 | { 334 | return sendto(radioFd, (const char *)pkt, size, 0, 335 | (struct sockaddr *)&radioAddr, sizeof(radioAddr)); 336 | } 337 | 338 | 339 | //_____________________________________________________________________________ 340 | // 341 | // serTimedRead - read from serial port with timeout 342 | // 343 | //_____________________________________________________________________________ 344 | 345 | int serTimedRead(void *buf, int cnt) 346 | { 347 | #ifdef WIN32 348 | DWORD dwBytes = 0; 349 | COMMTIMEOUTS timeouts={0}; 350 | 351 | if (timeoutMs == 0) 352 | { 353 | // windows way to specify zero timeout 354 | timeouts.ReadIntervalTimeout = MAXDWORD; 355 | timeouts.ReadTotalTimeoutConstant = 0; 356 | timeouts.ReadTotalTimeoutMultiplier = 0; 357 | } 358 | else 359 | { 360 | timeouts.ReadIntervalTimeout = timeoutMs; 361 | timeouts.ReadTotalTimeoutConstant = timeoutMs; 362 | timeouts.ReadTotalTimeoutMultiplier = 0; 363 | } 364 | timeouts.WriteTotalTimeoutConstant = timeoutMs; 365 | timeouts.WriteTotalTimeoutMultiplier = 0; 366 | 367 | SetCommTimeouts(hComm, &timeouts); 368 | 369 | if (!ReadFile(hComm, buf, cnt, &dwBytes, NULL)) 370 | return ERR; 371 | 372 | return dwBytes; 373 | #else 374 | fd_set fds; 375 | struct timeval tv; 376 | int total = 0, i; 377 | char *ptr = (char *)buf; 378 | 379 | while (cnt) 380 | { 381 | // basic select call setup 382 | FD_ZERO(&fds); 383 | FD_SET(radioFd, &fds); 384 | 385 | // Set up timeout 386 | tv.tv_sec = timeoutMs / 1000; 387 | tv.tv_usec = (timeoutMs * 1000) % 1000000; 388 | 389 | if (select(radioFd + 1, &fds, NULL, NULL, &tv) > 0) 390 | { 391 | i = read(radioFd, ptr, cnt); 392 | if (i > 0) 393 | { 394 | cnt -= i; 395 | ptr += i; 396 | total += i; 397 | } 398 | } 399 | else 400 | { 401 | total = -1; 402 | break; 403 | } 404 | } 405 | 406 | return total; 407 | #endif 408 | } 409 | 410 | 411 | //_____________________________________________________________________________ 412 | // 413 | // serWrite - write data to serial port 414 | // 415 | //_____________________________________________________________________________ 416 | 417 | int serWrite(void *buf, int cnt) 418 | { 419 | #ifdef WIN32 420 | DWORD dwBytes = 0; 421 | 422 | if (!WriteFile(hComm, buf, cnt, &dwBytes, NULL)) 423 | return ERR; 424 | 425 | return dwBytes; 426 | //error occurred. Report to user. 427 | #else 428 | return write(radioFd, buf, cnt); 429 | #endif 430 | } 431 | 432 | 433 | //_____________________________________________________________________________ 434 | // 435 | // rcmIfGetPacketSerial - read packet from serial port 436 | // 437 | //_____________________________________________________________________________ 438 | 439 | int rcmIfGetPacketSerial(void *pkt, unsigned maxSize) 440 | { 441 | int c=0, i, crc; 442 | unsigned short val; 443 | 444 | while (1) 445 | { 446 | // read first sync byte 447 | if (serTimedRead(&c, 1) <= 0) 448 | return ERR; 449 | if (c != 0xa5) 450 | continue; 451 | 452 | // read second sync byte 453 | if (serTimedRead(&c, 1) <= 0) 454 | return ERR; 455 | if (c != 0xa5) 456 | continue; 457 | 458 | break; 459 | } 460 | 461 | // read size 462 | if (serTimedRead(&val, sizeof(val)) <= 0) 463 | return ERR; 464 | val = ntohs(val); 465 | 466 | // read packet 467 | if (val > maxSize) 468 | val = maxSize; 469 | i = serTimedRead((char *)pkt, val); 470 | 471 | // read crc for serial port 472 | if (rcmIf == rcmIfSerial) 473 | { 474 | if (serTimedRead(&crc, 2) != 2) 475 | return ERR; 476 | crc = ntohs(crc); 477 | if (crc != crc16(pkt, val)) 478 | { 479 | printf("CRC error\n"); 480 | return ERR; 481 | } 482 | } 483 | return i; 484 | } 485 | 486 | 487 | //_____________________________________________________________________________ 488 | // 489 | // rcmIfSendPacketSerial - write packet to serial port 490 | // 491 | //_____________________________________________________________________________ 492 | 493 | int rcmIfSendPacketSerial(void *pkt, unsigned size) 494 | { 495 | int i; 496 | unsigned short val; 497 | 498 | // send sync bytes 499 | i = serWrite("\xa5\xa5", 2); 500 | if (i < 0) 501 | return ERR; 502 | 503 | // send size bytes 504 | val = htons(size); 505 | i = serWrite(&val, sizeof(val)); 506 | if (i < 0) 507 | return ERR; 508 | 509 | // send packet 510 | i = serWrite(pkt, size); 511 | if (i < 0) 512 | return ERR; 513 | 514 | // send CRC for serial port 515 | if (rcmIf == rcmIfSerial) 516 | { 517 | val = crc16(pkt, size); 518 | val = htons(val); 519 | if (serWrite(&val, sizeof(val)) < 0) 520 | return ERR; 521 | } 522 | return OK; 523 | } 524 | 525 | 526 | 527 | // Table of CRC constants - implements x^16+x^12+x^5+1 528 | static const unsigned short crc16_tab[] = { 529 | 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 530 | 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 531 | 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 532 | 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 533 | 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 534 | 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 535 | 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 536 | 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 537 | 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, 538 | 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 539 | 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 540 | 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 541 | 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 542 | 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 543 | 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 544 | 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 545 | 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 546 | 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 547 | 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 548 | 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 549 | 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 550 | 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 551 | 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 552 | 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 553 | 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 554 | 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 555 | 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 556 | 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 557 | 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 558 | 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 559 | 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 560 | 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0, 561 | }; 562 | 563 | 564 | //_____________________________________________________________________________ 565 | // 566 | // crc16 - calculate 16-bit CRC on buffer 567 | // 568 | //_____________________________________________________________________________ 569 | 570 | unsigned short crc16(void *ptr, int len) 571 | { 572 | int i; 573 | unsigned short cksum; 574 | unsigned char *buf = (unsigned char *)ptr; 575 | 576 | cksum = 0; 577 | for (i = 0; i < len; i++) { 578 | cksum = crc16_tab[((cksum>>8) ^ *buf++) & 0xFF] ^ (cksum << 8); 579 | } 580 | return cksum; 581 | } 582 | 583 | -------------------------------------------------------------------------------- /src/rcmIf.h: -------------------------------------------------------------------------------- 1 | //_____________________________________________________________________________ 2 | // 3 | // Copyright 2011-2 Time Domain Corporation 4 | // 5 | // 6 | // rcmIf.h 7 | // 8 | // Declarations for RCM interface functions. 9 | // 10 | //_____________________________________________________________________________ 11 | 12 | #ifndef __rcmIf_h 13 | #define __rcmIf_h 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | 19 | //_____________________________________________________________________________ 20 | // 21 | // #includes 22 | //_____________________________________________________________________________ 23 | 24 | 25 | 26 | //_____________________________________________________________________________ 27 | // 28 | // #defines 29 | //_____________________________________________________________________________ 30 | 31 | #ifndef OK 32 | #define OK 0 33 | #define ERR (-1) 34 | #endif 35 | 36 | #ifndef RANGEINFO 37 | #define RANGEINFO 1 38 | #endif 39 | 40 | #ifndef DATAINFO 41 | #define DATAINFO 2 42 | #endif 43 | 44 | #ifndef FULLNDB 45 | #define FULLNDB 3 46 | #endif 47 | 48 | //_____________________________________________________________________________ 49 | // 50 | // typedefs 51 | //_____________________________________________________________________________ 52 | 53 | 54 | typedef enum {rcmIfIp, rcmIfSerial, rcmIfUsb} rcmIfType; 55 | 56 | //_____________________________________________________________________________ 57 | // 58 | // Function prototypes 59 | //_____________________________________________________________________________ 60 | 61 | 62 | // 63 | // rcmIfInit 64 | // 65 | // Parameters: rcmIfType ifType - type of connection to RCM 66 | // char *destAddr - IP address or serial/USB port name 67 | // Return: OK or ERR 68 | // 69 | // Performs initialization necessary for particular type of interface. 70 | // Returns ERR on failure. 71 | // 72 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 73 | int rcmIfInit(rcmIfType ifType, char *destAddr); 74 | 75 | 76 | // 77 | // rcmIfClose 78 | // 79 | // Parameters: void 80 | // Return: void 81 | // 82 | // Closes socket or port to radio. 83 | // 84 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 85 | void rcmIfClose(void); 86 | 87 | 88 | // 89 | // rcmIfGetPacket 90 | // 91 | // Parameters: void *pkt - pointer to location to receive packet 92 | // unsigned maxSize - max size of packet to receive 93 | // Return: OK or ERR 94 | // 95 | // Reads from RCM interface until up to maxSize bytes have been received. 96 | // Returns ERR on read error, otherwise OK. 97 | // 98 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 99 | int rcmIfGetPacket(void *pkt, unsigned maxSize); 100 | 101 | 102 | // 103 | // rcmIfSendPacket 104 | // 105 | // Parameters: void *pkt - pointer to packet to send 106 | // unsigned size - size of packet to send 107 | // Return: OK or ERR 108 | // 109 | // Sends packet to RCM interface. 110 | // Returns ERR on write error, otherwise OK. 111 | // 112 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 113 | int rcmIfSendPacket(void *pkt, unsigned size); 114 | 115 | 116 | // 117 | // rcmIfFlush 118 | // 119 | // Parameters: void 120 | // 121 | // Return: void 122 | // 123 | // Flushes any unread packets. 124 | // 125 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 126 | void rcmIfFlush(void); 127 | 128 | #ifdef __cplusplus 129 | } 130 | #endif 131 | 132 | 133 | #endif 134 | -------------------------------------------------------------------------------- /src/rn.c: -------------------------------------------------------------------------------- 1 | //_____________________________________________________________________________ 2 | // 3 | // Copyright 2013-4 Time Domain Corporation 4 | // 5 | // 6 | // rcm.c 7 | // 8 | // A collection of functions to communicate with RangeNet on an RCM. 9 | // 10 | // 11 | //_____________________________________________________________________________ 12 | 13 | 14 | //_____________________________________________________________________________ 15 | // 16 | // #includes 17 | //_____________________________________________________________________________ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #ifdef WIN32 24 | #include "winsock2.h" 25 | #else // linux 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #endif 32 | 33 | #include "rcmIf.h" 34 | #include "rcm.h" 35 | #include "rn.h" 36 | 37 | 38 | //_____________________________________________________________________________ 39 | // 40 | // #defines 41 | //_____________________________________________________________________________ 42 | 43 | 44 | //_____________________________________________________________________________ 45 | // 46 | // typedefs 47 | //_____________________________________________________________________________ 48 | 49 | 50 | //_____________________________________________________________________________ 51 | // 52 | // static data 53 | //_____________________________________________________________________________ 54 | 55 | static int msgIdCount; 56 | 57 | 58 | //_____________________________________________________________________________ 59 | // 60 | // Private function prototypes 61 | //_____________________________________________________________________________ 62 | 63 | 64 | 65 | //_____________________________________________________________________________ 66 | // 67 | // rnConfigGet - get RangeNet base configuration from radio 68 | //_____________________________________________________________________________ 69 | 70 | int rnConfigGet(rnConfiguration *config) 71 | { 72 | rnMsg_GetConfigRequest request; 73 | rnMsg_GetConfigConfirm confirm; 74 | int retVal = ERR, numBytes; 75 | 76 | // create request message 77 | request.msgType = htons(RN_GET_CONFIG_REQUEST); 78 | request.msgId = htons(msgIdCount++); 79 | 80 | // make sure no pending messages 81 | rcmIfFlush(); 82 | 83 | // send message to RCM 84 | rcmIfSendPacket(&request, sizeof(request)); 85 | 86 | // wait for response 87 | numBytes = rcmIfGetPacket(&confirm, sizeof(rnMsg_GetConfigConfirm)); 88 | 89 | // did we get a response from the RCM? 90 | if (numBytes == sizeof(rnMsg_GetConfigConfirm)) 91 | { 92 | // Handle byte ordering 93 | confirm.msgType = ntohs(confirm.msgType); 94 | confirm.msgId = ntohs(confirm.msgId); 95 | 96 | // is this the correct message type? 97 | if (confirm.msgType == RN_GET_CONFIG_CONFIRM) 98 | { 99 | // copy config from message to caller's structure 100 | memcpy(config, &confirm.config, sizeof(*config)); 101 | 102 | // Handle byte ordering 103 | config->maxNeighborAgeMs = ntohl(config->maxNeighborAgeMs); 104 | config->autosendNeighborDbUpdateIntervalMs = ntohs(config->autosendNeighborDbUpdateIntervalMs); 105 | config->rnFlags = ntohs(config->rnFlags); 106 | config->defaultIfAddr1 = ntohl(config->defaultIfAddr1); 107 | config->defaultIfAddr2 = ntohl(config->defaultIfAddr2); 108 | 109 | // milliseconds since radio boot 110 | confirm.timestamp = ntohl(confirm.timestamp); 111 | 112 | // status code 113 | confirm.status = ntohl(confirm.status); 114 | 115 | // only return OK if status is OK 116 | if (confirm.status == OK) 117 | retVal = OK; 118 | } 119 | } 120 | return retVal; 121 | } 122 | 123 | 124 | //_____________________________________________________________________________ 125 | // 126 | // rnConfigSet - set RangeNet base configuration in radio 127 | //_____________________________________________________________________________ 128 | 129 | int rnConfigSet(rnConfiguration *config) 130 | { 131 | rnMsg_SetConfigRequest request; 132 | rnMsg_SetConfigConfirm confirm; 133 | int retVal = ERR, numBytes; 134 | 135 | // create request message 136 | request.msgType = htons(RN_SET_CONFIG_REQUEST); 137 | request.msgId = htons(msgIdCount++); 138 | memcpy(&request.config, config, sizeof(*config)); 139 | 140 | // Handle byte ordering in config struct 141 | request.config.maxNeighborAgeMs = htonl(config->maxNeighborAgeMs); 142 | request.config.autosendNeighborDbUpdateIntervalMs = htons(config->autosendNeighborDbUpdateIntervalMs); 143 | request.config.rnFlags = htons(config->rnFlags); 144 | request.config.defaultIfAddr1 = htonl(config->defaultIfAddr1); 145 | request.config.defaultIfAddr2 = htonl(config->defaultIfAddr2); 146 | request.persistFlag = 1; 147 | 148 | // make sure no pending messages 149 | rcmIfFlush(); 150 | 151 | // send message to RCM 152 | rcmIfSendPacket(&request, sizeof(request)); 153 | 154 | // wait for response 155 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 156 | 157 | // did we get a response from the RCM? 158 | if (numBytes == sizeof(confirm)) 159 | { 160 | // Handle byte ordering 161 | confirm.msgType = ntohs(confirm.msgType); 162 | confirm.status = ntohl(confirm.status); 163 | 164 | // is this the correct message type and is status good? 165 | if (confirm.msgType == RN_SET_CONFIG_CONFIRM && 166 | confirm.status == OK) 167 | retVal = OK; 168 | } 169 | return retVal; 170 | } 171 | 172 | 173 | //_____________________________________________________________________________ 174 | // 175 | // rnTdmaConfigGet - get RangeNet TDMA configuration from radio 176 | //_____________________________________________________________________________ 177 | 178 | int rnTdmaConfigGet(rnTDMAConfiguration *config) 179 | { 180 | rnMsg_GetTDMAConfigRequest request; 181 | rnMsg_GetTDMAConfigConfirm confirm; 182 | int retVal = ERR, numBytes; 183 | 184 | // create request message 185 | request.msgType = htons(RN_GET_TDMA_CONFIG_REQUEST); 186 | request.msgId = htons(msgIdCount++); 187 | 188 | // make sure no pending messages 189 | rcmIfFlush(); 190 | 191 | // send message to RCM 192 | rcmIfSendPacket(&request, sizeof(request)); 193 | 194 | // wait for response 195 | numBytes = rcmIfGetPacket(&confirm, sizeof(rnMsg_GetTDMAConfigConfirm)); 196 | 197 | // did we get a response from the RCM? 198 | if (numBytes == sizeof(rnMsg_GetTDMAConfigConfirm)) 199 | { 200 | // Handle byte ordering 201 | confirm.msgType = ntohs(confirm.msgType); 202 | confirm.msgId = ntohs(confirm.msgId); 203 | 204 | // is this the correct message type? 205 | if (confirm.msgType == RN_GET_TDMA_CONFIG_CONFIRM) 206 | { 207 | // copy config from message to caller's structure 208 | memcpy(config, &confirm.config, sizeof(*config)); 209 | 210 | // Handle byte ordering 211 | config->maxRequestDataSize = ntohs(config->maxRequestDataSize); 212 | config->maxResponseDataSize = ntohs(config->maxResponseDataSize); 213 | 214 | // status code 215 | confirm.status = ntohl(confirm.status); 216 | 217 | // only return OK if status is OK 218 | if (confirm.status == OK) 219 | retVal = OK; 220 | } 221 | } 222 | return retVal; 223 | } 224 | 225 | //_____________________________________________________________________________ 226 | // 227 | // rnTdmaConfigSet - set RangeNet TDMA configuration in radio 228 | //_____________________________________________________________________________ 229 | 230 | int rnTdmaConfigSet(rnTDMAConfiguration *config) 231 | { 232 | rnMsg_SetTDMAConfigRequest request; 233 | rnMsg_SetTDMAConfigConfirm confirm; 234 | int retVal = ERR, numBytes; 235 | 236 | // create request message 237 | request.msgType = htons(RN_SET_TDMA_CONFIG_REQUEST); 238 | request.msgId = htons(msgIdCount++); 239 | memcpy(&request.config, config, sizeof(*config)); 240 | 241 | // Handle byte ordering in config struct 242 | request.config.maxRequestDataSize = htons(config->maxRequestDataSize); 243 | request.config.maxResponseDataSize = htons(config->maxResponseDataSize); 244 | request.persistFlag = 1; 245 | 246 | // make sure no pending messages 247 | rcmIfFlush(); 248 | 249 | // send message to RCM 250 | rcmIfSendPacket(&request, sizeof(request)); 251 | 252 | // wait for response 253 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 254 | 255 | // did we get a response from the RCM? 256 | if (numBytes == sizeof(confirm)) 257 | { 258 | // Handle byte ordering 259 | confirm.msgType = ntohs(confirm.msgType); 260 | confirm.status = ntohl(confirm.status); 261 | 262 | // is this the correct message type and is status good? 263 | if (confirm.msgType == RN_SET_TDMA_CONFIG_CONFIRM && 264 | confirm.status == OK) 265 | retVal = OK; 266 | } 267 | return retVal; 268 | } 269 | 270 | //_____________________________________________________________________________ 271 | // 272 | // rnTdmaSlotMapGet - get TDMA Slotmap from radio 273 | //_____________________________________________________________________________ 274 | 275 | int rnTdmaSlotMapGet(rnMsg_GetTDMASlotmapConfirm *slotMap) 276 | { 277 | int MIN_CONFIRM_PACKET_SIZE = 12; 278 | 279 | rnMsg_GetTDMASlotmapRequest request; 280 | rnMsg_GetTDMASlotmapConfirm confirm; 281 | int retVal = ERR, numBytes; 282 | int i; 283 | 284 | // create request message 285 | request.msgType = htons(RN_GET_TDMA_SLOTMAP_REQUEST); 286 | request.msgId = htons(msgIdCount++); 287 | 288 | // make sure no pending messages 289 | rcmIfFlush(); 290 | 291 | // send message to RCM 292 | rcmIfSendPacket(&request, sizeof(request)); 293 | 294 | // wait for response 295 | numBytes = rcmIfGetPacket(&confirm, sizeof(rnMsg_GetTDMASlotmapConfirm)); 296 | 297 | // did we get a response from the RCM? 298 | if (numBytes >= MIN_CONFIRM_PACKET_SIZE) 299 | { 300 | // Handle byte ordering 301 | confirm.msgType = ntohs(confirm.msgType); 302 | confirm.msgId = ntohs(confirm.msgId); 303 | 304 | // is this the correct message type? 305 | if (confirm.msgType == RN_GET_TDMA_SLOTMAP_CONFIRM) 306 | { 307 | // Let's take care of the slot 308 | for (i = 0; i < confirm.numSlots; i++) 309 | { 310 | confirm.slots[i].minimumDurationMicroseconds = ntohl(confirm.slots[i].minimumDurationMicroseconds); 311 | confirm.slots[i].slot.flags = ntohs(confirm.slots[i].slot.flags); 312 | confirm.slots[i].slot.requesterId = ntohl(confirm.slots[i].slot.requesterId); 313 | confirm.slots[i].slot.responderId = ntohl(confirm.slots[i].slot.responderId); 314 | confirm.slots[i].slot.requestedDurationMicroseconds = ntohl(confirm.slots[i].slot.requestedDurationMicroseconds); 315 | } 316 | 317 | // status code 318 | confirm.status = ntohl(confirm.status); 319 | 320 | // only return OK if status is OK 321 | if (confirm.status == OK) 322 | { 323 | memcpy(slotMap, &confirm, sizeof(rnMsg_GetTDMASlotmapConfirm)); 324 | retVal = OK; 325 | } 326 | } 327 | } 328 | return retVal; 329 | } 330 | 331 | //_____________________________________________________________________________ 332 | // 333 | // rnTdmaSlotMapSet - set TDMA Slotmap in radio 334 | //_____________________________________________________________________________ 335 | 336 | int rnTdmaSlotMapSet(rnMsg_SetTDMASlotmapRequest *slotMap) 337 | { 338 | rnMsg_SetTDMASlotmapRequest request; 339 | rnMsg_SetTDMASlotmapConfirm confirm; 340 | int retVal = ERR, numBytes, i; 341 | 342 | // create request message 343 | request.msgType = htons(RN_SET_TDMA_SLOTMAP_REQUEST); 344 | request.msgId = htons(msgIdCount++); 345 | 346 | // Set message flags 347 | request.persistFlag = 1; 348 | request.slotmapFlags = 0; 349 | 350 | // Handle byte ordering for slotmap members 351 | request.numSlots = slotMap->numSlots; 352 | 353 | for (i = 0; i < request.numSlots; i++) 354 | { 355 | request.slots[i].antennaMode = slotMap->slots[i].antennaMode; 356 | request.slots[i].codeChannel = slotMap->slots[i].codeChannel; 357 | request.slots[i].flags = htons(slotMap->slots[i].flags); 358 | request.slots[i].integrationIndex = slotMap->slots[i].integrationIndex; 359 | request.slots[i].requestedDurationMicroseconds = htonl(slotMap->slots[i].requestedDurationMicroseconds); 360 | request.slots[i].requesterId = htonl(slotMap->slots[i].requesterId); 361 | request.slots[i].responderId = htonl(slotMap->slots[i].responderId); 362 | request.slots[i].slotNumber = slotMap->slots[i].slotNumber; 363 | request.slots[i].slotType = slotMap->slots[i].slotType; 364 | } 365 | 366 | // make sure no pending messages 367 | rcmIfFlush(); 368 | 369 | // send message to RCM 370 | rcmIfSendPacket(&request, sizeof(request)); 371 | 372 | // wait for response 373 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 374 | 375 | // did we get a response from the RCM? 376 | if (numBytes == sizeof(confirm)) 377 | { 378 | // Handle byte ordering 379 | confirm.msgType = ntohs(confirm.msgType); 380 | confirm.status = ntohl(confirm.status); 381 | 382 | // is this the correct message type and is status good? 383 | if (confirm.msgType == RN_SET_TDMA_SLOTMAP_CONFIRM && 384 | confirm.status == OK) 385 | retVal = OK; 386 | } 387 | 388 | return retVal; 389 | } 390 | 391 | //_____________________________________________________________________________ 392 | // 393 | // rnResetNDBStats - reset the statistics for each radio in the NDB 394 | //_____________________________________________________________________________ 395 | 396 | int rnResetNDBStats() 397 | { 398 | rnMsg_ResetDatabaseAndStatsRequest request; 399 | rnMsg_ResetDatabaseAndStatsConfirm confirm; 400 | int retVal = ERR, numBytes; 401 | 402 | // create request message 403 | request.msgType = htons(RN_RESET_DATABASE_AND_STATS_REQUEST); 404 | request.msgId = htons(msgIdCount++); 405 | 406 | // Handle byte ordering in config struct 407 | request.resetFlags = (1 << 2); 408 | request.resetFlags = htonl(request.resetFlags); 409 | request.nodeId = 0; 410 | 411 | // make sure no pending messages 412 | rcmIfFlush(); 413 | 414 | // send message to RCM 415 | rcmIfSendPacket(&request, sizeof(request)); 416 | 417 | // wait for response 418 | numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 419 | 420 | // did we get a response from the RCM? 421 | if (numBytes == sizeof(confirm)) 422 | { 423 | // Handle byte ordering 424 | confirm.msgType = ntohs(confirm.msgType); 425 | confirm.status = ntohl(confirm.status); 426 | 427 | // is this the correct message type and is status good? 428 | if (confirm.msgType == RN_RESET_DATABASE_AND_STATS_CONFIRM && 429 | confirm.status == OK) 430 | retVal = OK; 431 | } 432 | return retVal; 433 | } 434 | -------------------------------------------------------------------------------- /src/rn.h: -------------------------------------------------------------------------------- 1 | //_____________________________________________________________________________ 2 | // 3 | // Copyright 2011-2 Time Domain Corporation 4 | // 5 | // 6 | // rcm.h 7 | // 8 | // Declarations for RCM communications functions. 9 | // 10 | //_____________________________________________________________________________ 11 | 12 | #ifndef __rn_h 13 | #define __rn_h 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | 19 | //_____________________________________________________________________________ 20 | // 21 | // #includes 22 | //_____________________________________________________________________________ 23 | 24 | // pull in message structure declarations 25 | #include "hostInterfaceRN.h" 26 | 27 | 28 | //_____________________________________________________________________________ 29 | // 30 | // #defines 31 | //_____________________________________________________________________________ 32 | 33 | #ifndef OK 34 | #define OK 0 35 | #define ERR (-1) 36 | #endif 37 | 38 | //_____________________________________________________________________________ 39 | // 40 | // typedefs 41 | //_____________________________________________________________________________ 42 | 43 | 44 | 45 | //_____________________________________________________________________________ 46 | // 47 | // Function prototypes 48 | //_____________________________________________________________________________ 49 | 50 | 51 | // 52 | // rnConfigGet 53 | // 54 | // Parameters: rnConfiguration *config - pointer to structure to hold 55 | // configuration 56 | // Return: OK or ERR 57 | // 58 | // Sends RN_GET_CONFIG_REQUEST to radio and waits for RN_GET_CONFIG_CONFIRM. 59 | // If confirm message is received, fills in config and returns OK. 60 | // If confirm message is not received, returns ERR. 61 | // 62 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 63 | int rnConfigGet(rnConfiguration *config); 64 | 65 | 66 | // 67 | // rnConfigSet 68 | // 69 | // Parameters: rnConfiguration *config - pointer to structure containing 70 | // new configuration 71 | // Return: OK or ERR 72 | // 73 | // Sends RN_SET_CONFIG_REQUEST to radio and waits for RN_SET_CONFIG_CONFIRM. 74 | // If confirm message is received, returns OK. 75 | // If confirm message is not received, returns ERR. 76 | // 77 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 78 | int rnConfigSet(rnConfiguration *config); 79 | 80 | 81 | // 82 | // rnTdmaConfigGet 83 | // 84 | // Parameters: rnTDMAConfiguration *config - pointer to structure containing 85 | // new configuration 86 | // Return: OK or ERR 87 | // 88 | // Sends RN_GET_TDMA_CONFIG_REQUEST to radio and waits for RN_GET_TDMA_CONFIG_CONFIRM. 89 | // If confirm message is received, returns OK. 90 | // If confirm message is not received, returns ERR. 91 | // 92 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 93 | int rnTdmaConfigGet(rnTDMAConfiguration *config); 94 | 95 | 96 | // 97 | // rnTdmaConfigSet 98 | // 99 | // Parameters: rnTDMAConfiguration *config - pointer to structure to hold 100 | // configuration 101 | // Return: OK or ERR 102 | // 103 | // Sends RN_SET_TDMA_CONFIG_REQUEST to radio and waits for RN_SET_TDMA_CONFIG_CONFIRM. 104 | // If confirm message is received, returns OK. 105 | // If confirm message is not received, returns ERR. 106 | // 107 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 108 | int rnTdmaConfigSet(rnTDMAConfiguration *config); 109 | 110 | 111 | // 112 | // rnTdmaSlotMapGet 113 | // 114 | // Parameters: rnMsg_GetTDMASlotmapConfirm *config - pointer to structure containing 115 | // new configuration 116 | // Return: OK or ERR 117 | // 118 | // Sends RN_GET_TDMA_SLOTMAP_REQUEST to radio and waits for RN_GET_TDMA_SLOTMAP_CONFIRM. 119 | // If confirm message is received, returns OK. 120 | // If confirm message is not received, returns ERR. 121 | // 122 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 123 | int rnTdmaSlotMapGet(rnMsg_GetTDMASlotmapConfirm *slotMap); 124 | 125 | 126 | // 127 | // rnTdmaSlotMapSet 128 | // 129 | // Parameters: rnMsg_SetTDMASlotmapRequest *config - pointer to structure to hold 130 | // configuration 131 | // Return: OK or ERR 132 | // 133 | // Sends RN_SET_TDMA_SLOTMAP_REQUEST to radio and waits for RN_SET_TDMA_SLOTMAP_CONFIRM. 134 | // If confirm message is received, returns OK. 135 | // If confirm message is not received, returns ERR. 136 | // 137 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 138 | int rnTdmaSlotMapSet(rnMsg_SetTDMASlotmapRequest *slotMap); 139 | 140 | 141 | // 142 | // rnResetNDBStats 143 | // 144 | // Parameters: void 145 | // Return: OK or ERR 146 | // 147 | // Sends RN_RESET_DATABASE_AND_STATS_REQUEST to radio and waits for 148 | // RN_RESET_DATABASE_AND_STATS_CONFIRM. If confirm message is received, 149 | // returns OK. If confirm message is not received, returns ERR. 150 | // 151 | //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- 152 | int rnResetNDBStats(void); 153 | 154 | 155 | #ifdef __cplusplus 156 | } 157 | #endif 158 | 159 | 160 | #endif 161 | -------------------------------------------------------------------------------- /src/rtwtypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: rtwtypes.h 3 | * 4 | * MATLAB Coder version : 2.6 5 | * C/C++ source code generated on : 24-Mar-2016 13:18:21 6 | */ 7 | 8 | #ifndef __RTWTYPES_H__ 9 | #define __RTWTYPES_H__ 10 | #ifndef __TMWTYPES__ 11 | #define __TMWTYPES__ 12 | 13 | /*=======================================================================* 14 | * Target hardware information 15 | * Device type: ARM Compatible->ARM Cortex 16 | * Number of bits: char: 8 short: 16 int: 32 17 | * long: 32 long long: 64 18 | * native word size: 32 19 | * Byte ordering: LittleEndian 20 | * Signed integer division rounds to: Undefined 21 | * Shift right on a signed integer as arithmetic shift: on 22 | *=======================================================================*/ 23 | 24 | /*=======================================================================* 25 | * Fixed width word size data types: * 26 | * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * 27 | * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * 28 | * real32_T, real64_T - 32 and 64 bit floating point numbers * 29 | *=======================================================================*/ 30 | 31 | typedef signed char int8_T; 32 | typedef unsigned char uint8_T; 33 | typedef short int16_T; 34 | typedef unsigned short uint16_T; 35 | typedef int int32_T; 36 | typedef unsigned int uint32_T; 37 | typedef long long int64_T; 38 | typedef unsigned long long uint64_T; 39 | typedef float real32_T; 40 | typedef double real64_T; 41 | 42 | /*===========================================================================* 43 | * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * 44 | * ulong_T, ulonglong_T, char_T and byte_T. * 45 | *===========================================================================*/ 46 | 47 | typedef double real_T; 48 | typedef double time_T; 49 | typedef unsigned char boolean_T; 50 | typedef int int_T; 51 | typedef unsigned int uint_T; 52 | typedef unsigned long ulong_T; 53 | typedef unsigned long long ulonglong_T; 54 | typedef char char_T; 55 | typedef char_T byte_T; 56 | 57 | /*===========================================================================* 58 | * Complex number type definitions * 59 | *===========================================================================*/ 60 | #define CREAL_T 61 | typedef struct { 62 | real32_T re; 63 | real32_T im; 64 | } creal32_T; 65 | 66 | typedef struct { 67 | real64_T re; 68 | real64_T im; 69 | } creal64_T; 70 | 71 | typedef struct { 72 | real_T re; 73 | real_T im; 74 | } creal_T; 75 | 76 | typedef struct { 77 | int8_T re; 78 | int8_T im; 79 | } cint8_T; 80 | 81 | typedef struct { 82 | uint8_T re; 83 | uint8_T im; 84 | } cuint8_T; 85 | 86 | typedef struct { 87 | int16_T re; 88 | int16_T im; 89 | } cint16_T; 90 | 91 | typedef struct { 92 | uint16_T re; 93 | uint16_T im; 94 | } cuint16_T; 95 | 96 | typedef struct { 97 | int32_T re; 98 | int32_T im; 99 | } cint32_T; 100 | 101 | typedef struct { 102 | uint32_T re; 103 | uint32_T im; 104 | } cuint32_T; 105 | 106 | typedef struct { 107 | int64_T re; 108 | int64_T im; 109 | } cint64_T; 110 | 111 | typedef struct { 112 | uint64_T re; 113 | uint64_T im; 114 | } cuint64_T; 115 | 116 | 117 | /*=======================================================================* 118 | * Min and Max: * 119 | * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * 120 | * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * 121 | *=======================================================================*/ 122 | 123 | #define MAX_int8_T ((int8_T)(127)) 124 | #define MIN_int8_T ((int8_T)(-128)) 125 | #define MAX_uint8_T ((uint8_T)(255)) 126 | #define MIN_uint8_T ((uint8_T)(0)) 127 | #define MAX_int16_T ((int16_T)(32767)) 128 | #define MIN_int16_T ((int16_T)(-32768)) 129 | #define MAX_uint16_T ((uint16_T)(65535)) 130 | #define MIN_uint16_T ((uint16_T)(0)) 131 | #define MAX_int32_T ((int32_T)(2147483647)) 132 | #define MIN_int32_T ((int32_T)(-2147483647-1)) 133 | #define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) 134 | #define MIN_uint32_T ((uint32_T)(0)) 135 | #define MAX_int64_T ((int64_T)(9223372036854775807LL)) 136 | #define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) 137 | #define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) 138 | #define MIN_uint64_T ((uint64_T)(0ULL)) 139 | 140 | /* Logical type definitions */ 141 | #if !defined(__cplusplus) && !defined(__true_false_are_keywords) 142 | # ifndef false 143 | # define false (0U) 144 | # endif 145 | # ifndef true 146 | # define true (1U) 147 | # endif 148 | #endif 149 | 150 | /* 151 | * Maximum length of a MATLAB identifier (function/variable) 152 | * including the null-termination character. Referenced by 153 | * rt_logging.c and rt_matrx.c. 154 | */ 155 | #define TMW_NAME_LENGTH_MAX 64 156 | 157 | #endif 158 | #endif 159 | /* 160 | * File trailer for rtwtypes.h 161 | * 162 | * [EOF] 163 | */ 164 | -------------------------------------------------------------------------------- /src/uwb_driver.cpp: -------------------------------------------------------------------------------- 1 | //_____________________________________________________________________________ 2 | // 3 | // #includes 4 | //_____________________________________________________________________________ 5 | 6 | //#define ROS_MASTER_URI "http://localhost:11311" 7 | //#define ROS_ROOT "/opt/ros/indigo/share/ros" 8 | #include 9 | #include "stdio.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "rcmIf.h" 27 | #include "rcm.h" 28 | #include "rn.h" 29 | #include "hostInterfaceCommon.h" 30 | #include "hostInterfaceRCM.h" 31 | #include "hostInterfaceRN.h" 32 | #include 33 | #include 34 | 35 | //For log file use 36 | #include 37 | #include 38 | 39 | #include "ros/ros.h" 40 | #include "time.h" 41 | #include 42 | 43 | #include "uwb_driver/UwbRange.h" //self-defined msg file to inform of a range info package from p4xx 44 | #include "uwb_driver/UwbData.h" //self-defined msg file to inform of a data info package from p4xx 45 | 46 | //Services headers 47 | #include "uwb_driver/uwbModeConfig.h" //Mode selection service 48 | #include "uwb_driver/uwbRangeComm.h" 49 | #include "srv_return_verbal.h" 50 | 51 | //Force configuration for nodes ID 52 | #include 53 | 54 | //_____________________________________________________________________________ 55 | // 56 | // Debugging utilities 57 | //_____________________________________________________________________________ 58 | 59 | #define KNRM "\x1B[0m" 60 | #define KRED "\x1B[31m" 61 | #define KGRN "\x1B[32m" 62 | #define KYEL "\x1B[33m" 63 | #define KBLU "\x1B[34m" 64 | #define KMAG "\x1B[35m" 65 | #define KCYN "\x1B[36m" 66 | #define KWHT "\x1B[37m" 67 | #define RESET "\033[0m" 68 | 69 | #define DFLT_NODE_RATE 40 70 | 71 | //_____________________________________________________________________________ 72 | // 73 | // P4xx Serial Communications 74 | //_____________________________________________________________________________ 75 | #define DFLT_P4xx_PORT "/dev/ttyACM0" 76 | #define P4XX_ITF_SERIAL 0 77 | #define P4XX_ITF_USB 1 78 | #define P4XX_ITF_IP 2 79 | 80 | #define MODE_RCM 0 81 | #define MODE_RN 1 82 | #define MODE_SLUMBER 2 83 | 84 | extern int msgIdCount; 85 | 86 | int status; 87 | rcmIfType rcmIf; 88 | rcmConfiguration rcmConfig; 89 | rnConfiguration rnInitConfig; 90 | rnTDMAConfiguration rnTDMAConfig; 91 | rnMsg_GetTDMASlotmapConfirm rnInitTdmaSlotMap; 92 | rnMsg_SetTDMASlotmapRequest initTdmaSlotMapSet; 93 | // Info message structures 94 | rcmMsg_FullRangeInfo rangeInfo; 95 | rnMsg_GetFullNeighborDatabaseConfirm ndbInfo; 96 | rcmMsg_DataInfo dataInfo; 97 | uint8_t uwb_msg[RN_USER_DATA_LENGTH]; 98 | 99 | //To search for the device name 100 | glob_t glob_results; 101 | 102 | //_____________________________________________________________________________ 103 | // 104 | // ID Management 105 | //_____________________________________________________________________________ 106 | std::vector nodesId; 107 | uint8_t nodesTotal = 0; 108 | std::vectornodesPos; 109 | static int32_t hostedP4xxId = -1; 110 | static int32_t hostedP4xxIdx = -1; 111 | int commonId = -1; 112 | int commonIdx = -1; 113 | 114 | int p4xxCurrMode = MODE_RCM; 115 | int p4xxMode = MODE_RCM; 116 | bool slumberTime = false; 117 | 118 | /*------------------------------------------------------Function prototypes---------------------------------------------*/ 119 | bool uwb_mode_config(uwb_driver::uwbModeConfig::Request &req, uwb_driver::uwbModeConfig::Response &res) 120 | { 121 | ros::Time startTime = ros::Time::now(); 122 | int success = -1; 123 | 124 | //If mode is 2 we have to close the node 125 | if(req.p4xxmode == MODE_SLUMBER) 126 | { 127 | slumberTime = true; 128 | return true; 129 | } 130 | 131 | // Set the mode with 1 second time out 132 | while(ros::ok() && (ros::Time::now() - startTime).toSec() < 2) 133 | { 134 | success = -1; 135 | if(req.p4xxmode == MODE_RCM) 136 | success = rcmOpModeSet(RCRM_OPMODE_RCM); 137 | else if(req.p4xxmode == MODE_RN) 138 | success = rcmOpModeSet(RCRM_OPMODE_RN); 139 | else 140 | { 141 | ROS_INFO("There's no mode %d\n", req.p4xxmode); 142 | return true; 143 | } 144 | 145 | if (success != OK) 146 | ROS_INFO("Time out waiting for opmode confirm. Retrying...\n"); 147 | else 148 | { 149 | ROS_INFO(KGRN "Mode %d confirmed!" RESET, req.p4xxmode); 150 | res.result = P4XX_CONFIRMED; 151 | p4xxCurrMode = req.p4xxmode; 152 | return true; 153 | } 154 | } 155 | 156 | if(success != OK) 157 | { 158 | ROS_INFO(KRED "Time out to get confirm!" RESET); 159 | res.result = P4XX_CONFIRM_TIMEOUT; 160 | } 161 | 162 | return true; 163 | } 164 | 165 | bool uwb_range_comm(uwb_driver::uwbRangeComm::Request &req, uwb_driver::uwbRangeComm::Response &res) 166 | { 167 | switch(req.action) 168 | { 169 | case RANGE_RQST: 170 | { 171 | rcmMsg_SendRangeRequest request; 172 | rcmMsg_SendRangeRequestConfirm confirm; 173 | 174 | // create request message 175 | request.msgType = htons(RCM_SEND_RANGE_REQUEST); 176 | request.msgId = htons(msgIdCount++); 177 | request.responderId = htonl(req.responder); 178 | request.antennaMode = req.antenna; 179 | request.dataSize = htons(0); 180 | 181 | // make sure no pending messages 182 | rcmIfFlush(); 183 | 184 | // send request to RCM 185 | rcmIfSendPacket(&request, sizeof(request)); 186 | 187 | // wait for response 188 | int numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 189 | 190 | // did we get a response from the RCM? 191 | if (numBytes == sizeof(confirm)) 192 | { 193 | // Handle byte ordering 194 | confirm.msgType = ntohs(confirm.msgType); 195 | confirm.status = ntohs(confirm.status); 196 | 197 | // is this the correct message type? 198 | if(confirm.msgType == RCM_SEND_RANGE_REQUEST_CONFIRM && confirm.status == OK) 199 | { 200 | res.result = P4XX_CONFIRMED; 201 | ROS_INFO(KGRN "Range request confirmed by P4xx." RESET); 202 | } 203 | else 204 | { 205 | res.result = P4XX_CONFIRM_TIMEOUT; 206 | ROS_INFO(KYEL "Range request error. Message Type: 0x%0x, Status: 0x%0x." RESET, confirm.msgType, confirm.status); 207 | } 208 | 209 | } 210 | else 211 | { 212 | res.result = P4XX_CONFIRM_TIMEOUT; 213 | ROS_INFO(KYEL "Confirm message not properly received." RESET); 214 | } 215 | 216 | break; 217 | } 218 | case SEND_DATA: 219 | { 220 | rcmMsg_SendDataRequest request; 221 | rcmMsg_SendDataConfirm confirm; 222 | 223 | int dataSize = req.data_size; 224 | 225 | // create request message 226 | request.msgType = htons(RCM_SEND_DATA_REQUEST); 227 | request.msgId = htons(msgIdCount++); 228 | request.antennaMode = req.antenna; 229 | request.dataSize = htons(dataSize); 230 | // make sure there isn't too much data 231 | if (dataSize > RCM_USER_DATA_LENGTH) 232 | dataSize = RCM_USER_DATA_LENGTH; 233 | 234 | // copy data into message 235 | memcpy(request.data, &req.data[0], dataSize); 236 | 237 | // make sure no pending messages 238 | rcmIfFlush(); 239 | 240 | // send message to RCM 241 | rcmIfSendPacket(&request, sizeof(request)); 242 | 243 | // wait for response 244 | int numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 245 | 246 | // did we get a response from the RCM? 247 | if (numBytes == sizeof(confirm)) 248 | { 249 | // Handle byte ordering 250 | confirm.msgType = ntohs(confirm.msgType); 251 | confirm.status = ntohs(confirm.status); 252 | 253 | // is this the correct message type? 254 | if (confirm.msgType == RCM_SEND_DATA_CONFIRM && confirm.status == OK) 255 | { 256 | res.result = P4XX_CONFIRMED; 257 | ROS_INFO(KGRN "Data broadcast confirmed by P4xx." RESET); 258 | } 259 | else 260 | { 261 | res.result = P4XX_CONFIRM_TIMEOUT; 262 | ROS_INFO(KYEL "Data broadcast error. Message Type: 0x%0x, Status: 0x%0x." RESET, confirm.msgType, confirm.status); 263 | } 264 | } 265 | else 266 | { 267 | res.result = P4XX_CONFIRM_TIMEOUT; 268 | ROS_INFO(KYEL "Confirm message not properly received." RESET); 269 | } 270 | 271 | break; 272 | } 273 | case RANGE_RQST_DATA: 274 | { 275 | rcmMsg_SendChannelizedRangeRequest request; 276 | rcmMsg_SendChannelizedRangeRequestConfirm confirm; 277 | 278 | int dataSize = req.data_size; 279 | 280 | // create request message 281 | request.msgType = htons(RCM_SEND_CHANNELIZED_RANGE_REQUEST); 282 | request.msgId = htons(msgIdCount); 283 | request.responderId = htonl(req.responder); 284 | request.antennaMode = req.antenna; 285 | request.codeChannel = req.channel; 286 | request.dataSize = htons(dataSize); 287 | 288 | memcpy(request.data, &req.data[0], dataSize); 289 | 290 | // make sure no pending messages 291 | rcmIfFlush(); 292 | 293 | // send message to RCM 294 | rcmIfSendPacket(&request, sizeof(request)); 295 | 296 | // wait for response 297 | int numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 298 | 299 | // did we get a response from the RCM? 300 | if (numBytes == sizeof(confirm)) 301 | { 302 | // Handle byte ordering 303 | confirm.msgType = ntohs(confirm.msgType); 304 | confirm.status = ntohs(confirm.status); 305 | 306 | // is this the correct message type? 307 | if (confirm.msgType == RCM_SEND_CHANNELIZED_RANGE_REQUEST_CONFIRM && confirm.status == OK) 308 | { 309 | res.result = P4XX_CONFIRMED; 310 | ROS_INFO(KGRN "Range + Data request confirmed by P4xx." RESET); 311 | } 312 | else 313 | { 314 | res.result = P4XX_CONFIRM_TIMEOUT; 315 | ROS_INFO(KYEL "Channelized range + data broadcast error. Message Type: 0x%0x, Status: 0x%0x." RESET, confirm.msgType, confirm.status); 316 | } 317 | 318 | } 319 | else 320 | { 321 | res.result = P4XX_CONFIRM_TIMEOUT; 322 | ROS_INFO(KYEL "Confirm message not properly received." RESET); 323 | } 324 | 325 | break; 326 | } 327 | case SET_RQST_DATA: 328 | { 329 | rnMsg_SetRequestDataRequest request; 330 | rnMsg_SetRequestDataConfirm confirm; 331 | 332 | int dataSize = req.data_size; 333 | 334 | // create request message 335 | request.msgType = htons(RN_SET_REQUEST_USER_DATA_REQUEST); 336 | request.msgId = htons(msgIdCount++); 337 | request.dataSize = htons(dataSize); 338 | 339 | memcpy(request.data, &req.data[0], dataSize); 340 | rcmIfFlush(); 341 | 342 | // send message to RCM 343 | rcmIfSendPacket(&request, sizeof(request)); 344 | 345 | // wait for response 346 | int numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 347 | 348 | // did we get a response from the RCM? 349 | if (numBytes == sizeof(rnMsg_SetRequestDataConfirm)) 350 | { 351 | // Handle byte ordering 352 | confirm.msgType = ntohs(confirm.msgType); 353 | confirm.msgId = ntohs(confirm.msgId); 354 | 355 | // is this the correct message type? 356 | if (confirm.msgType == RN_SET_REQUEST_USER_DATA_CONFIRM && confirm.status == OK) 357 | { 358 | res.result = P4XX_CONFIRMED; 359 | ROS_INFO(KGRN "Set request data confirmed by P4xx." RESET); 360 | } 361 | else 362 | { 363 | res.result = P4XX_CONFIRM_TIMEOUT; 364 | ROS_INFO(KYEL "Set request data error. Message Type: 0x%0x, Status: 0x%0x." RESET, confirm.msgType, confirm.status); 365 | } 366 | } 367 | else 368 | { 369 | res.result = P4XX_CONFIRM_TIMEOUT; 370 | ROS_INFO(KYEL "Confirm message not properly received!" RESET); 371 | } 372 | break; 373 | 374 | } 375 | case SET_RSPD_DATA: 376 | { 377 | rnMsg_SetResponseDataRequest request; 378 | rnMsg_SetResponseDataConfirm confirm; 379 | 380 | int dataSize = req.data_size; 381 | 382 | // create request message 383 | request.msgType = htons(RN_SET_RESPONSE_USER_DATA_REQUEST); 384 | request.msgId = htons(msgIdCount++); 385 | request.dataSize = htons(dataSize); 386 | 387 | memcpy(request.data, &req.data[0], dataSize); 388 | rcmIfFlush(); 389 | 390 | // send message to RCM 391 | rcmIfSendPacket(&request, sizeof(request)); 392 | 393 | // wait for response 394 | int numBytes = rcmIfGetPacket(&confirm, sizeof(confirm)); 395 | 396 | // did we get a response from the RCM? 397 | if (numBytes == sizeof(rnMsg_SetResponseDataConfirm)) 398 | { 399 | // Handle byte ordering 400 | confirm.msgType = ntohs(confirm.msgType); 401 | confirm.msgId = ntohs(confirm.msgId); 402 | 403 | // is this the correct message type? 404 | if (confirm.msgType == RN_SET_RESPONSE_USER_DATA_CONFIRM && confirm.status == 0) 405 | { 406 | res.result = P4XX_CONFIRMED; 407 | ROS_INFO(KGRN "Set response data confirmed by P4xx." RESET); 408 | } 409 | else 410 | { 411 | res.result = P4XX_CONFIRM_TIMEOUT; 412 | ROS_INFO(KYEL "Set request data error. Message Type: 0x%0x, Status: 0x%0x." RESET, confirm.msgType, confirm.status); 413 | } 414 | } 415 | else 416 | { 417 | res.result = P4XX_CONFIRM_TIMEOUT; 418 | ROS_INFO(KYEL "Confirm message not received!" RESET); 419 | } 420 | break; 421 | 422 | } 423 | default: 424 | break; 425 | } 426 | 427 | return true; 428 | } 429 | 430 | 431 | /*------------------------------------------------------Function prototypes---------------------------------------------*/ 432 | 433 | //_____________________________________________________________________________ 434 | // 435 | // main function 436 | //_____________________________________________________________________________ 437 | using namespace std; 438 | 439 | int main(int argc, char *argv[]) 440 | { 441 | //Create ros handler to node 442 | ros::init(argc, argv, "uwb_driver"); 443 | ros::NodeHandle uwbDriverNodeHandle("~"); 444 | ros::Publisher uwb_range_publisher; 445 | ros::Publisher uwb_data_publisher; 446 | 447 | /*-----------------------------------------------Mischelaneous-----------------------------------------------------------*/ 448 | bool restEnable = true; 449 | if(uwbDriverNodeHandle.getParam("restEnable", restEnable)) 450 | { 451 | if(restEnable) 452 | printf(KBLU "UWB node allowed to sleep!\n" RESET); 453 | else 454 | printf(KBLU "Not resting at all!\n" RESET); 455 | } 456 | else 457 | { 458 | printf(KRED "Couldn't retrieve param 'restEnable'. Sleep for 16ms after getting a range." RESET); 459 | restEnable = true; 460 | } 461 | 462 | //Collect the nodes IDs 463 | if(uwbDriverNodeHandle.getParam("nodesId", nodesId)) 464 | { 465 | nodesTotal = nodesId.size(); 466 | printf(KBLU "Retrieved %d nodes IDs: ", nodesTotal); 467 | for (int i = 0; i < nodesTotal; i++) 468 | printf("%d\t", nodesId[i]); 469 | printf("\n" RESET); 470 | } 471 | else 472 | { 473 | printf(KRED "Failed to collect nodes IDs, terminating program!\n" RESET); 474 | return 0; 475 | } 476 | 477 | //Collect the node Positions 478 | if(uwbDriverNodeHandle.getParam("nodesPos", nodesPos)) 479 | { 480 | if( (nodesPos.size() / (double)nodesTotal) != 3) 481 | { 482 | if(nodesTotal != 0) 483 | { 484 | printf(KRED "Nodes locations not whole. Exitting\n" RESET); 485 | return 0; 486 | } 487 | } 488 | else 489 | { 490 | printf(KBLU "Retrieved %d pre-defined cordinates:\n" RESET, nodesTotal); 491 | 492 | for(int i = 0; i < nodesTotal; i++) 493 | { 494 | printf(KBLU "\ta%d = ", nodesId[i]); 495 | for( int j = 0; j < 3; j++) 496 | printf("%4.2f\t", nodesPos[i*3 + j]); 497 | printf("\n" RESET); 498 | } 499 | } 500 | } 501 | else 502 | { 503 | printf(KRED "Nodes location not specified!\n" RESET); 504 | exit(0); 505 | } 506 | 507 | //Collect the common ID 508 | if(uwbDriverNodeHandle.getParam("commonId", commonId)) 509 | { 510 | printf(KBLU "Retrieved common ID %d for all range measurement: ", commonId); 511 | for (uint32_t i = 0; i < nodesId.size(); i++) 512 | { 513 | if(commonId == nodesId[i]) 514 | commonIdx = i; 515 | break; 516 | } 517 | printf("Common node ID: %d. Index: %d\n", commonId, commonIdx); 518 | 519 | } 520 | else 521 | { 522 | printf(KRED "Failed to collect common IDs. Use the default hostedP4xxId and index!\n" RESET); 523 | commonId = -1; 524 | commonIdx = -1; 525 | } 526 | 527 | //Collect the calibration numbers 528 | std::vector> albega; 529 | std::vector albegaA, albegaB; 530 | if(uwbDriverNodeHandle.getParam("albegaA", albegaA)) 531 | { 532 | if((albegaA.size() %3) != 0 || (albegaA.size() == 0)) 533 | { 534 | printf(KRED "Calibration locations not whole. Use identity calibration\n" RESET); 535 | albegaA.push_back(1.0); 536 | albegaA.push_back(0.0); 537 | albegaA.push_back(200); 538 | } 539 | else 540 | { 541 | printf(KBLU "Retrieved calibration values: \n" RESET); 542 | for(int i = 0; i < albega.size(); i++) 543 | printf(KBLU "\t%f", albegaA[i]); 544 | printf("\n" RESET); 545 | 546 | } 547 | } 548 | else 549 | { 550 | printf(KRED "Calibration not specified. Using default: 1.0, 0, 200!\n" RESET); 551 | albegaA.push_back(1.0); 552 | albegaA.push_back(0.0); 553 | albegaA.push_back(200.0); 554 | } 555 | 556 | 557 | if(uwbDriverNodeHandle.getParam("albegaB", albegaB)) 558 | { 559 | if((albegaB.size() %3) != 0 || (albegaB.size() == 0)) 560 | { 561 | printf(KRED "Calibration locations not whole. Use identity calibration\n" RESET); 562 | albegaB.push_back(1.0); 563 | albegaB.push_back(0.0); 564 | albegaB.push_back(200); 565 | } 566 | else 567 | { 568 | printf(KBLU "Retrieved calibration values: \n" RESET); 569 | for(int i = 0; i < albega.size(); i++) 570 | printf(KBLU "\t%f", albegaB[i]); 571 | printf("\n" RESET); 572 | 573 | } 574 | } 575 | else 576 | { 577 | printf(KRED "Calibration not specified. Using default: 1.0, 0, 200!\n" RESET); 578 | albegaB.push_back(1.0); 579 | albegaB.push_back(0.0); 580 | albegaB.push_back(200.0); 581 | } 582 | 583 | albega.push_back(albegaA); 584 | albega.push_back(albegaB); 585 | 586 | //Redundant 587 | int p4xxQueryRate = DFLT_NODE_RATE; 588 | if(uwbDriverNodeHandle.getParam("p4xxQueryRate", p4xxQueryRate)) 589 | printf(KBLU "Retrieved value %d for param 'p4xxQueryRate'\n" RESET, p4xxQueryRate); 590 | else 591 | printf(KYEL "Couldn't retrieve param 'p4xxQueryRate', applying default value %dHz\n" RESET, p4xxQueryRate); 592 | 593 | bool ignrTimeoutUwbInit = true; 594 | if(uwbDriverNodeHandle.getParam("ignrTimeoutUwbInit", ignrTimeoutUwbInit)) 595 | if(ignrTimeoutUwbInit) 596 | printf(KBLU "Retrieved value 'true' for param 'ignrTimeoutUwbInit'!\n" RESET); 597 | else 598 | printf(KBLU "Retrieved value 'false' for param 'ignrTimeoutUwbInit'!\n" RESET); 599 | else 600 | { 601 | printf(KYEL "Couldn't retrieve param 'ignrTimeoutUwbInit', program will proceed if devices init. fails!\n" RESET); 602 | ignrTimeoutUwbInit = true; 603 | } 604 | 605 | bool publishUwbInfo = false; 606 | if(uwbDriverNodeHandle.getParam("publishUwbInfo", publishUwbInfo)) 607 | { 608 | if(publishUwbInfo) 609 | { 610 | printf(KBLU "Retrieved value 'true' for param 'publishUwbData'!\n" RESET); 611 | uwb_range_publisher = uwbDriverNodeHandle.advertise("/uwb_endorange_info", 0); 612 | uwb_data_publisher = uwbDriverNodeHandle.advertise("/uwb_data_info", 0); 613 | } 614 | 615 | else 616 | printf(KBLU "Retrieved value 'false' for param 'publishUwbInfo'!\n" RESET); 617 | } 618 | else 619 | { 620 | printf(KYEL "Couldn't retrieve param 'publishUwbInfo', program will proceed without publishing uwb data!\n" RESET); 621 | publishUwbInfo = false; 622 | } 623 | 624 | /*-----------------------------------------------Mischelaneous-----------------------------------------------------------*/ 625 | 626 | 627 | 628 | /*-----------------------------------------P4xx interface configuration---------------------------------------------------*/ 629 | //P4xx interface 630 | int p4xxInterface = P4XX_ITF_SERIAL; 631 | //check for the interface selected 632 | if(uwbDriverNodeHandle.getParam("p4xxInterfaceChoice", p4xxInterface)) 633 | switch(p4xxInterface) 634 | { 635 | case P4XX_ITF_SERIAL: 636 | printf(KBLU "P4xx Serial interface selected!\n" RESET); 637 | break; 638 | case P4XX_ITF_USB: 639 | printf(KBLU "P4xx USB interface selected!\n" RESET); 640 | break; 641 | default: 642 | break; 643 | } 644 | else 645 | { 646 | printf(KRED "Failed to select communication interface, terminating program!\n" RESET); 647 | return 0; 648 | } 649 | 650 | string p4xxSerialPort = string(DFLT_P4xx_PORT); 651 | //check for the p4xx port's name 652 | if(uwbDriverNodeHandle.getParam("p4xxSerialPort", p4xxSerialPort)) 653 | printf(KBLU "Retrieved value %s for param 'p4xxSerialPort'!\n" RESET, p4xxSerialPort.data()); 654 | else 655 | { 656 | printf(KRED "Couldn't retrieve param 'p4xxSerialPort', program closed!\n" RESET); 657 | return 0; 658 | } 659 | 660 | if(uwbDriverNodeHandle.getParam("p4xxMode", p4xxMode)) 661 | { 662 | switch(p4xxMode) 663 | { 664 | case MODE_RCM: 665 | printf(KBLU "Started with RCM mode\n" RESET); 666 | break; 667 | case MODE_RN: 668 | printf(KBLU "Started with RN mode\n" RESET); 669 | break; 670 | default: 671 | printf(KYEL "Not RCM or RN specific! Default is RCM.\n" RESET); 672 | p4xxMode = MODE_RCM; 673 | break; 674 | break; 675 | } 676 | 677 | } 678 | else 679 | { 680 | printf(KYEL "Couldn't retrieve param 'p4xxCurrMode'. Started with default mode RCM!\n" RESET); 681 | p4xxCurrMode = MODE_RCM; 682 | } 683 | 684 | bool autoConfigRn = true; 685 | if(uwbDriverNodeHandle.getParam("autoConfigRn", autoConfigRn)) 686 | if(autoConfigRn) 687 | printf(KBLU "Retrieved value 'true' for param 'autoConfigRn'!\n" RESET); 688 | else 689 | printf(KBLU "Retrieved value 'false' for param 'autoConfigRn'!\n" RESET); 690 | else 691 | { 692 | printf(KYEL "Couldn't retrieve param 'autoConfigRn'. Retaining configuration on P440 instead!\n" RESET); 693 | autoConfigRn = false; 694 | } 695 | 696 | bool portFound = false; 697 | int firstUnmatchedPort = -1; 698 | string p4xxIdNum; 699 | 700 | string p4xxSerialPortAffix = string("/dev/serial/by-id/usb-1027_Time_Domain_P440_*-if00"); 701 | string p4xxSerialPortFullName = string("/dev/serial/by-id/usb-1027_Time_Domain_P440_") + p4xxSerialPort + string("-if00"); 702 | 703 | while(!portFound && ros::ok()) 704 | { 705 | glob(p4xxSerialPortAffix.c_str(), 0, NULL, &glob_results); 706 | // printf("Port \"%s\" not found!\n"RESET, p4xxSerialPortFull.data()); 707 | int matches = glob_results.gl_pathc; 708 | if (matches != 0) 709 | { 710 | printf("Ports found: \n"); 711 | for(int i = 0; i < matches; i++) 712 | { 713 | if(strcmp(glob_results.gl_pathv[glob_results.gl_offs + i], 714 | p4xxSerialPortFullName.data()) == 0) 715 | { 716 | printf("\tMatched: %s\n", glob_results.gl_pathv[glob_results.gl_offs + i]); 717 | globfree(&glob_results); 718 | portFound = true; 719 | break; 720 | } 721 | else 722 | { 723 | printf(KYEL "\tPassed: %s\n" RESET, glob_results.gl_pathv[glob_results.gl_offs + i]); 724 | if(firstUnmatchedPort == -1) 725 | firstUnmatchedPort = i; 726 | } 727 | 728 | } 729 | } 730 | else 731 | printf("No ports available!\n"); 732 | 733 | if(portFound) 734 | { 735 | p4xxIdNum = p4xxSerialPort; 736 | globfree(&glob_results); 737 | break; 738 | } 739 | else if(firstUnmatchedPort != -1) 740 | { 741 | p4xxSerialPortFullName = string(glob_results.gl_pathv[glob_results.gl_offs + firstUnmatchedPort]); 742 | //Erase the end 743 | p4xxSerialPortFullName.erase(p4xxSerialPortFullName.find("-if00"), 5); 744 | //get the ID number 745 | p4xxIdNum = p4xxSerialPortFullName.substr(p4xxSerialPortAffix.length()-6, 746 | p4xxSerialPortFullName.length()-(p4xxSerialPortAffix.length()-1)); 747 | printf("\tPort %s will be reconfigured to %s.\n", p4xxIdNum.data(), p4xxSerialPort.data()); 748 | globfree(&glob_results); 749 | break; 750 | 751 | } 752 | ros::Duration(0.5).sleep(); 753 | } 754 | 755 | 756 | 757 | printf("\nRCM Localization App\n\n"); 758 | 759 | if(p4xxInterface == P4XX_ITF_SERIAL) 760 | rcmIf = rcmIfSerial; 761 | else if (p4xxInterface == P4XX_ITF_USB) 762 | rcmIf = rcmIfUsb; 763 | 764 | // initialize the interface to the P4xx 765 | if (rcmIfInit(rcmIf, &p4xxIdNum[0]) != OK) 766 | { 767 | printf("Initialization failed.\n"); 768 | exit(0); 769 | } 770 | 771 | //Change to RCM mode directly to avoid all the unexpected confirm messages 772 | while(rcmOpModeSet(RCRM_OPMODE_RCM) != 0 && ros::ok()) 773 | { 774 | printf("Time out waiting for opmode set.\n"); 775 | if(!ignrTimeoutUwbInit) 776 | exit(0); 777 | } 778 | p4xxCurrMode = MODE_RCM; 779 | printf("RCM mode confirmed\n"); 780 | 781 | // Make sure RCM is awake 782 | if (rcmSleepModeSet(RCRM_SLEEP_MODE_ACTIVE) != 0) 783 | { 784 | printf("Time out waiting for sleep mode set.\n"); 785 | if(!ignrTimeoutUwbInit) 786 | exit(0); 787 | } 788 | 789 | // execute Built-In Test - verify that radio is healthy 790 | if (rcmBit(&status) != 0) 791 | { 792 | printf("Time out waiting for BIT.\n"); 793 | if(!ignrTimeoutUwbInit) 794 | exit(0); 795 | } 796 | 797 | if (status != OK) 798 | { 799 | printf("Built-in test failed - status %d.\n", status); 800 | if(!ignrTimeoutUwbInit) 801 | exit(0); 802 | } 803 | else 804 | { 805 | printf("Radio passes built-in test.\n\n"); 806 | } 807 | 808 | // retrieve config from RCM 809 | if (rcmConfigGet(&rcmConfig) != 0) 810 | { 811 | printf("Time out waiting for config confirm.\n"); 812 | if(!ignrTimeoutUwbInit) 813 | exit(0); 814 | } 815 | else 816 | { 817 | if(!portFound) 818 | { 819 | rcmConfig.nodeId = stoi(p4xxSerialPort); 820 | 821 | while(rcmConfigSet(&rcmConfig) != 0 && ros::ok()) 822 | { 823 | printf("Time out waiting for RCM_CONFIG_SET_CONFIRM.\n"); 824 | if(!ignrTimeoutUwbInit) 825 | exit(0); 826 | } 827 | printf("Node ID reconfigured successfully\n"); 828 | 829 | rcmConfig.nodeId = -1; 830 | while(rcmConfigGet(&rcmConfig) != 0) 831 | { 832 | printf("Time out waiting for RCM_CONFIG_GET_CONFIRM.\n"); 833 | if(!ignrTimeoutUwbInit) 834 | exit(0); 835 | } 836 | } 837 | } 838 | 839 | hostedP4xxId = rcmConfig.nodeId; 840 | for (uint32_t i = 0; i < nodesId.size(); i++) 841 | if(hostedP4xxId == nodesId[i]) 842 | hostedP4xxIdx = i; 843 | printf("Node ID: %d. Node Index: %d\n", hostedP4xxId, hostedP4xxIdx); 844 | 845 | if(autoConfigRn) 846 | { 847 | string smFileName = string(""); 848 | if(uwbDriverNodeHandle.getParam("smFileName", smFileName)) 849 | printf(KBLU "Retrieved value %s for param 'smFileName'!\n" RESET, smFileName.data()); 850 | else 851 | { 852 | printf(KRED "Couldn't retrieve param 'smFileName', program closed!\n" RESET); 853 | return 0; 854 | } 855 | 856 | struct stat buf; 857 | if(stat(smFileName.c_str(), &buf) != -1) 858 | printf("\tCSV found: %s\n", smFileName.c_str()); 859 | else 860 | { 861 | printf(KRED "\tCSV not found!\n" RESET); 862 | exit(-1); 863 | } 864 | 865 | ifstream smFile (smFileName.data()); 866 | string line; 867 | 868 | //Ditch the first row of titles 869 | getline(smFile, line); 870 | 871 | uint8_t k = 0; 872 | while(!smFile.eof()) 873 | { 874 | //Get the slot number 875 | getline(smFile, line, ','); 876 | if(line.length() == 0) 877 | break; 878 | 879 | cout << "# " << line << endl; 880 | initTdmaSlotMapSet.slots[k].slotNumber = atoi(line.c_str()); 881 | 882 | getline(smFile, line, ','); 883 | cout << "rqsterId: " << line << endl; 884 | initTdmaSlotMapSet.slots[k].requesterId = atoi(line.c_str()); 885 | 886 | getline(smFile, line, ','); 887 | cout << "responderId: " << line << endl; 888 | initTdmaSlotMapSet.slots[k].responderId = atoi(line.c_str()); 889 | 890 | getline(smFile, line, ','); 891 | cout << "integrationIndex: " << line << endl; 892 | initTdmaSlotMapSet.slots[k].integrationIndex = atoi(line.c_str()); 893 | 894 | getline(smFile, line, ','); 895 | cout << "codeChannel: " << line << endl; 896 | initTdmaSlotMapSet.slots[k].codeChannel = atoi(line.c_str()); 897 | 898 | getline(smFile, line, ','); 899 | cout << "antennaMode: " << line << endl; 900 | if (strcmp(line.data(), "Antenna A") == 0) 901 | initTdmaSlotMapSet.slots[k].antennaMode = 0; 902 | else if (strcmp(line.data(), "Antenna B") == 0) 903 | initTdmaSlotMapSet.slots[k].antennaMode = 1; 904 | else if (strcmp(line.data(), "TX A / RX B") == 0) 905 | initTdmaSlotMapSet.slots[k].antennaMode = 2; 906 | else if (strcmp(line.data(), "TX B / RX A") == 0) 907 | initTdmaSlotMapSet.slots[k].antennaMode = 3; 908 | else if (strcmp(line.data(), "Toggle A/B") == 0) 909 | initTdmaSlotMapSet.slots[k].antennaMode = 128; 910 | else 911 | initTdmaSlotMapSet.slots[k].antennaMode = 0; 912 | 913 | initTdmaSlotMapSet.slots[k].flags = 0; 914 | getline(smFile, line, ','); 915 | cout << "rqstData: " << line << endl; 916 | if (strcmp(line.data(), "1") == 0) 917 | initTdmaSlotMapSet.slots[k].flags |= 0x02; 918 | 919 | getline(smFile, line, ','); 920 | cout << "rspdData: " << line << endl; 921 | if (strcmp(line.data(), "1") == 0) 922 | initTdmaSlotMapSet.slots[k].flags |= 0x04; 923 | 924 | //Slot type is range by default 925 | getline(smFile, line, ','); 926 | cout << "slotType: " << line << endl; 927 | initTdmaSlotMapSet.slots[k].slotType = 1; 928 | 929 | // Ditch the sleep entry 930 | getline(smFile, line, ','); 931 | cout << "sleep: " << line << endl; 932 | 933 | getline(smFile, line, ','); 934 | cout << "requestedDurationMicroseconds: " << line << endl; 935 | initTdmaSlotMapSet.slots[k].requestedDurationMicroseconds = atoi(line.c_str())*1000; 936 | 937 | // Ditch the calculated time entry 938 | getline(smFile, line, '\n'); 939 | cout << "calc. requestedDurationMicroseconds: " << line << endl << endl; 940 | 941 | k++; 942 | initTdmaSlotMapSet.numSlots = k; 943 | } 944 | 945 | // Set TDMA slot map 946 | while (rnTdmaSlotMapSet(&initTdmaSlotMapSet) != 0 && ros::ok()) 947 | { 948 | printf("Time out waiting for rnTdmaSlotMap confirm.\n"); 949 | if(!ignrTimeoutUwbInit) 950 | exit(0); 951 | } 952 | 953 | // Set the NDB Neighbor Age arbitrarily high to keep nodes from dropping out 954 | rnInitConfig.maxNeighborAgeMs = 4294967295; 955 | 956 | // Set network mode to TDMA 957 | rnInitConfig.networkSyncMode = RN_NETWORK_SYNC_MODE_TDMA; 958 | 959 | //Range type for NDB = PRM; 960 | rnInitConfig.rnFlags &= ~(1<<2); 961 | 962 | // Autosend the range. 963 | rnInitConfig.autosendType = 0x01; 964 | 965 | if (rnConfigSet(&rnInitConfig) != 0) 966 | { 967 | printf("Time out waiting for rnConfig confirm.\n"); 968 | if(!ignrTimeoutUwbInit) 969 | exit(0); 970 | } 971 | 972 | // Retrieve TDMA slot map 973 | while (rnTdmaSlotMapGet(&rnInitTdmaSlotMap) != 0 && ros::ok()) 974 | { 975 | printf("Time out waiting for rnTdmaSlotMap confirm.\n"); 976 | if(!ignrTimeoutUwbInit) 977 | exit(0); 978 | } 979 | 980 | if (rnTdmaConfigGet(&rnTDMAConfig) != 0) 981 | { 982 | printf("Time out waiting for rnConfig confirm.\n"); 983 | if(!ignrTimeoutUwbInit) 984 | exit(0); 985 | } 986 | 987 | printf("TDMA Current config: Max rqst data %d, Max rspd data %d\n", 988 | rnTDMAConfig.maxRequestDataSize, rnTDMAConfig.maxResponseDataSize); 989 | 990 | rnTDMAConfig.maxRequestDataSize = 100; 991 | rnTDMAConfig.maxResponseDataSize = 0; 992 | 993 | // Retrieve TDMA config 994 | while (rnTdmaConfigSet(&rnTDMAConfig) != 0 && ros::ok()) 995 | { 996 | printf("Time out waiting for rnTdmaSlotMap confirm.\n"); 997 | if(!ignrTimeoutUwbInit) 998 | exit(0); 999 | } 1000 | 1001 | for (uint32_t i = 0; i < rnInitTdmaSlotMap.numSlots; i++) 1002 | { 1003 | printf(KBLU "SLOT %02d: ", rnInitTdmaSlotMap.slots[i].slot.slotNumber); 1004 | printf("Req ID:%u ", rnInitTdmaSlotMap.slots[i].slot.requesterId); 1005 | printf("Rsp ID:%u ", rnInitTdmaSlotMap.slots[i].slot.responderId); 1006 | printf("Pii:%u ", rnInitTdmaSlotMap.slots[i].slot.integrationIndex); 1007 | printf("Channel:%u ", rnInitTdmaSlotMap.slots[i].slot.codeChannel); 1008 | printf("Antenna:%u ", rnInitTdmaSlotMap.slots[i].slot.antennaMode); 1009 | printf("Flags:0x%X ", rnInitTdmaSlotMap.slots[i].slot.flags); 1010 | printf("Type:%u ", rnInitTdmaSlotMap.slots[i].slot.slotType); 1011 | printf("Man Time:%u\n" RESET, rnInitTdmaSlotMap.slots[i].slot.requestedDurationMicroseconds); 1012 | } 1013 | 1014 | 1015 | //Change to RN if requested 1016 | if(p4xxMode == MODE_RN) 1017 | { 1018 | // Make sure opmode is RN mode 1019 | while(rcmOpModeSet(RCRM_OPMODE_RN) != 0 && ros::ok()) 1020 | { 1021 | printf("Time out waiting for opmode set.\n"); 1022 | if(!ignrTimeoutUwbInit) 1023 | exit(0); 1024 | } 1025 | p4xxCurrMode = MODE_RN; 1026 | printf("RN mode confirmed\n"); 1027 | } 1028 | } 1029 | 1030 | // Set the P4xx to active mode 1031 | if (rcmSleepModeSet(RCRM_SLEEP_MODE_ACTIVE) != 0) 1032 | { 1033 | printf("Time out waiting for sleep mode set.\n"); 1034 | if(!ignrTimeoutUwbInit) 1035 | exit(0); 1036 | } 1037 | 1038 | // Set the hosted Id number 1039 | uwbDriverNodeHandle.setParam("/uwb/hostedP4xxId", hostedP4xxId); 1040 | 1041 | ros::ServiceServer modeService = uwbDriverNodeHandle.advertiseService("/uwb_mode_config", uwb_mode_config); 1042 | ROS_INFO(KGRN "UWB MODE SERVICE AVAILABLE." RESET); 1043 | 1044 | ros::ServiceServer rangeCommService = uwbDriverNodeHandle.advertiseService("/uwb_range_comm", uwb_range_comm); 1045 | ROS_INFO(KGRN "UWB RANGE-COMM SERVICE AVAILABLE" RESET); 1046 | 1047 | /*-----------------------------------------P4xx interface configuration---------------------------------------------------*/ 1048 | 1049 | 1050 | /*-------------------------------------------------Initial Trilateration----------------------------------------------------*/ 1051 | 1052 | ros::Duration sleepTime = ros::Duration(1.0/p4xxQueryRate); 1053 | //get the initial position by trilaterating the average 1054 | while(ros::ok()) 1055 | { 1056 | ros::spinOnce(); 1057 | std::vector ancsPosUpd; 1058 | 1059 | //Check if the positions have been updated 1060 | if(uwbDriverNodeHandle.getParam("/uwb/ancsPosUpd", ancsPosUpd)) 1061 | { 1062 | if(ancsPosUpd.size() == nodesPos.size() - 3) 1063 | { 1064 | for(int i = 0; i < ancsPosUpd.size(); i++) 1065 | nodesPos[i] = ancsPosUpd[i]; 1066 | } 1067 | } 1068 | uint8_t msg_type = -1; 1069 | //Check and find the coresponding index of the update 1070 | int nodeIndex = -1; 1071 | uint8_t msg_type_rt = 1; 1072 | switch (msg_type_rt = rcmInfoGet(&rangeInfo, &dataInfo, &ndbInfo))//get distance 1073 | { 1074 | case RANGEINFO: 1075 | { 1076 | if(rangeInfo.precisionRangeMm < 75000) 1077 | { 1078 | for(int i = 0; i < nodesTotal; i++) 1079 | if((uint32_t)nodesId[i] == rangeInfo.responderId) 1080 | { 1081 | nodeIndex = i; 1082 | msg_type = RANGEINFO; 1083 | break; 1084 | } 1085 | } 1086 | } 1087 | case DATAINFO: 1088 | { 1089 | for(int i = 0; i < nodesTotal; i++) 1090 | if((uint32_t)nodesId[i] == dataInfo.sourceId) 1091 | { 1092 | nodeIndex = i; 1093 | msg_type = DATAINFO; 1094 | break; 1095 | } 1096 | } 1097 | 1098 | default: 1099 | break; 1100 | 1101 | } 1102 | if (nodeIndex != -1) 1103 | { 1104 | if(msg_type == RANGEINFO && rangeInfo.rangeStatus == OK) 1105 | { 1106 | uwb_driver::UwbRange uwb_range_info_msg; 1107 | uwb_range_info_msg.header = std_msgs::Header(); 1108 | uwb_range_info_msg.header.frame_id = "uwb"; 1109 | uwb_range_info_msg.header.stamp = ros::Time::now(); 1110 | uwb_range_info_msg.requester_id = (commonId == -1)?hostedP4xxId:commonId; 1111 | uwb_range_info_msg.requester_idx = (commonId == -1)?hostedP4xxIdx:commonIdx; 1112 | uwb_range_info_msg.responder_id = nodesId[nodeIndex]; 1113 | uwb_range_info_msg.responder_idx = nodeIndex; 1114 | uwb_range_info_msg.requester_LED_flag = rangeInfo.reqLEDFlags; 1115 | uwb_range_info_msg.responder_LED_flag = rangeInfo.respLEDFlags; 1116 | uwb_range_info_msg.noise = rangeInfo.noise; 1117 | uwb_range_info_msg.vPeak = rangeInfo.vPeak; 1118 | uwb_range_info_msg.distance = (rangeInfo.precisionRangeMm/1000.0 > albega[rangeInfo.antennaMode][2])?(rangeInfo.precisionRangeMm/1000.0) : (albega[rangeInfo.antennaMode][0]*rangeInfo.precisionRangeMm/1000.0 + albega[rangeInfo.antennaMode][1]); 1119 | uwb_range_info_msg.distance_err = rangeInfo.precisionRangeErrEst/1000.0; 1120 | uwb_range_info_msg.distance_dot = rangeInfo.filteredRangeVel/1000.0; 1121 | uwb_range_info_msg.distance_dot_err = rangeInfo.filteredRangeVel/1000.0; 1122 | uwb_range_info_msg.antenna = rangeInfo.antennaMode + (hostedP4xxIdx-1)*2 + 1; 1123 | uwb_range_info_msg.stopwatch_time = rangeInfo.stopwatchTime; 1124 | uwb_range_info_msg.uwb_time = rangeInfo.timestamp; 1125 | uwb_range_info_msg.responder_location.x = nodesPos[nodeIndex*3]; 1126 | uwb_range_info_msg.responder_location.y = nodesPos[nodeIndex*3+1]; 1127 | uwb_range_info_msg.responder_location.z = nodesPos[nodeIndex*3+2]; 1128 | 1129 | if(publishUwbInfo) 1130 | uwb_range_publisher.publish(uwb_range_info_msg); 1131 | 1132 | printf("RANGEINFO:Time=%.4f\ttu = %zu\tant=%02x\tRqIDx=%d\tRqID=%d\tRSIDx=%d\tRSID=%d\td=%6.3f, de = %6.3f, dd = %6.3f, dde = %6.3f\trqLED = %02x\trsLED = %02x\tnoise = %d\tVp = %d\tsw=%d\tx=%6.2f\ty=%6.2f\tz=%6.2f\n", 1133 | uwb_range_info_msg.header.stamp.toSec(), 1134 | uwb_range_info_msg.uwb_time, 1135 | uwb_range_info_msg.antenna, 1136 | uwb_range_info_msg.requester_idx+1, 1137 | uwb_range_info_msg.requester_id, 1138 | uwb_range_info_msg.responder_idx+1, 1139 | uwb_range_info_msg.responder_id, 1140 | uwb_range_info_msg.distance, 1141 | uwb_range_info_msg.distance_err, 1142 | uwb_range_info_msg.distance_dot, 1143 | uwb_range_info_msg.distance_dot_err, 1144 | uwb_range_info_msg.requester_LED_flag, 1145 | uwb_range_info_msg.responder_LED_flag, 1146 | uwb_range_info_msg.noise, 1147 | uwb_range_info_msg.vPeak, 1148 | uwb_range_info_msg.stopwatch_time, 1149 | uwb_range_info_msg.responder_location.x, 1150 | uwb_range_info_msg.responder_location.y, 1151 | uwb_range_info_msg.responder_location.z); 1152 | 1153 | } 1154 | else if(msg_type == DATAINFO) 1155 | { 1156 | uwb_driver::UwbData uwb_data_info_msg; 1157 | uwb_data_info_msg.header = std_msgs::Header(); 1158 | uwb_data_info_msg.header.frame_id = "uwb"; 1159 | uwb_data_info_msg.header.stamp = ros::Time::now(); 1160 | uwb_data_info_msg.source_id = nodesId[nodeIndex]; 1161 | uwb_data_info_msg.source_idx = nodeIndex; 1162 | 1163 | uwb_data_info_msg.antenna = dataInfo.antennaId; 1164 | uwb_data_info_msg.uwb_time = dataInfo.timestamp; 1165 | uwb_data_info_msg.data_size = dataInfo.dataSize; 1166 | uwb_data_info_msg.data.insert(uwb_data_info_msg.data.end(), &dataInfo.data[0], &dataInfo.data[dataInfo.dataSize]); 1167 | 1168 | if(publishUwbInfo) 1169 | uwb_data_publisher.publish(uwb_data_info_msg); 1170 | 1171 | printf("DATAINFO: Time=%.4f\ttu = %zu\tant=0x%02x\tIndex=%d\tID=%d\tData bytes: %d\t{", 1172 | uwb_data_info_msg.header.stamp.toSec(), 1173 | uwb_data_info_msg.uwb_time, 1174 | uwb_data_info_msg.antenna, 1175 | uwb_data_info_msg.source_idx + 1, 1176 | uwb_data_info_msg.source_id, 1177 | uwb_data_info_msg.data_size); 1178 | for(int i = 0; i < uwb_data_info_msg.data_size-1; i++) 1179 | printf(KGRN "%02x ", uwb_data_info_msg.data[i]); 1180 | printf("%02x" RESET, uwb_data_info_msg.data[uwb_data_info_msg.data_size-1]); 1181 | printf("}\n"); 1182 | } 1183 | } 1184 | 1185 | if(slumberTime) 1186 | { 1187 | rcmIfClose(); 1188 | printf("uwb_driver recessing...!\n"); 1189 | ros::Duration oneMonthTime(2592000); 1190 | oneMonthTime.sleep(); 1191 | } 1192 | 1193 | if(restEnable) 1194 | sleepTime.sleep(); 1195 | } 1196 | rcmIfClose(); 1197 | } 1198 | -------------------------------------------------------------------------------- /src/uwb_driver_gazebo.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "uwb_driver/UwbRange.h" 3 | #include "geometry_msgs/PoseStamped.h" 4 | #include 5 | #include "eigen_conversions/eigen_msg.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace Eigen; 13 | // using namespace std; 14 | using namespace tf; 15 | ros::Publisher uwb_pub; 16 | std::vector nodesId; 17 | std::vector nodesPos; 18 | std::vector antennaOffset; 19 | int node_num; 20 | int antenna_num; 21 | int count = 0; 22 | int count_antenna = 0; 23 | std::random_device rd; 24 | std::mt19937 gen(rd()); 25 | std::normal_distribution<> d(0,0.02); 26 | 27 | void gazeboCallback(const geometry_msgs::PoseStamped pose) 28 | { 29 | // frame_id : world 30 | uwb_driver::UwbRange msg; 31 | msg.header = std_msgs::Header(); 32 | msg.header.frame_id = "uwb"; 33 | msg.header.stamp = pose.header.stamp; 34 | msg.requester_id = nodesId.back(); 35 | msg.requester_idx = nodesId.back()%100; 36 | msg.responder_id = nodesId[count]; 37 | msg.responder_idx = nodesId[count]%100; 38 | 39 | Eigen::Affine3d pose_eigen; 40 | poseMsgToEigen(pose.pose, pose_eigen); 41 | static tf::TransformBroadcaster br; 42 | tf::Transform transform; 43 | 44 | if(antenna_num == 0) 45 | { 46 | msg.antenna = 0; 47 | tf::transformEigenToTF (pose_eigen, transform); 48 | br.sendTransform(tf::StampedTransform(transform, pose.header.stamp, pose.header.frame_id, "antenna")); 49 | } 50 | else 51 | { 52 | int antenna_id = count_antenna%antenna_num; 53 | msg.antenna = antenna_id + 1; 54 | double x = antennaOffset[antenna_id*3]; 55 | double y = antennaOffset[antenna_id*3+1]; 56 | double z = antennaOffset[antenna_id*3+2]; 57 | Eigen::Affine3d trans = Eigen::Affine3d::Identity(); 58 | trans.translate(Vector3d(x,y,z)); 59 | pose_eigen = pose_eigen * trans; 60 | tf::transformEigenToTF(pose_eigen, transform); 61 | br.sendTransform(tf::StampedTransform(transform, pose.header.stamp, pose.header.frame_id, "antenna"+std::to_string(msg.antenna))); 62 | // std::cout << " " <<"antenna"+std::to_string(msg.antenna)<< " " << std::endl; 63 | } 64 | 65 | count_antenna = count_antenna+1; 66 | 67 | double distance = sqrt( 68 | pow(pose_eigen(0,3) - nodesPos[count*3 ], 2)+ 69 | pow(pose_eigen(1,3) - nodesPos[count*3+1], 2)+ 70 | pow(pose_eigen(2,3) - nodesPos[count*3+2], 2)); 71 | 72 | transform.setOrigin( tf::Vector3(nodesPos[count*3 ],nodesPos[count*3+1], nodesPos[count*3+2])); 73 | br.sendTransform(tf::StampedTransform(transform, pose.header.stamp, pose.header.frame_id, std::to_string(count)+"anchor")); 74 | 75 | msg.distance = distance + d(gen); 76 | msg.distance_err = 0.02; 77 | msg.responder_location.x = nodesPos[count*3]; 78 | msg.responder_location.y = nodesPos[count*3+1]; 79 | msg.responder_location.z = nodesPos[count*3+2]; 80 | uwb_pub.publish(msg); 81 | 82 | if (antenna_num==0 || count_antenna%antenna_num == 0) 83 | count = (count+1)%node_num; 84 | 85 | ros::Duration(0.025).sleep(); 86 | } 87 | 88 | int main(int argc, char **argv) 89 | { 90 | ros::init(argc, argv, "uwb_driver_gazebo"); 91 | 92 | ros::NodeHandle n; 93 | 94 | if(n.getParam("/uwb/nodesId", nodesId)) 95 | for (auto it:nodesId) 96 | ROS_WARN("Get node ID: %d", it); 97 | 98 | if(n.getParam("/uwb/nodesPos", nodesPos)) 99 | for(auto it:nodesPos) 100 | ROS_WARN("Get node position: %4.2f", it); 101 | 102 | if(n.getParam("/uwb/antennaOffset", antennaOffset)) 103 | for(auto it:antennaOffset) 104 | ROS_WARN("Get antenna Offset: %4.2f", it); 105 | 106 | antenna_num = antennaOffset.size()/3; 107 | ROS_WARN("Using %d antennas", antenna_num); 108 | 109 | ros::Subscriber sub = n.subscribe("/ground_truth_to_tf/pose", 1, gazeboCallback); 110 | 111 | uwb_pub = n.advertise("/uwb_endorange_info", 0); 112 | 113 | node_num = nodesId.size() - 1; 114 | 115 | ROS_WARN("Publishing.."); 116 | 117 | ros::spin(); 118 | 119 | return 0; 120 | } -------------------------------------------------------------------------------- /srv/uwbModeConfig.srv: -------------------------------------------------------------------------------- 1 | uint8 p4xxmode #mode to let p4xx operate at 2 | --- 3 | uint8 result #result of the command, check for verbal reference in the 'srv_return_verbal.h' 4 | -------------------------------------------------------------------------------- /srv/uwbRangeComm.srv: -------------------------------------------------------------------------------- 1 | uint8 action #This value specifies the specific action: 2 | #0: make a range request by the hosted p4xx 3 | #1: broadcast a message on the preconfigured channel 4 | #2: send a range request + broadcast data on a selected channel 5 | #3: set requester data (for network mode) 6 | #4: set responder data (for network mode) 7 | uint8 responder #ID of the responder node 8 | uint8 antenna #Antenna to be used for this command 9 | uint8 channel #Channel to be specified for action 3 10 | uint16 data_size #Number of data bytes to send 11 | uint8[] data #Data array collected from the message 12 | --- 13 | uint8 result #result of the command, check for verbal reference in the 'srv_return_verbal.h' 14 | --------------------------------------------------------------------------------