Added configuration for outputs. Changed config format.

CMakeLists.txt

@@ -13,6 +13,7 @@ set(SOURCE_FILES
  src/cycle_phase_classifier.cpp
  src/data_frame_decoder.cpp
  src/debug_node.cpp
+ src/formatters.cpp
  src/geometry.cpp
  src/input.cpp
  src/input_cmp.cpp

notes.md

@@ -1,13 +1,10 @@
 
 ### TODO
 
-Next:
  * [ ] Output point distance & fix level
- * [ ] Add outputs, geo objects and coord conversions to settings
- * [ ] Re-check all last-success timestamps - they don't survive the overflow.
+ * [ ] Re-check all last-success timestamps (LongTimestamp) - they don't survive the overflow.
  * [ ] Add FTM input
  * [ ] Rework docs.
- * [ ] Make USB Serial switchable between debug io and regular mode.
  * [ ] Assertion/termination system.
 
 Later:
@@ -17,8 +14,10 @@ Later:
  * [ ] 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.
  * [ ] 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).
 
 ### Style guide

src/common.h

@@ -6,7 +6,8 @@
 
 // Tunable
 constexpr int max_num_inputs = 8; // Number of concurrent sensors we support.
-constexpr int max_bytes_in_data_frame = 50;
+constexpr int max_bytes_in_data_frame = 64;
+constexpr int max_bytes_in_data_chunk = 64;
 
 // Not tunable: constant for Lighthouse system.
 constexpr int num_base_stations = 2;
