INSTINCT Code Coverage Report

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 "Ln200UartSensor.hpp"
10
11			#include "util/Logger.hpp"
12
13		✗	NAV::vendor::ln::Ln200UartSensor::Ln200UartSensor(std::string name)
14		✗	: _name(std::move(name)), _bitBuffer(uart::sensors::UartSensor::DefaultReadBufferSize), _flagWindow(8)
15		✗	{}
16
17		✗	std::unique_ptr<uart::protocol::Packet> NAV::vendor::ln::Ln200UartSensor::findPacket(uint8_t dataByte)
18			{
19			// If we receive a full idle byte (0xFF), mark the idle period.
20		✗	if (dataByte == 0xFF)
21			{
22		✗	_wasIdle = true;
23			}
24
25			// Define the lambda for byte-to-bits conversion.
26		✗	auto byteToBits = [](uint8_t byte) -> std::vector<bool> {
27		✗	std::vector<bool> bits(8);
28		✗	for (uint8_t i = 0; i < 8; ++i)
29			{
30		✗	bits[7 - i] = (byte >> i) & 0x1;
31			}
32		✗	return bits;
33		✗	};
34
35			// Lambda for checking the flag sequence.
36		✗	auto isFlagSequence = [](const ScrollingBuffer<bool>& window) -> bool {
37			// The flag is 01111110.
38		✗	constexpr std::bitset<8> flagPattern("01111110");
39
40			// Compare the available bits in the window with the corresponding bits in the flag pattern.
41		✗	for (size_t i = 0; i < window.size(); ++i)
42			{
43		✗	if (window.at(i) != flagPattern[i])
44			{
45		✗	return false;
46			}
47			}
48		✗	return true;
49			};
50
51		✗	auto bitBufferToByteVector = [](const ScrollingBuffer<bool>& bitBuffer) -> std::vector<uint8_t> {
52		✗	std::vector<uint8_t> byteVector((bitBuffer.size() + 7) / 8, 0); // Allocate enough bytes
53		✗	for (size_t i = 0; i < bitBuffer.size(); ++i)
54			{
55		✗	if (bitBuffer.at(i))
56			{
57		✗	byteVector[i / 8] |= (1 << (7 - (i % 8))); // Set the corresponding bit in the byte
58			}
59			}
60		✗	return byteVector;
61		✗	};
62
63			// Convert the received byte to bits.
64		✗	auto bits = byteToBits(dataByte);
65		✗	for (auto bit : bits)
66			{
67			// Update the flag window (we use a sliding window of 8 bits).
68		✗	_flagWindow.push_back(bit);
69		✗	if (_flagWindow.size() > 8)
70			{
71		✗	_flagWindow.pop_front();
72			}
73
74			// Check for flag sequence.
75		✗	if (_flagWindow.size() == 8 && isFlagSequence(_flagWindow))
76			{
77		✗	if (_state == State::Receiving)
78			{
79			// End of frame detected.
80		✗	if (!_bitBuffer.empty())
81			{
82			// Remove the flag bits from the _bitBuffer (assume the last 7 bits are the flag).
83		✗	if (_bitBuffer.size() >= 7)
84			{
85		✗	for (int i = 0; i < 7; ++i)
86			{
87		✗	_bitBuffer.pop_back();
88			}
89			}
90		✗	std::vector<uint8_t> byteVector = bitBufferToByteVector(_bitBuffer);
91			LOG_DATA("{}: Valid binary packet: Length={}", _name, _bitBuffer.size());
92		✗	auto p = std::make_unique<uart::protocol::Packet>(byteVector, &_sensor);
93		✗	_bitBuffer.clear();
94		✗	_state = State::Idle;
95		✗	_consecutiveOnes = 0;
96		✗	_wasIdle = false; // Clear the idle flag.
97		✗	return p;
98		✗	}
99		✗	_state = State::Idle;
100		✗	_wasIdle = false;
101			}
102			else // _state == Idle
103			{
104			// Only consider this flag as a start sequence if the bus was idle (0xFF) just before.
105		✗	if (_wasIdle)
106			{
107			// Start of frame detected.
108		✗	_state = State::Receiving;
109		✗	_consecutiveOnes = 0;
110		✗	_bitBuffer.clear(); // Clear any previous payload.
111		✗	_wasIdle = false; // Reset the idle flag.
112			}
113			// If _wasIdle is false, then the flag might be an ending flag from a previous message.
114			}
115		✗	continue;
116		✗	}
117
118		✗	if (_state == State::Receiving)
119			{
120			// If the current bit is false and it follows five consecutive ones,
121			// it's a stuffed zero which should be ignored.
122		✗	if (!bit && _consecutiveOnes == 5)
123			{
124		✗	_consecutiveOnes = 0;
125		✗	continue;
126			}
127
128			// For a true bit, increment the counter; for a false bit, reset it.
129		✗	_consecutiveOnes = bit ? (_consecutiveOnes + 1) : 0;
130
131			// Append the bit to the buffer.
132		✗	_bitBuffer.push_back(bit);
133			}
134			}
135		✗	return nullptr;
136		✗	}
137
138		✗	void NAV::vendor::ln::Ln200UartSensor::packetFinderFunction(const std::vector<uint8_t>& data, const uart::xplat::TimeStamp& timestamp, uart::sensors::UartSensor::ValidPacketFoundHandler dispatchPacket, void* dispatchPacketUserData, void* userData)
139			{
140		✗	auto* sensor = static_cast<Ln200UartSensor*>(userData);
141
142		✗	for (size_t i = 0; i < data.size(); ++i, sensor->_runningDataIndex++)
143			{
144		✗	auto packetPointer = sensor->findPacket(data.at(i));
145
146		✗	if (packetPointer != nullptr)
147			{
148		✗	uart::protocol::Packet packet = *packetPointer;
149		✗	dispatchPacket(dispatchPacketUserData, packet, sensor->_runningDataIndex, timestamp);
150		✗	}
151		✗	}
152		✗	}
153
154		✗	uart::protocol::Packet::Type NAV::vendor::ln::Ln200UartSensor::packetTypeFunction([[maybe_unused]] const uart::protocol::Packet& packet)
155			{
156		✗	return uart::protocol::Packet::Type::TYPE_BINARY;
157			}
158
159		✗	bool NAV::vendor::ln::Ln200UartSensor::checksumFunction(const uart::protocol::Packet& packet)
160			{
161			// TODO: Use 14th word of message as checksum
162		✗	if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY)
163			{
164		✗	return true;
165			}
166		✗	LOG_CRITICAL("Can't calculate checksum of packet with unknown type");
167			return false;
168			}
169
170		✗	bool NAV::vendor::ln::Ln200UartSensor::isErrorFunction([[maybe_unused]] const uart::protocol::Packet& packet)
171			{
172		✗	return false;
173			}
174
175		✗	bool NAV::vendor::ln::Ln200UartSensor::isResponseFunction([[maybe_unused]] const uart::protocol::Packet& packet)
176			{
177		✗	return false;
178			}
179