Code cleanup

notes.md

@@ -6,6 +6,8 @@
 
 Later:
  * [ ] Create multi-sensor geometry processing unit
+ * [ ] Increase precision by applying geometry adjustments for base stations. 1:1 with Unity.
+ * [ ] Create Unity tutorial.
  * [ ] 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)
@@ -14,6 +16,7 @@ Later:
  * [ ] Add unit testing
  * [ ] Add polling mode for outputs
  * [ ] DataFrame: Check CRC32.
+ * [ ] Avoid using double (-Wdouble-promotion). This will require killing all printf-s.
  * [ ] 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).

src/cycle_phase_classifier.cpp

@@ -58,7 +58,7 @@ inline float CyclePhaseClassifier::expected_pulse_len(bool skip, bool data, bool
  return pulse_base_len_ + (skip << 2 | data << 1 | axis) * 10.416f;
 }
 
-DataFrameBitPair CyclePhaseClassifier::get_data_bits(uint32_t cycle_idx, const TimeDelta (&pulse_lens)[num_base_stations]) {
+CyclePhaseClassifier::DataFrameBitPair CyclePhaseClassifier::get_data_bits(uint32_t cycle_idx, const TimeDelta (&pulse_lens)[num_base_stations]) {
  // This is almost naive algorithm that tracks/adjusts just one variable, pulse_base_len_, with the assumption that
  // all pulses can be shorter/longer than ideal by the same amount.
  // We might need to introduce tracking of each phase mid_len if sensors are not linear enough.

src/cycle_phase_classifier.h

@@ -1,10 +1,6 @@
 #pragma once
-#include "common.h"
+#include "messages.h"
 #include "primitives/string_utils.h"
-class Print;
-
-// Reference to a pair of DataFrameBit-s
-typedef DataFrameBit (&DataFrameBitPair)[num_base_stations];
 
 // Given pairs of pulse lens from 2 base stations, this class determines the phase for current cycle
 // Phases are: 
@@ -24,6 +20,9 @@ class CyclePhaseClassifier {
  // Get current cycle phase. -1 if phase is not known (no fix achieved).
  int get_phase(uint32_t cycle_idx);
 
+ // Reference to a pair of DataFrameBit-s
+ typedef DataFrameBit (&DataFrameBitPair)[num_base_stations];
+
  // Get updated data bits from the pulse lens for current cycle.
  // Both bits are always returned, but caller needs to make sure they were updated this cycle by
  // checking DataFrameBit.cycle_idx == cycle_idx.

src/data_frame_decoder.h

@@ -1,7 +1,7 @@
 #pragma once
 #include "primitives/workers.h"
 #include "primitives/producer_consumer.h"
-#include "common.h"
+#include "messages.h"
 
 // See data frame description here: 
 // https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md#ootx-frame

src/debug_node.h

@@ -1,6 +1,6 @@
 #pragma once
 #include "primitives/workers.h"
-#include "outputs.h"
+#include "primitives/producer_consumer.h"
 #include "print_helpers.h"
 
 // This node calls debug_cmd and debug_print for all pipeline nodes periodically,

src/formatters.h

@@ -1,7 +1,7 @@
 #pragma once
 #include "primitives/workers.h"
 #include "primitives/producer_consumer.h"
-#include "common.h"
+#include "messages.h"
 #include "geometry.h"
 
 enum class FormatterType {
@@ -44,7 +44,7 @@ class FormatterNode
  FormatterDef def_;
 };
 
-// Format sensor angles in a text form.
+// Format sensor angles to a text form.
 class SensorAnglesTextFormatter
  : public FormatterNode
  , public Consumer<SensorAnglesFrame> {

src/geometry.cpp

@@ -175,17 +175,17 @@ CoordinateSystemConverter::CoordinateSystemConverter(float mat[9]) {
 std::unique_ptr<CoordinateSystemConverter> CoordinateSystemConverter::create(CoordSysType type, const CoordSysDef& def) {
  switch (type) {
  case CoordSysType::kDefault: return nullptr; // Do nothing.
- case CoordSysType::kNED: return CoordinateSystemConverter::NED(def.ned);
+ case CoordSysType::kNED: return CoordinateSystemConverter::NED(def);
  default: throw_printf("Unknown coord sys type: %d", type);
  }
 }
 
-std::unique_ptr<CoordinateSystemConverter> CoordinateSystemConverter::NED(const NEDCoordDef &def) {
- float ne_angle = def.north_angle / 360.0f * (float)M_PI;
+std::unique_ptr<CoordinateSystemConverter> CoordinateSystemConverter::NED(const CoordSysDef &def) {
+ float ne_angle = def.ned.north_angle / 360.0f * (float)M_PI;
  float mat[9] = {
  // Convert Y up -> Z down; then rotate XY around Z clockwise and inverse X & Y
- -cosf(ne_angle), 0.0f, sinf(ne_angle),
- -sinf(ne_angle), 0.0f, -cosf(ne_angle),
+ -arm_cos_f32(ne_angle), 0.0f, arm_sin_f32(ne_angle),
+ -arm_sin_f32(ne_angle), 0.0f, -arm_cos_f32(ne_angle),
  0.0f, -1.0f, 0.0f,
  };
  return std::make_unique<CoordinateSystemConverter>(mat);

src/geometry.h

@@ -2,18 +2,16 @@
 #include "primitives/workers.h"
 #include "primitives/producer_consumer.h"
 #include "primitives/vector.h"
-#include "common.h"
+#include "messages.h"
 
+// Naive 3d vector type.
 constexpr int vec3d_size = 3;
 typedef float vec3d[vec3d_size];
 
-class Print;
-class HashedWord;
-
 // Stored definition of Base Stations
 struct BaseStationGeometryDef {
- float mat[9];  // Normalized rotation matrix.
- vec3d origin; // Origin point
+ float mat[9]; // Normalized rotation matrix.
+ vec3d origin;  // Origin point
 
  void print_def(uint32_t idx, Print &stream);
  bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream);
@@ -63,18 +61,16 @@ class PointGeometryBuilder : public GeometryBuilder {
 };
 
 
+// Stored type and definition for CoordinateSystemConverter.
 enum class CoordSysType {
  kDefault, // No conversion.
  kNED, // North-East-Down.
 };
 
-struct NEDCoordDef {
- // Angle between North and X axis, in radians.
- float north_angle;
-};
-
 union CoordSysDef {
- NEDCoordDef ned;
+ struct {
+ float north_angle; // Angle between North and X axis, in degrees.
+ } ned;
 };
 
 // Helper node to convert coordinates to a different coordinate system.
@@ -89,7 +85,7 @@ class CoordinateSystemConverter
 
  // Convert from standard Vive coordinate system to NED.
  // Needs angle between North and X axis, in degrees.
- static std::unique_ptr<CoordinateSystemConverter> NED(const NEDCoordDef &def);
+ static std::unique_ptr<CoordinateSystemConverter> NED(const CoordSysDef &def);
 
  virtual void consume(const ObjectPosition& geo);
  virtual bool debug_cmd(HashedWord *input_words);

src/input.h

@@ -2,9 +2,9 @@
 #include "primitives/workers.h"
 #include "primitives/producer_consumer.h"
 #include "primitives/circular_buffer.h"
-#include "common.h"
+#include "messages.h"
 
-// We can used different Teensy modules to measure pulse timing, each with different pros and cons.
+// We can use different Teensy hardware features to measure pulse timing, each with different pros and cons.
 // Look into each input type's header for details.
 enum class InputType {
  kCMP = 0, // Comparator
@@ -13,9 +13,7 @@ enum class InputType {
 };
 constexpr int kInputTypeCount = 3;
 
-class Print;
-class HashedWord;
-
+// Stored definition of an InputNode.
 struct InputDef {
  uint32_t pin; // Teensy PIN number
  bool pulse_polarity; // true = Positive, false = Negative.
@@ -27,9 +25,7 @@ struct InputDef {
 };
 
 
-constexpr int pulses_buffer_len = 32;
-typedef CircularBuffer<Pulse, pulses_buffer_len> PulseCircularBuffer;
-
+// Base class for input nodes. They all produce Pulse-s.
 class InputNode 
  : public WorkerNode
  , public Producer<Pulse> {
@@ -50,6 +46,7 @@ class InputNode
  uint32_t input_idx_;
 
  // We keep the pulse buffer to move Pulse-s from irq context to main thread context.
- PulseCircularBuffer pulses_buf_;
+ static constexpr int pulses_buffer_len = 32;
+ CircularBuffer<Pulse, pulses_buffer_len> pulses_buf_;
 };
 

src/input_cmp.cpp

@@ -14,7 +14,6 @@
 //
 #include "input_cmp.h"
 #include "settings.h"
-#include "common.h"
 #include <Arduino.h>
 
 constexpr int num_comparators = 3; // Number of Comparator modules in Teensy.

src/input_cmp.h

@@ -4,6 +4,7 @@
 struct ComparatorPorts;
 struct ComparatorInputPin;
 
+// Input node using Teensy's comparator modules. See description in .cpp file.
 class InputCmpNode : public InputNode {
 public:
  InputCmpNode(uint32_t input_idx, const InputDef &def);

src/main.cpp

@@ -8,7 +8,7 @@ extern "C" int main() {
  settings.initialize_from_user_input(Serial);
  }
 
- // Create worker node pipeline from settings.
+ // Create worker nodes pipeline from settings.
  auto pipeline = create_vive_sensor_pipeline(settings);
 
  // Register & start input nodes' interrupts.

src/common.h → src/messages.h

@@ -1,19 +1,19 @@
-// This file contains constants, enums and structures that are used in different components.
+// This file contains definitions of messages that are passed between pipeline nodes.
 #pragma once
 #include "primitives/timestamp.h"
 #include "primitives/vector.h"
 #include <stdint.h>
 
-// Tunable
-constexpr int max_num_inputs = 8; // Number of concurrent sensors we support.
+// Tunable constants
+constexpr int max_num_inputs = 8; // Number of concurrent sensors supported.
 constexpr int max_bytes_in_data_frame = 64; // Current DataFrame length is 33. This param should be larger.
 constexpr int max_bytes_in_data_chunk = 64;
 
 // Not tunable: constant for Lighthouse system.
 constexpr int num_base_stations = 2;
 constexpr int num_cycle_phases = 4;
 
-// Pulses are generated by Inputs and processed by PulseProcessor
+// Pulses are generated by InputNodes and processed by PulseProcessor
 struct Pulse {
  uint32_t input_idx;
  Timestamp start_time;
@@ -34,28 +34,32 @@ struct SensorAngles {
  uint32_t updated_cycles[num_cycle_phases]; // Cycle id when this angle was last updated.
 };
 
+// SensorAnglesFrame is produced by PulseProcessor every 4 cycles and consumed by GeometryBuilders. It contains
+// a snapshot of angles visible by sensors.
 struct SensorAnglesFrame {
  Timestamp time;
- FixLevel fix_level;
- uint32_t cycle_idx;
- int32_t phase_id;
+ FixLevel fix_level; // Up to kCycleSynced
+ uint32_t cycle_idx; // Increasing number of cycles since last fix.
+ int32_t phase_id; // 0..3
  Vector<SensorAngles, max_num_inputs> sensors;
 };
 
+// One data bit extracted from a long pulse from one base station. Produced by PulseProcessor and consumed by DataFrameDecoder.
 struct DataFrameBit {
  Timestamp time;
  uint32_t base_station_idx;
  uint32_t cycle_idx;
  bool bit;
 };
 
+// Decoded data frame. Produced by DataFrameDecoder. 'bytes' array can be casted to DecodedDataFrame to get meaningful values.
 struct DataFrame {
  Timestamp time;
  uint32_t base_station_idx;
  Vector<uint8_t, max_bytes_in_data_frame> bytes;
 };
 
-// Position of an object.
+// Position of an object. Calculated by GeometryBuilders and consumed by FormatterNodes.
 struct ObjectPosition {
  Timestamp time;
  uint32_t object_idx; // Index of the object.
@@ -79,6 +83,7 @@ enum class OutputCommandType {
  kMakeNonExclusive, // Remove exclusivity.
 };
 
+// OutputCommand is used to control OutputNodes, see OutputCommandType enum for types of commands.
 struct OutputCommand {
  OutputCommandType type;
  uint32_t stream_idx;

src/outputs.h

@@ -1,9 +1,11 @@
+// This file defines OutputNode-s. These are dumb IO Nodes reading and writing DataChunk-s to/from different
+// hardware streams.
 #pragma once
 #include "primitives/workers.h"
 #include "primitives/producer_consumer.h"
-#include "common.h"
+#include "messages.h"
 
-// usb serial + 3x hardware serials.
+// Currently supported: usb serial + 3x hardware serials.
 constexpr int num_outputs = 4;
 
 struct OutputDef {

src/platform.h

@@ -1,6 +1,6 @@
 #pragma once
 
-// Platform-specific constants
+// Platform-specific functions and constants.
 
 // Max stack size.
 // NOTE: Stack overflow is not checked for now. We do check heap from growing into stack, though.

src/primitives/hash.h

@@ -4,7 +4,7 @@
 
 // MurmurHash3 32 bit, derived from https://github.com/aappleby/smhasher
 // Both compile-time and runtime versions provided.
-// User literal version is also defined to allow "abc"_hash to return hash of "abc".
+// User literal version is also defined to allow "abc"_hash to be compiled to a hash of "abc".
 // One useful property of this hash is that hash("") == 0.
 
 constexpr uint32_t rotl32(uint32_t x, int8_t r) {

src/primitives/string_utils.h

@@ -4,6 +4,7 @@
 #include "../primitives/hash.h"
 
 class Stream;
+class Print;
 template<typename T, unsigned C> class Vector;
 
 // Input string length constants.

src/primitives/workers.h

@@ -4,8 +4,6 @@
 #include <list>
 #include <memory>
 
-class Print;
-
 // Simple worker node pattern. 
 // To create a worker node, inherit from this interface and override functions needed.
 class WorkerNode {

src/print_helpers.h

@@ -1,11 +1,10 @@
 #pragma once
-#include "common.h"
 #include "primitives/producer_consumer.h"
 #include "primitives/timestamp.h"
-#include <algorithm>
+#include "messages.h"
 #include <Print.h>
 
-// Implement Print interface which sends DataChunks as a Producer.
+// Implements Print interface which sends DataChunks as a Producer.
 // Note, the data is buffered and the last chunk is sent either on flush(), newline (if not binary), 
 // or in destructor.
 class DataChunkPrint : public Print {

src/pulse_processor.cpp

@@ -123,7 +123,7 @@ void PulseProcessor::process_cycle_fix(Timestamp cur_time) {
 
  // If needed, get the data bits from pulse lengths and send them down the pipeline
  if (Producer<DataFrameBit>::has_consumers()) {
- DataFrameBitPair bits = phase_classifier_.get_data_bits(cycle_idx_, pulse_lens);
+ CyclePhaseClassifier::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_) {
  bits[b].time = cycle_start_time_;

src/pulse_processor.h

@@ -2,11 +2,11 @@
 #include "primitives/workers.h"
 #include "primitives/producer_consumer.h"
 #include "primitives/vector.h"
-#include "common.h"
+#include "messages.h"
 #include "cycle_phase_classifier.h"
 
-// This node processes pulses from several sensors, tries to match them to cycle structure and
-// output matched set of angles.
+// This node processes Pulses from several sensors, tries to match them to cycle structure and
+// output matched set of angles (SensorAnglesFrame) and data bits (DataFrameBit).
 class PulseProcessor
  : public WorkerNode
  , public Consumer<Pulse>

src/settings.h

@@ -1,11 +1,13 @@
 #pragma once
-#include "common.h"
+#include "messages.h"
 #include "input.h"
 #include "geometry.h"
 #include "formatters.h"
 #include "outputs.h"
 #include <type_traits>
 
+// This class provides configurability to our project. It reads/writes configuration data to EEPROM and provides
+// configuration command interface.
 class PersistentSettings {
 public:
  // Data accessors