| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | #include "EmlidUartSensor.hpp" | ||
| 10 | |||
| 11 | #include "EmlidUtilities.hpp" | ||
| 12 | #include "util/Logger.hpp" | ||
| 13 | |||
| 14 | 228 | NAV::vendor::emlid::EmlidUartSensor::EmlidUartSensor(std::string name) | |
| 15 |
2/4✓ Branch 3 taken 228 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 228 times.
✗ Branch 7 not taken.
|
456 | : _name(std::move(name)), _buffer(uart::sensors::UartSensor::DefaultReadBufferSize) |
| 16 | { | ||
| 17 |
1/2✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
|
228 | resetTracking(); |
| 18 | 228 | } | |
| 19 | |||
| 20 | 228 | void NAV::vendor::emlid::EmlidUartSensor::resetTracking() | |
| 21 | { | ||
| 22 | 228 | _currentlyBuildingBinaryPacket = false; | |
| 23 | 228 | _currentlyBuildingAsciiPacket = false; | |
| 24 | |||
| 25 | 228 | _asciiEndChar1Found = false; | |
| 26 | 228 | _binarySyncChar2Found = false; | |
| 27 | 228 | _binaryMsgIdFound = false; | |
| 28 | 228 | _binaryPayloadLength1Found = false; | |
| 29 | 228 | _binaryPayloadLength2Found = false; | |
| 30 | |||
| 31 | 228 | _binaryMsgId = 0; | |
| 32 | 228 | _binaryPayloadLength = 0; | |
| 33 | |||
| 34 | 228 | _buffer.resize(0); | |
| 35 | 228 | _numOfBytesRemainingForCompletePacket = 0; | |
| 36 | 228 | } | |
| 37 | |||
| 38 | ✗ | std::unique_ptr<uart::protocol::Packet> NAV::vendor::emlid::EmlidUartSensor::findPacket(uint8_t dataByte) | |
| 39 | { | ||
| 40 | ✗ | if (_buffer.size() == _buffer.capacity()) | |
| 41 | { | ||
| 42 | // Buffer is full | ||
| 43 | ✗ | resetTracking(); | |
| 44 | ✗ | LOG_ERROR("{}: Discarding current packet, because buffer is full.", _name); | |
| 45 | } | ||
| 46 | |||
| 47 | ✗ | if (!_currentlyBuildingAsciiPacket && !_currentlyBuildingBinaryPacket) | |
| 48 | { | ||
| 49 | // This byte must be the start char | ||
| 50 | ✗ | if (dataByte == BINARY_SYNC_CHAR_1) | |
| 51 | { | ||
| 52 | ✗ | resetTracking(); | |
| 53 | ✗ | _currentlyBuildingBinaryPacket = true; | |
| 54 | ✗ | _buffer.push_back(dataByte); | |
| 55 | } | ||
| 56 | ✗ | else if (dataByte == ASCII_START_CHAR) | |
| 57 | { | ||
| 58 | ✗ | resetTracking(); | |
| 59 | ✗ | _currentlyBuildingAsciiPacket = true; | |
| 60 | ✗ | _buffer.push_back(dataByte); | |
| 61 | } | ||
| 62 | } | ||
| 63 | ✗ | else if (_currentlyBuildingBinaryPacket) | |
| 64 | { | ||
| 65 | ✗ | _buffer.push_back(dataByte); | |
| 66 | |||
| 67 | ✗ | if (!_binarySyncChar2Found) | |
| 68 | { | ||
| 69 | // This byte must be the second sync char | ||
| 70 | ✗ | if (dataByte == BINARY_SYNC_CHAR_2) | |
| 71 | { | ||
| 72 | ✗ | _binarySyncChar2Found = true; | |
| 73 | } | ||
| 74 | else | ||
| 75 | { | ||
| 76 | ✗ | resetTracking(); | |
| 77 | } | ||
| 78 | } | ||
| 79 | ✗ | else if (!_binaryMsgIdFound) | |
| 80 | { | ||
| 81 | // This byte must be the message id | ||
| 82 | ✗ | _binaryMsgIdFound = true; | |
| 83 | ✗ | _binaryMsgId = dataByte; | |
| 84 | } | ||
| 85 | ✗ | else if (!_binaryPayloadLength1Found) | |
| 86 | { | ||
| 87 | ✗ | _binaryPayloadLength1Found = true; | |
| 88 | ✗ | _binaryPayloadLength = static_cast<uint16_t>(dataByte); | |
| 89 | } | ||
| 90 | ✗ | else if (!_binaryPayloadLength2Found) | |
| 91 | { | ||
| 92 | ✗ | _binaryPayloadLength2Found = true; | |
| 93 | ✗ | _binaryPayloadLength |= static_cast<uint16_t>(static_cast<uint16_t>(dataByte) << 8U); | |
| 94 | ✗ | _binaryPayloadLength = uart::stoh(_binaryPayloadLength, ENDIANNESS); | |
| 95 | ✗ | _numOfBytesRemainingForCompletePacket = _binaryPayloadLength + 2U; | |
| 96 | LOG_DATA("{}: Binary packet: Id={:0x}, payload length={}", _name, _binaryMsgId, _binaryPayloadLength); | ||
| 97 | } | ||
| 98 | else | ||
| 99 | { | ||
| 100 | // We are currently collecting data for our packet. | ||
| 101 | ✗ | _numOfBytesRemainingForCompletePacket--; | |
| 102 | |||
| 103 | ✗ | if (_numOfBytesRemainingForCompletePacket == 0) | |
| 104 | { | ||
| 105 | // We have a possible binary packet! | ||
| 106 | ✗ | auto p = std::make_unique<uart::protocol::Packet>(_buffer, &_sensor); | |
| 107 | |||
| 108 | ✗ | if (p->isValid()) | |
| 109 | { | ||
| 110 | // We have a valid binary packet!!!. | ||
| 111 | ✗ | resetTracking(); | |
| 112 | ✗ | return p; | |
| 113 | } | ||
| 114 | // Invalid packet! | ||
| 115 | ✗ | LOG_DEBUG("{}: Invalid binary packet: Id={:0x}, payload length={}", _name, _binaryMsgId, _binaryPayloadLength); | |
| 116 | ✗ | resetTracking(); | |
| 117 | ✗ | } | |
| 118 | } | ||
| 119 | } | ||
| 120 | ✗ | else if (_currentlyBuildingAsciiPacket) | |
| 121 | { | ||
| 122 | ✗ | _buffer.push_back(dataByte); | |
| 123 | |||
| 124 | ✗ | if (dataByte == ASCII_ESCAPE_CHAR) | |
| 125 | { | ||
| 126 | ✗ | resetTracking(); | |
| 127 | } | ||
| 128 | ✗ | else if (dataByte == ASCII_END_CHAR_1) | |
| 129 | { | ||
| 130 | ✗ | _asciiEndChar1Found = true; | |
| 131 | } | ||
| 132 | ✗ | else if (_asciiEndChar1Found) | |
| 133 | { | ||
| 134 | ✗ | if (dataByte == ASCII_END_CHAR_2) | |
| 135 | { | ||
| 136 | // We have a possible data packet | ||
| 137 | ✗ | auto p = std::make_unique<uart::protocol::Packet>(_buffer, &_sensor); | |
| 138 | |||
| 139 | ✗ | if (p->isValid()) | |
| 140 | { | ||
| 141 | // We have a valid ascii packet!!!. | ||
| 142 | LOG_DATA("{}: Valid ascii packet: {}", _name, p->datastr().substr(0, p->getRawDataLength() - 2)); | ||
| 143 | ✗ | resetTracking(); | |
| 144 | ✗ | return p; | |
| 145 | } | ||
| 146 | // Invalid packet! | ||
| 147 | ✗ | LOG_ERROR("Invalid ascii packet: {}", p->datastr()); | |
| 148 | ✗ | } | |
| 149 | |||
| 150 | ✗ | resetTracking(); | |
| 151 | } | ||
| 152 | } | ||
| 153 | |||
| 154 | ✗ | return nullptr; | |
| 155 | } | ||
| 156 | |||
| 157 | ✗ | void NAV::vendor::emlid::EmlidUartSensor::packetFinderFunction(const std::vector<uint8_t>& data, const uart::xplat::TimeStamp& timestamp, uart::sensors::UartSensor::ValidPacketFoundHandler dispatchPacket, void* dispatchPacketUserData, void* userData) | |
| 158 | { | ||
| 159 | ✗ | auto* sensor = static_cast<EmlidUartSensor*>(userData); | |
| 160 | |||
| 161 | ✗ | for (size_t i = 0; i < data.size(); i++, sensor->_runningDataIndex++) | |
| 162 | { | ||
| 163 | ✗ | auto packetPointer = sensor->findPacket(data.at(i)); | |
| 164 | |||
| 165 | ✗ | if (packetPointer != nullptr) | |
| 166 | { | ||
| 167 | ✗ | uart::protocol::Packet packet = *packetPointer; | |
| 168 | ✗ | dispatchPacket(dispatchPacketUserData, packet, sensor->_runningDataIndex, timestamp); | |
| 169 | ✗ | } | |
| 170 | ✗ | } | |
| 171 | ✗ | } | |
| 172 | |||
| 173 | ✗ | uart::protocol::Packet::Type NAV::vendor::emlid::EmlidUartSensor::packetTypeFunction(const uart::protocol::Packet& packet) | |
| 174 | { | ||
| 175 | ✗ | if (packet.getRawDataLength() < 1) | |
| 176 | { | ||
| 177 | ✗ | LOG_CRITICAL("Packet does not contain any data."); | |
| 178 | } | ||
| 179 | |||
| 180 | ✗ | if (packet.getRawData().at(0) == '$') | |
| 181 | { | ||
| 182 | ✗ | return uart::protocol::Packet::Type::TYPE_ASCII; | |
| 183 | } | ||
| 184 | ✗ | if (packet.getRawData().at(0) == BINARY_SYNC_CHAR_1) | |
| 185 | { | ||
| 186 | ✗ | if (packet.getRawData().at(1) == BINARY_SYNC_CHAR_2) | |
| 187 | { | ||
| 188 | ✗ | return uart::protocol::Packet::Type::TYPE_BINARY; | |
| 189 | } | ||
| 190 | } | ||
| 191 | |||
| 192 | ✗ | return uart::protocol::Packet::Type::TYPE_UNKNOWN; | |
| 193 | } | ||
| 194 | |||
| 195 | ✗ | bool NAV::vendor::emlid::EmlidUartSensor::checksumFunction(const uart::protocol::Packet& packet) | |
| 196 | { | ||
| 197 | ✗ | if (packet.getRawDataLength() <= 8) | |
| 198 | { | ||
| 199 | ✗ | return false; | |
| 200 | } | ||
| 201 | |||
| 202 | ✗ | if (packet.type() == uart::protocol::Packet::Type::TYPE_ASCII) | |
| 203 | { | ||
| 204 | ✗ | return true; | |
| 205 | } | ||
| 206 | |||
| 207 | ✗ | if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) | |
| 208 | { | ||
| 209 | ✗ | std::pair<uint8_t, uint8_t> checksum = emlid::checksumUBX(packet.getRawData()); | |
| 210 | |||
| 211 | ✗ | return packet.getRawData().at(packet.getRawDataLength() - 2) == checksum.first | |
| 212 | ✗ | && packet.getRawData().at(packet.getRawDataLength() - 1) == checksum.second; | |
| 213 | } | ||
| 214 | |||
| 215 | ✗ | LOG_CRITICAL("Can't calculate checksum of packet with unknown type"); | |
| 216 | return false; | ||
| 217 | } | ||
| 218 | |||
| 219 | ✗ | bool NAV::vendor::emlid::EmlidUartSensor::isErrorFunction([[maybe_unused]] const uart::protocol::Packet& packet) | |
| 220 | { | ||
| 221 | ✗ | return false; | |
| 222 | } | ||
| 223 | |||
| 224 | ✗ | bool NAV::vendor::emlid::EmlidUartSensor::isResponseFunction([[maybe_unused]] const uart::protocol::Packet& packet) | |
| 225 | { | ||
| 226 | ✗ | return false; | |
| 227 | } | ||
| 228 |