Branch data Line data Source code
1 : : // Copyright 2018 the Autoware Foundation 2 : : // 3 : : // Licensed under the Apache License, Version 2.0 (the "License"); 4 : : // you may not use this file except in compliance with the License. 5 : : // You may obtain a copy of the License at 6 : : // 7 : : // http://www.apache.org/licenses/LICENSE-2.0 8 : : // 9 : : // Unless required by applicable law or agreed to in writing, software 10 : : // distributed under the License is distributed on an "AS IS" BASIS, 11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 : : // See the License for the specific language governing permissions and 13 : : // limitations under the License. 14 : : // 15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc. 16 : : 17 : : #include <cstring> 18 : : #include <cmath> 19 : : #include <limits> 20 : : #include <numeric> 21 : : #include <utility> 22 : : #include <vector> 23 : : #include <iostream> 24 : : 25 : : #include <iomanip> 26 : : 27 : : #include "xsens_driver/xsens_common.hpp" 28 : : 29 : : namespace autoware 30 : : { 31 : : namespace drivers 32 : : { 33 : : namespace xsens_driver 34 : : { 35 : : 36 : 2 : MID MID_from_int(uint16_t value) 37 : : { 38 [ - - - - : 2 : switch (value) { - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - - - - - - - ] 39 : : case 0x42: 40 : : return MID::ERROR; 41 : 0 : case 0x3E: 42 : 0 : return MID::WAKE_UP; 43 : 0 : case 0x3F: 44 : 0 : return MID::WAKE_UP_ACK; 45 : 0 : case 0x30: 46 : 0 : return MID::GO_TO_CONFIG; 47 : 0 : case 0x10: 48 : 0 : return MID::GO_TO_MEASUREMENT; 49 : 0 : case 0x40: 50 : 0 : return MID::RESET; 51 : 0 : case 0x00: 52 : 0 : return MID::REQ_DID; 53 : 0 : case 0x01: 54 : 0 : return MID::DEVICE_ID; 55 : 0 : case 0x1C: 56 : 0 : return MID::REQ_PRODUCT_CODE; 57 : 0 : case 0x1D: 58 : 0 : return MID::PRODUCT_CODE; 59 : 0 : case 0x12: 60 : 0 : return MID::REQ_FW_REV; 61 : 0 : case 0x13: 62 : 0 : return MID::FIRMWARE_REV; 63 : 0 : case 0x0E: 64 : 0 : return MID::RESTORE_FACTORY_DEF; 65 : 0 : case 0x18: 66 : 0 : return MID::SET_BAUDRATE; 67 : 0 : case 0x24: 68 : 0 : return MID::RUN_SELFTEST; 69 : 0 : case 0x25: 70 : 0 : return MID::SELFTEST_ACK; 71 : 0 : case 0xDA: 72 : 0 : return MID::SET_ERROR_MODE; 73 : 0 : case 0xDC: 74 : 0 : return MID::SET_TRANSMIT_DELAY; 75 : 0 : case 0x48: 76 : 0 : return MID::SET_OPTION_FLAGS; 77 : 0 : case 0x84: 78 : 0 : return MID::SET_LOCATION_ID; 79 : 0 : case 0x2C: 80 : 0 : return MID::SET_SYNC_SETTINGS; 81 : 0 : case 0x0C: 82 : 0 : return MID::REQ_CONFIGURATION; 83 : 0 : case 0x0D: 84 : 0 : return MID::CONFIGURATION; 85 : 0 : case 0x04: 86 : 0 : return MID::SET_PERIOD; 87 : 0 : case 0x86: 88 : 0 : return MID::SET_EXT_OUTPUT_MODE; 89 : 0 : case 0xC0: 90 : 0 : return MID::SET_OUTPUT_CONFIGURATION; 91 : 0 : case 0x8E: 92 : 0 : return MID::SET_STRING_OUTPUT_TYPE; 93 : 0 : case 0xEC: 94 : 0 : return MID::SET_ALIGNMENT_ROTATION; 95 : 0 : case 0xD0: 96 : 0 : return MID::SET_OUTPUT_MODE; 97 : 0 : case 0xD2: 98 : 0 : return MID::SET_OUTPUT_SETTINGS; 99 : 0 : case 0x34: 100 : 0 : return MID::REQ_DATA; 101 : 0 : case 0x32: 102 : 0 : return MID::MT_DATA; 103 : 2 : case 0x36: 104 : 2 : return MID::MT_DATA2; 105 : 0 : case 0xA4: 106 : 0 : return MID::RESET_ORIENTATION; 107 : 0 : case 0x60: 108 : 0 : return MID::SET_UTC_TIME; 109 : 0 : case 0xA8: 110 : 0 : return MID::ADJUST_UTC_TIME; 111 : 0 : case 0x61: 112 : 0 : return MID::UTC_TIME; 113 : 0 : case 0x62: 114 : 0 : return MID::REQ_AVAILABLE_SCENARIOS; 115 : 0 : case 0x63: 116 : 0 : return MID::AVAILABLE_SCENARIOS; 117 : 0 : case 0x64: 118 : 0 : return MID::SET_CURRENT_SCENARIO; 119 : 0 : case 0x66: 120 : 0 : return MID::SET_GRAVITY_MAGNITUDE; 121 : 0 : case 0x6E: 122 : 0 : return MID::SET_LAT_LON_ALT; 123 : 0 : case 0x22: 124 : 0 : return MID::SET_NO_ROTATION; 125 : 0 : default: 126 [ # # # # : 0 : throw std::runtime_error("Unknown value: " + std::to_string(value)); # # # # ] 127 : : } 128 : : } 129 : : 130 : 4 : XDIGroup XDIGroup_from_int(uint16_t value) 131 : : { 132 [ + - - - : 4 : switch (value) { - + - - - - - - - - - ] 133 : : case 0x0800: 134 : : return XDIGroup::TEMPERATURE; 135 : 2 : case 0x1000: 136 : 2 : return XDIGroup::TIMESTAMP; 137 : 0 : case 0x2000: 138 : 0 : return XDIGroup::ORIENTATION_DATA; 139 : 0 : case 0x3000: 140 : 0 : return XDIGroup::PRESSURE; 141 : 0 : case 0x4000: 142 : 0 : return XDIGroup::ACCELERATION; 143 : 0 : case 0x5000: 144 : 0 : return XDIGroup::POSITION; 145 : 2 : case 0x7000: 146 : 2 : return XDIGroup::GNSS; 147 : 0 : case 0x8000: 148 : 0 : return XDIGroup::ANGULAR_VELOCITY; 149 : 0 : case 0x8800: 150 : 0 : return XDIGroup::GPS; 151 : 0 : case 0xA000: 152 : 0 : return XDIGroup::SENSOR_COMPONENT_READOUT; 153 : 0 : case 0xB000: 154 : 0 : return XDIGroup::ANALOG_IN; 155 : 0 : case 0xC000: 156 : 0 : return XDIGroup::MAGNETIC; 157 : 0 : case 0xD000: 158 : 0 : return XDIGroup::VELOCITY; 159 : 0 : case 0xE000: 160 : 0 : return XDIGroup::STATUS; 161 : 0 : default: 162 [ # # # # : 0 : throw std::runtime_error("Unknown value: " + std::to_string(value)); # # # # ] 163 : : } 164 : : } 165 : : 166 : 1 : GNSS GNSS_from_int(uint8_t value) 167 : : { 168 [ - - + ]: 1 : switch (value) { 169 : : case 0x10: 170 : : return GNSS::PVT_DATA; 171 : 0 : case 0x20: 172 : 0 : return GNSS::SATELLITES_INFO; 173 : 0 : default: 174 [ # # # # : 0 : throw std::runtime_error("Unknown value: " + std::to_string(value)); # # # # ] 175 : : } 176 : : } 177 : : 178 : : } // namespace xsens_driver 179 : : } // namespace drivers 180 : : } // namespace autoware