monitoring_frame_deserialization.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020-2022 Pilz GmbH & Co. KG
2 //
3 // This program is free software: you can redistribute it and/or modify
4 // it under the terms of the GNU Lesser General Public License as published by
5 // the Free Software Foundation, either version 3 of the License, or
6 // (at your option) any later version.
7 //
8 // This program is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 // GNU Lesser General Public License for more details.
12 //
13 // You should have received a copy of the GNU Lesser General Public License
14 // along with this program. If not, see <https://www.gnu.org/licenses/>.
15 
16 #include <array>
17 #include <functional>
18 #include <istream>
19 #include <sstream>
20 #include <vector>
21 
22 #include <fmt/format.h>
23 
33 
35 {
36 namespace data_conversion_layer
37 {
38 namespace monitoring_frame
39 {
40 using namespace std::placeholders;
41 
42 AdditionalFieldHeader::AdditionalFieldHeader(Id id, Length length) : id_(id), length_(length)
43 {
44 }
45 
47  OpCode op_code,
48  WorkingMode working_mode,
49  TransactionType transaction_type,
50  configuration::ScannerId scanner_id,
51  FromTheta from_theta,
52  Resolution resolution)
53  : device_status_(device_status)
54  , op_code_(op_code)
55  , working_mode_(working_mode)
56  , transaction_type_(transaction_type)
57  , scanner_id_(scanner_id)
58  , from_theta_(from_theta)
59  , resolution_(resolution)
60 {
61 }
62 
63 static constexpr double toMeter(const uint16_t& value)
64 {
65  if ((value == NO_SIGNAL_ARRIVED) || (value == SIGNAL_TOO_LATE))
66  {
67  return std::numeric_limits<double>::infinity();
68  }
69  return static_cast<double>(value) / 1000.;
70 }
71 
72 static constexpr double toIntensities(const uint16_t& value)
73 {
74  // Neglegt the first two bytes.
75  uint16_t retval{ value };
76  return static_cast<double>(retval & 0b0011111111111111);
77 }
78 
79 monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data, const std::size_t& num_bytes)
80 {
82 
83  std::stringstream ss;
84  ss.write(data.data(), num_bytes);
85 
86  FixedFields frame_header = readFixedFields(ss);
87 
88  msg_builder.scannerId(frame_header.scannerId());
89  msg_builder.fromTheta(frame_header.fromTheta());
90  msg_builder.resolution(frame_header.resolution());
91 
92  bool end_of_frame{ false };
93  while (!end_of_frame)
94  {
95  const AdditionalFieldHeader additional_header{ readAdditionalField(ss, num_bytes) };
96  switch (static_cast<AdditionalFieldHeaderID>(additional_header.id()))
97  {
99  if (additional_header.length() != NUMBER_OF_BYTES_SCAN_COUNTER)
100  {
101  throw AdditionalFieldUnexpectedSize(fmt::format("Length of scan counter field is {}, but should be {}.",
102  additional_header.length(),
104  }
105  uint32_t scan_counter_read_buffer;
106  raw_processing::read<uint32_t>(ss, scan_counter_read_buffer);
107  msg_builder.scanCounter(scan_counter_read_buffer);
108  break;
109 
111  const size_t num_measurements{ static_cast<size_t>(additional_header.length()) /
113  std::vector<double> measurements;
114  raw_processing::readArray<uint16_t, double>(ss, measurements, num_measurements, toMeter);
115  msg_builder.measurements(measurements);
116  break;
117  }
119  end_of_frame = true;
120  break;
121 
123  if (additional_header.length() != NUMBER_OF_BYTES_ZONE_SET)
124  {
125  throw AdditionalFieldUnexpectedSize(fmt::format("Length of zone set field is {}, but should be {}.",
126  additional_header.length(),
128  }
129  uint8_t zone_set_read_buffer;
130  raw_processing::read<uint8_t>(ss, zone_set_read_buffer);
131  msg_builder.activeZoneset(zone_set_read_buffer);
132  break;
133 
135  if (additional_header.length() != io::RAW_CHUNK_LENGTH_IN_BYTES)
136  {
137  throw AdditionalFieldUnexpectedSize(fmt::format("Length of io state field is {}, but should be {}.",
138  additional_header.length(),
140  }
141  msg_builder.iOPinData(io::deserializePins(ss));
142  break;
143 
146  break;
147 
149  const size_t num_measurements{ static_cast<size_t>(additional_header.length()) /
151  std::vector<double> intensities;
152  raw_processing::readArray<uint16_t, double>(ss, intensities, num_measurements, std::bind(toIntensities, _1));
153  msg_builder.intensities(intensities);
154  break;
155  }
156  default:
157  throw DecodingFailure(
158  fmt::format("Header Id {:#04x} unknown. Cannot read additional field of monitoring frame on position {}.",
159  additional_header.id(),
160  ss.tellp()));
161  }
162  }
163  return msg_builder.build();
164 }
165 
166 AdditionalFieldHeader readAdditionalField(std::istream& is, const std::size_t& max_num_bytes)
167 {
168  auto const id = raw_processing::read<AdditionalFieldHeader::Id>(is);
169  auto length = raw_processing::read<AdditionalFieldHeader::Length>(is);
170 
171  if (length >= max_num_bytes)
172  {
173  throw DecodingFailure(
174  fmt::format("Length given in header of additional field is too large: {}, id: {:#04x}", length, id));
175  }
176  if (length > 0)
177  {
178  length--;
179  }
180  return AdditionalFieldHeader(id, length);
181 }
182 
183 namespace io
184 {
185 PinData deserializePins(std::istream& is)
186 {
188 
190  std::array<uint8_t, 3 * (RAW_CHUNK_LENGTH_RESERVED_IN_BYTES + RAW_CHUNK_PHYSICAL_INPUT_SIGNALS_IN_BYTES)>>(is);
191 
192  raw_processing::read<std::array<uint8_t, RAW_CHUNK_LENGTH_RESERVED_IN_BYTES>>(is);
193  deserializePinField(is, io_pin_data.input_state);
194 
195  raw_processing::read<std::array<uint8_t, RAW_CHUNK_LENGTH_RESERVED_IN_BYTES>>(is);
196  deserializePinField(is, io_pin_data.output_state);
197 
198  return io_pin_data;
199 }
200 } // namespace io
201 
202 namespace diagnostic
203 {
204 std::vector<diagnostic::Message> deserializeMessages(std::istream& is)
205 {
206  std::vector<diagnostic::Message> diagnostic_messages;
207 
208  // Read-in unused data fields
209  raw_processing::read<std::array<uint8_t, diagnostic::RAW_CHUNK_UNUSED_OFFSET_IN_BYTES>>(is);
210 
211  for (const auto& scanner_id : configuration::VALID_SCANNER_IDS)
212  {
213  for (size_t byte_n = 0; byte_n < diagnostic::RAW_CHUNK_LENGTH_FOR_ONE_DEVICE_IN_BYTES; byte_n++)
214  {
215  const auto raw_byte = raw_processing::read<uint8_t>(is);
216  const std::bitset<8> raw_bits(raw_byte);
217 
218  for (size_t bit_n = 0; bit_n < raw_bits.size(); ++bit_n)
219  {
220  if (raw_bits.test(bit_n) && (diagnostic::ErrorType::unused != diagnostic::ERROR_BITS[byte_n][bit_n]))
221  {
222  diagnostic_messages.push_back(diagnostic::Message(static_cast<configuration::ScannerId>(scanner_id),
223  diagnostic::ErrorLocation(byte_n, bit_n)));
224  }
225  }
226  }
227  }
228  return diagnostic_messages;
229 }
230 } // namespace diagnostic
231 
232 FixedFields readFixedFields(std::istream& is)
233 {
234  const auto device_status = raw_processing::read<FixedFields::DeviceStatus>(is);
235  const auto op_code = raw_processing::read<FixedFields::OpCode>(is);
236  const auto working_mode = raw_processing::read<FixedFields::WorkingMode>(is);
237  const auto transaction_type = raw_processing::read<FixedFields::TransactionType>(is);
238  const auto scanner_id = raw_processing::read<configuration::ScannerId>(is);
239 
240  const auto from_theta = raw_processing::read<int16_t, FixedFields::FromTheta>(is);
241  const auto resolution = raw_processing::read<int16_t, FixedFields::Resolution>(is);
242 
243  // LCOV_EXCL_START
244  if (OP_CODE_MONITORING_FRAME != op_code)
245  {
247  0.1, "monitoring_frame::Message", "Unexpected opcode during deserialization of MonitoringFrame.");
248  }
249 
250  if (ONLINE_WORKING_MODE != working_mode)
251  {
252  PSENSCAN_ERROR_THROTTLE(0.1, "monitoring_frame::Message", "Invalid working mode (not online)");
253  }
254 
255  if (GUI_MONITORING_TRANSACTION != transaction_type)
256  {
257  PSENSCAN_ERROR_THROTTLE(0.1, "monitoring_frame::Message", "Invalid transaction type.");
258  }
259 
260  if (MAX_SCANNER_ID < static_cast<uint8_t>(scanner_id))
261  {
262  PSENSCAN_ERROR_THROTTLE(0.1, "monitoring_frame::Message", "Invalid Scanner id.");
263  }
264  // LCOV_EXCL_STOP
265 
266  return FixedFields(device_status, op_code, working_mode, transaction_type, scanner_id, from_theta, resolution);
267 }
268 } // namespace monitoring_frame
269 } // namespace data_conversion_layer
270 } // namespace psen_scan_v2_standalone
Higher level data type representing a single monitoring frame.
Defines an Diagnostic message by defining the ErrorLocation and a scanner ID.
Definition: diagnostics.h:181
monitoring_frame::Message deserialize(const data_conversion_layer::RawData &data, const std::size_t &num_bytes)
AdditionalFieldHeader readAdditionalField(std::istream &is, const std::size_t &max_num_bytes)
static constexpr std::array< ScannerId, 4 > VALID_SCANNER_IDS
Definition: scanner_ids.h:35
MessageBuilder & diagnosticMessages(const std::vector< diagnostic::Message > &diagnostic_messages)
static constexpr uint32_t RAW_CHUNK_LENGTH_FOR_ONE_DEVICE_IN_BYTES
Contains constants and types needed to define the diagnostic::Message.
Definition: diagnostics.h:45
FixedFields(DeviceStatus device_status, OpCode op_code, WorkingMode working_mode, TransactionType transaction_type, configuration::ScannerId scanner_id, FromTheta from_theta, Resolution resolution)
Represents the IO PIN field of a monitoring frame.
Definition: io_pin_data.h:72
void deserializePinField(std::istream &is, std::array< std::bitset< 8 >, ChunkSize > &pin_states)
static constexpr std::array< std::array< ErrorType, 8 >, 9 > ERROR_BITS
Definition: diagnostics.h:123
#define PSENSCAN_ERROR_THROTTLE(period, name,...)
Definition: logging.h:74
Root namespace in which the software components to communicate with the scanner (firmware-version: 2)...
Definition: udp_client.h:41
Definition for the type and length of an additional field in a monitoring frame.
Exception thrown on problems during the extraction of the measurement data.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Defines a byte and bit position of an error in the diagnostic chunk.
Definition: diagnostics.h:148
std::array< std::bitset< 8 >, NUMBER_OF_OUTPUT_BYTES > output_state
Definition: io_pin_data.h:76
std::array< std::bitset< 8 >, NUMBER_OF_INPUT_BYTES > input_state
Definition: io_pin_data.h:75
Helper class representing angles in tenth of degree.


psen_scan_v2
Author(s): Pilz GmbH + Co. KG
autogenerated on Sat Nov 5 2022 02:13:36