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://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 {