Decode DataFrame-s

notes.md

@@ -1,20 +1,19 @@
-
 ### TODO
 
- * [ ] Re-check all last-success timestamps (LongTimestamp) - they don't survive the overflow.
- * [ ] Add FTM input
  * [ ] Rework docs.
  * [ ] Assertion/termination system.
+ * [ ] Add FTM input
 
 Later:
  * [ ] Create multi-sensor geometry processing unit
  * [ ] Increase precision by keeping an estimate of cycle and removing uncertainty of long pulses.
+ * [ ] Re-check all last-success timestamps (LongTimestamp) - they don't survive the overflow.
  * [ ] Remove Timestamp in favor of std::chrono::duration (http:https://en.cppreference.com/w/cpp/chrono/duration)
  * [ ] Remove Vector in favor of std::vector.
  * [ ] Rewrite _sbrk() to not allow heap to go to into stack.
  * [ ] Add unit testing
  * [ ] Add polling mode for outputs
- * [ ] DataFrame: Check CRC32; Decode values.
+ * [ ] DataFrame: Check CRC32.
  * [ ] Split PersistentSettings to Settings and Persistent<>
  * [ ] Get rid of Teensy's Print. Use vsnprintf instead. debug_print, print_def, parse_def, DataChunkPrint
  * [ ] (Maybe) Introduce EASTL library and all its niceties like fixed_vector. Tried it and it looks problematic (platform not supported + threading issues).
@@ -26,6 +25,16 @@ https://google.github.io/styleguide/cppguide.html
  * Update submodules to latest on branches/tags: `git submodule update --remote`
  * When changing .gitmodules, do `git submodule sync`
 
+### Example Base Station Data Frames
+
+base0: fw 436, id 0x4242089a, desync 16, hw 9, accel [-4, 126, 127], mode B, faults 0 
+ fcal0: phase 0.0500, tilt -0.0091, curve -0.0038, gibphase 1.8633, gibmag 0.0113 
+ fcal1: phase 0.0241, tilt 0.0008, curve -0.0015, gibphase -0.7920, gibmag -0.0112 (2 total)
+base1: fw 436, id 0x2f855022, desync 53, hw 9, accel [0, 118, 127], mode C, faults 0 
+ fcal0: phase 0.0284, tilt -0.0036, curve -0.0010, gibphase 2.1758, gibmag 0.0086 
+ fcal1: phase 0.0487, tilt -0.0080, curve -0.0023, gibphase 0.4695, gibmag -0.0086 (1 total)
+
+
 ### GCC Headers search for #include <..>
 
  /usr/local/Caskroom/gcc-arm-embedded/6_2-2016q4,20161216/gcc-arm-none-eabi-6_2-2016q4

src/data_frame_decoder.h

@@ -1,12 +1,32 @@
+#pragma once
 #include "primitives/workers.h"
 #include "primitives/producer_consumer.h"
 #include "common.h"
 
-// Data Frame Decoder for one base station.
 // See data frame description here: 
 // https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md#ootx-frame
 // https://github.com/nairol/LighthouseRedox/blob/master/docs/Base%20Station.md#base-station-info-block
-// Frame is 33 bytes long. NOTE: If we would want to decode float16, we can use ARM specific __fp16 type.
+struct DecodedDataFrame {
+ // The current protocol version is 6. For older protocol versions the data structure might look different.
+ static constexpr uint32_t cur_protocol = 6;
+
+ uint8_t protocol : 6; // Protocol version (bits 5..0).
+ uint16_t fw_version : 10; // Firmware version (bits 15..6).
+ uint32_t id; // Unique identifier of the base station (CRC32 of the 128-bit MCU UID)
+ __fp16 fcal_phase[2]; // "phase" - probably phase difference between real angle and measured.
+ __fp16 fcal_tilt[2]; // "tilt" - probably rotation of laser plane
+ uint8_t sys_unlock_count; // Lowest 8 bits of the rotor desynchronization counter
+ uint8_t hw_version; // Hardware version
+ __fp16 fcal_curve[2]; // "curve"
+ int8_t accel_dir[3]; // Orientation vector, scaled so that largest component is always +-127.
+ __fp16 fcal_gibphase[2]; // "gibbous phase" (normalized angle)
+ __fp16 fcal_gibmag[2]; // "gibbous magnitude"
+ uint8_t mode_current; // Currently selected mode (default: 0=A, 1=B, 2=C)
+ uint8_t sys_faults; // "fault detect flags" (should be 0)
+} __attribute__((packed));
+static_assert(sizeof(DecodedDataFrame) == 33, "DataFrame should be 33 bytes long. Check it's byte-level packed.");
+
+// Data Frame Decoder for one base station.
 // TODO: Check CRC32.
 class DataFrameDecoder
  : public WorkerNode

src/message_logging.h

@@ -5,6 +5,7 @@
 #include "primitives/producer_consumer.h"
 #include "primitives/circular_buffer.h"
 #include "primitives/hash.h"
+#include "data_frame_decoder.h"
 #include <Arduino.h>
 
 // This function is used to print messages used in Producer-Consumer pattern.
@@ -42,9 +43,22 @@ inline void print_value<DataFrameBit>(Print &stream, const DataFrameBit& val) {
 
 template<>
 inline void print_value<DataFrame>(Print &stream, const DataFrame& frame) {
- stream.printf("\n%dms: base %d, data ", frame.time.get_value(msec), frame.base_station_idx);
- for (uint32_t i = 0; i < frame.bytes.size(); i++)
- stream.printf("%02X ", frame.bytes[i]);
+ stream.printf("\n%dms: ", frame.time.get_value(msec));
+ const DecodedDataFrame *df = reinterpret_cast<const DecodedDataFrame *>(&frame.bytes[0]);
+ if (frame.bytes.size() == 33 && df->protocol == DecodedDataFrame::cur_protocol) {
+ stream.printf("fw %u, id 0x%08x, desync %u, hw %u, accel [%d, %d, %d], mode %c, faults %u ", 
+ (uint32_t)df->fw_version, df->id, (uint32_t)df->sys_unlock_count, (uint32_t)df->hw_version, 
+ (int32_t)df->accel_dir[0], (int32_t)df->accel_dir[1], (int32_t)df->accel_dir[2],
+ df->mode_current+'A', (uint32_t)df->sys_faults);
+ for (uint32_t i = 0; i < num_base_stations; i++)
+ stream.printf("\n fcal%d: phase %.4f, tilt %.4f, curve %.4f, gibphase %.4f, gibmag %.4f ", 
+ i, (float)df->fcal_phase[i], (float)df->fcal_tilt[i], (float)df->fcal_curve[i], (float)df->fcal_gibphase[i], (float)df->fcal_gibmag[i]);
+ } else {
+ // Unknown protocol.
+ stream.printf("bytes ");
+ for (uint32_t i = 0; i < frame.bytes.size(); i++)
+ stream.printf("%02X ", frame.bytes[i]);
+ }
 }
 
 template<>

src/pulse_processor.cpp

@@ -125,8 +125,10 @@ void PulseProcessor::process_cycle_fix(Timestamp cur_time) {
  if (Producer<DataFrameBit>::has_consumers()) {
  DataFrameBitPair bits = phase_classifier_.get_data_bits(cycle_idx_, pulse_lens);
  for (int b = 0; b < num_base_stations; b++)
- if (bits[b].cycle_idx == cycle_idx_)
+ if (bits[b].cycle_idx == cycle_idx_) {
+ bits[b].time = cycle_start_time_;
  Producer<DataFrameBit>::produce(bits[b]);
+ }
  }
 
  } else {