@@ -46,6 +47,22 @@ struct DataFrame {
 
 struct ObjectGeometry {
  Timestamp time;
- float xyz[3]; // Position
+ float pos[3]; // Object position
  float q[4]; // Rotation quaternion
 };
+
+// DataChunk is used to send raw data to outputs.
+struct DataChunk {
+ Timestamp time;
+ Vector<uint8_t, max_bytes_in_data_chunk> data; // Data of this chunk.
+ uint32_t stream_idx; // Used to distinguish between different streams going to the same output. Useful for polling mode.
+ bool last_chunk; // True if this is the last chunk in a "packet". Useful for polling mode.
+};
+
+struct OutputCommand {
+ enum OutputCommandType {
+ kMakeExclusive, // Make given stream_idx exclusive and don't accept data chunks from other streams.
+ kMakeNonExclusive, // Remove exclusivity.
+ } type;
+ uint32_t stream_idx;
+};

src/debug_node.cpp

@@ -5,49 +5,60 @@
 #include "settings.h"
 #include "led_state.h"
 #include "platform.h"
+#include "print_helpers.h"
 
-#include <Stream.h>
-#include <core_pins.h>
-#include <pins_arduino.h> // For blinker
 
-
-DebugNode::DebugNode(Pipeline *pipeline, Stream &debug_stream) 
+DebugNode::DebugNode(Pipeline *pipeline)
  : pipeline_(pipeline)
- , debug_stream_(debug_stream)
- , detachable_print_(std::make_unique<DetachablePrint>(debug_stream))
  , continuous_debug_print_(0)
+ , stream_idx_(0x1000)
+ , output_attached_(true)
  , print_debug_memory_(false) {
  assert(pipeline);
 }
 
-void DebugNode::do_work(Timestamp cur_time) {
+void DebugNode::consume_line(char *input_cmd, Timestamp time) {
  // Process debug input commands
- if (char *input_cmd = read_line(debug_stream_)) {
- bool print_debug = !detachable_print_->is_attached() && !continuous_debug_print_;
- detachable_print_->set_attached(false);
- continuous_debug_print_ = 0;
-
- HashedWord* hashed_words = hash_words(input_cmd);
- bool res = !*hashed_words || pipeline_->debug_cmd(hashed_words);
- if (!detachable_print_->is_attached() && !continuous_debug_print_) {
- if (!res)
- debug_stream_.println("Unknown command."); 
- else if (print_debug)
- pipeline_->debug_print(debug_stream_);
- debug_stream_.print("debug> ");
- }
+ bool print_debug = !output_attached_ && !continuous_debug_print_;
+ set_output_attached(false);
+ continuous_debug_print_ = 0;
+
+ HashedWord* hashed_words = hash_words(input_cmd);
+ bool res = !*hashed_words || pipeline_->debug_cmd(hashed_words);
+ if (!output_attached_ && !continuous_debug_print_) {
+ DataChunkPrint printer(this, time, stream_idx_);
+ if (!res)
+ printer.printf("Unknown command.\n"); 
+ else if (print_debug)
+ pipeline_->debug_print(printer);
+ printer.printf("debug> ");
+ }
+}
+
+void DebugNode::do_work(Timestamp cur_time) {
+ // Print current debug state if continuous printing is enabled.
+ if (continuous_debug_print_ > 0 
+ && throttle_ms(TimeDelta(continuous_debug_print_, ms), cur_time, &continuous_print_period_)) {
+ DataChunkPrint printer(this, cur_time, stream_idx_);
+ pipeline_->debug_print(printer);
  }
 
- // Print current debug state
- if (continuous_debug_print_ && throttle_ms(TimeDelta(continuous_debug_print_, ms), cur_time, &debug_print_period_))
- pipeline_->debug_print(debug_stream_);
-
  // Update led pattern.
  update_led_pattern(cur_time);
 }
 
-Print &DebugNode::stream() {
- return *detachable_print_.get();
+// Sometimes the same output is used both for debug and to print values. We want to detach the values stream
+// while we're in debug mode.
+void DebugNode::set_output_attached(bool attached) {
+ if (output_attached_ == attached)
+ return;
+
+ // Send command to the output node we're working with.
+ Producer<OutputCommand>::produce(attached 
+ ? OutputCommand{.type = OutputCommand::kMakeNonExclusive} 
+ : OutputCommand{.type = OutputCommand::kMakeExclusive, .stream_idx = stream_idx_});
+
+ output_attached_ = attached;
 }
 
 bool DebugNode::debug_cmd(HashedWord *input_words) {
@@ -62,19 +73,22 @@ bool DebugNode::debug_cmd(HashedWord *input_words) {
  break;
 
  case "!"_hash: settings.restart_in_configuration_mode(); return true;
- case "o"_hash: detachable_print_->set_attached(true); return true;
+ case "o"_hash: set_output_attached(true); return true;
  case "c"_hash:
  uint32_t val;
  if (!*input_words) {
- continuous_debug_print_ = 1000; debug_print_period_ = Timestamp::cur_time(); return true;
+ continuous_debug_print_ = 1000; continuous_print_period_ = Timestamp::cur_time(); return true;
  } else if (input_words->as_uint32(&val) && val >= 10 && val <= 100000) {
- continuous_debug_print_ = val; debug_print_period_ = Timestamp::cur_time(); return true;
+ continuous_debug_print_ = val; continuous_print_period_ = Timestamp::cur_time(); return true;
  }
  break;
  }
  return false;
 }
 
+
+// ====== System-wide debug metrics =========================================
+
 // Link-time constant markers. Note, you need the *address* of these.
 extern char _sdata; // start of static data
 extern char _edata;

src/debug_node.h

@@ -1,51 +1,30 @@
 #pragma once
 #include "primitives/workers.h"
-#include <memory>
-#include <Print.h>
-
-class Stream;
-class DetachablePrint;
+#include "outputs.h"
+#include "print_helpers.h"
 
 // This node calls debug_cmd and debug_print for all pipeline nodes periodically,
 // provides some other debug facilities and blinks LED.
-class DebugNode : public WorkerNode {
+class DebugNode 
+ : public WorkerNode
+ , public DataChunkLineSplitter
+ , public Producer<DataChunk>
+ , public Producer<OutputCommand> {
 public:
- DebugNode(Pipeline *pipeline, Stream &debug_stream);
+ DebugNode(Pipeline *pipeline);
 
+ virtual void consume_line(char *line, Timestamp time);
  virtual void do_work(Timestamp cur_time);
  virtual bool debug_cmd(HashedWord *input_words);
  virtual void debug_print(Print &stream);
 
- // Detachable print stream - used to stop output to usb serial when debug mode is on.
- Print &stream();
-
- virtual ~DebugNode() = default;
 private:
+ void set_output_attached(bool attached);
+
  Pipeline *pipeline_;
- Stream &debug_stream_;
- std::unique_ptr<DetachablePrint> detachable_print_;
- Timestamp debug_print_period_, blinker_period_;
+ Timestamp continuous_print_period_;
  uint32_t continuous_debug_print_;
+ uint32_t stream_idx_;
+ bool output_attached_;
  bool print_debug_memory_;
 };
-
-// Detachable Print class is used to stop outputting geometry data when debug mode is on.
-class DetachablePrint : public Print {
-public:
- DetachablePrint(Print &source) : source_(source), attached_(true) {}
- void set_attached(bool attached) { attached_ = attached; }
- bool is_attached() const { return attached_; }
-
- // Main printing methods.
- virtual size_t write(uint8_t b) {
- return attached_ ? source_.write(b) : 0;
- }
- virtual size_t write(const uint8_t *buffer, size_t size) {
- return attached_ ? source_.write(buffer, size) : 0;
- }
-
- virtual ~DetachablePrint() = default;
-private:
- Print &source_;
- bool attached_;
-};