| 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 "EspressifUartSensor.hpp" | ||
| 10 | |||
| 11 | #include "EspressifUtilities.hpp" | ||
| 12 | #include "util/Logger.hpp" | ||
| 13 | |||
| 14 | 112 | NAV::vendor::espressif::EspressifUartSensor::EspressifUartSensor(std::string name) | |
| 15 |
2/4✓ Branch 3 taken 112 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
|
224 | : _name(std::move(name)), _buffer(uart::sensors::UartSensor::DefaultReadBufferSize) |
| 16 | { | ||
| 17 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
112 | resetTracking(); |
| 18 | 112 | } | |
| 19 | |||
| 20 | 112 | void NAV::vendor::espressif::EspressifUartSensor::resetTracking() | |
| 21 | { | ||
| 22 | 112 | _currentlyBuildingBinaryPacket = false; | |
| 23 | |||
| 24 | 112 | _binarySyncChar2Found = false; | |
| 25 | 112 | _binaryPayloadLength1Found = false; | |
| 26 | 112 | _binaryPayloadLength2Found = false; | |
| 27 | |||
| 28 | 112 | _buffer.resize(0); | |
| 29 | 112 | _numOfBytesRemainingForCompletePacket = 0; | |
| 30 | 112 | } | |
| 31 | |||
| 32 | ✗ | std::unique_ptr<uart::protocol::Packet> NAV::vendor::espressif::EspressifUartSensor::findPacket(uint8_t dataByte) | |
| 33 | { | ||
| 34 | ✗ | if (_buffer.size() == _buffer.capacity()) | |
| 35 | { | ||
| 36 | // Buffer is full | ||
| 37 | ✗ | resetTracking(); | |
| 38 | ✗ | LOG_ERROR("{}: Discarding current packet, because buffer is full.", _name); | |
| 39 | } | ||
| 40 | |||
| 41 | ✗ | if (!_currentlyBuildingBinaryPacket) | |
| 42 | { | ||
| 43 | // This byte must be the start char | ||
| 44 | ✗ | if (dataByte == BINARY_SYNC_CHAR_1) | |
| 45 | { | ||
| 46 | ✗ | resetTracking(); | |
| 47 | ✗ | _currentlyBuildingBinaryPacket = true; | |
| 48 | ✗ | _buffer.push_back(dataByte); | |
| 49 | } | ||
| 50 | } | ||
| 51 | ✗ | else if (_currentlyBuildingBinaryPacket) | |
| 52 | { | ||
| 53 | ✗ | _buffer.push_back(dataByte); | |
| 54 | |||
| 55 | ✗ | if (!_binarySyncChar2Found) | |
| 56 | { | ||
| 57 | // This byte must be the second sync char | ||
| 58 | ✗ | if (dataByte == BINARY_SYNC_CHAR_2) | |
| 59 | { | ||
| 60 | ✗ | _binarySyncChar2Found = true; | |
| 61 | } | ||
| 62 | else | ||
| 63 | { | ||
| 64 | ✗ | resetTracking(); | |
| 65 | } | ||
| 66 | } | ||
| 67 | ✗ | else if (!_binaryPayloadLength1Found) | |
| 68 | { | ||
| 69 | ✗ | _binaryPayloadLength1Found = true; | |
| 70 | ✗ | _binaryPayloadLength = static_cast<uint16_t>(dataByte); | |
| 71 | } | ||
| 72 | ✗ | else if (!_binaryPayloadLength2Found) | |
| 73 | { | ||
| 74 | ✗ | _binaryPayloadLength2Found = true; | |
| 75 | ✗ | _binaryPayloadLength |= static_cast<uint16_t>(static_cast<uint16_t>(dataByte) << 8U); | |
| 76 | ✗ | _binaryPayloadLength = uart::stoh(_binaryPayloadLength, ENDIANNESS); | |
| 77 | ✗ | _numOfBytesRemainingForCompletePacket = _binaryPayloadLength + 2U; | |
| 78 | LOG_DATA("{}: Binary packet: payload length={}", _name, _binaryPayloadLength); | ||
| 79 | } | ||
| 80 | else | ||
| 81 | { | ||
| 82 | // We are currently collecting data for our packet. | ||
| 83 | ✗ | _numOfBytesRemainingForCompletePacket--; | |
| 84 | |||
| 85 | ✗ | if (_numOfBytesRemainingForCompletePacket == 0) | |
| 86 | { | ||
| 87 | // We have a possible binary packet! | ||
| 88 | ✗ | auto p = std::make_unique<uart::protocol::Packet>(_buffer, &_sensor); | |
| 89 | |||
| 90 | ✗ | if (p->isValid()) | |
| 91 | { | ||
| 92 | // We have a valid binary packet!!!. | ||
| 93 | ✗ | resetTracking(); | |
| 94 | ✗ | return p; | |
| 95 | } | ||
| 96 | // Invalid packet! | ||
| 97 | ✗ | LOG_ERROR("{}: Invalid binary packet: payload length={}", _name, _binaryPayloadLength); | |
| 98 | ✗ | resetTracking(); | |
| 99 | ✗ | } | |
| 100 | } | ||
| 101 | } | ||
| 102 | |||
| 103 | ✗ | return nullptr; | |
| 104 | } | ||
| 105 | |||
| 106 | ✗ | void NAV::vendor::espressif::EspressifUartSensor::packetFinderFunction(const std::vector<uint8_t>& data, const uart::xplat::TimeStamp& timestamp, uart::sensors::UartSensor::ValidPacketFoundHandler dispatchPacket, void* dispatchPacketUserData, void* userData) | |
| 107 | { | ||
| 108 | ✗ | auto* sensor = static_cast<EspressifUartSensor*>(userData); | |
| 109 | |||
| 110 | ✗ | for (size_t i = 0; i < data.size(); i++, sensor->_runningDataIndex++) | |
| 111 | { | ||
| 112 | ✗ | auto packetPointer = sensor->findPacket(data.at(i)); | |
| 113 | |||
| 114 | ✗ | if (packetPointer != nullptr) | |
| 115 | { | ||
| 116 | ✗ | uart::protocol::Packet packet = *packetPointer; | |
| 117 | ✗ | dispatchPacket(dispatchPacketUserData, packet, sensor->_runningDataIndex, timestamp); | |
| 118 | ✗ | } | |
| 119 | ✗ | } | |
| 120 | ✗ | } | |
| 121 | |||
| 122 | ✗ | uart::protocol::Packet::Type NAV::vendor::espressif::EspressifUartSensor::packetTypeFunction(const uart::protocol::Packet& packet) | |
| 123 | { | ||
| 124 | ✗ | if (packet.getRawData().at(0) == BINARY_SYNC_CHAR_1) | |
| 125 | { | ||
| 126 | ✗ | if (packet.getRawData().at(1) == BINARY_SYNC_CHAR_2) | |
| 127 | { | ||
| 128 | ✗ | return uart::protocol::Packet::Type::TYPE_BINARY; | |
| 129 | } | ||
| 130 | } | ||
| 131 | |||
| 132 | ✗ | return uart::protocol::Packet::Type::TYPE_UNKNOWN; | |
| 133 | } | ||
| 134 | |||
| 135 | ✗ | bool NAV::vendor::espressif::EspressifUartSensor::checksumFunction(const uart::protocol::Packet& packet) | |
| 136 | { | ||
| 137 | ✗ | if (packet.getRawDataLength() <= 8) | |
| 138 | { | ||
| 139 | ✗ | return false; | |
| 140 | } | ||
| 141 | |||
| 142 | ✗ | std::pair<uint8_t, uint8_t> checksum = ublox::checksumUBX(packet.getRawData()); | |
| 143 | |||
| 144 | ✗ | return packet.getRawData().at(packet.getRawDataLength() - 2) == checksum.first | |
| 145 | ✗ | && packet.getRawData().at(packet.getRawDataLength() - 1) == checksum.second; | |
| 146 | |||
| 147 | LOG_CRITICAL("Can't calculate checksum of packet with unknown type"); | ||
| 148 | return false; | ||
| 149 | } | ||
| 150 | |||
| 151 | ✗ | bool NAV::vendor::espressif::EspressifUartSensor::isErrorFunction([[maybe_unused]] const uart::protocol::Packet& packet) | |
| 152 | { | ||
| 153 | ✗ | return false; | |
| 154 | } | ||
| 155 | |||
| 156 | ✗ | bool NAV::vendor::espressif::EspressifUartSensor::isResponseFunction([[maybe_unused]] const uart::protocol::Packet& packet) | |
| 157 | { | ||
| 158 | ✗ | return false; | |
| 159 | } | ||
| 160 |