WIP

notes.md

@@ -1,10 +1,6 @@
 
 ### TODO
 
-Until public:
- * [ ] Add ability to disable Serial-bound Outputs to be able to debug.
- * [ ] Add ability to shut debugging easily
-
 Next:
  * [ ] Add FTM input
  * [ ] Rework docs.

src/debug_node.cpp

@@ -13,26 +13,42 @@
 DebugNode::DebugNode(Pipeline *pipeline, Stream &debug_stream) 
  : pipeline_(pipeline)
  , debug_stream_(debug_stream)
+ , detachable_print_(std::make_unique<DetachablePrint>(debug_stream))
+ , continuous_debug_print_(0)
  , print_debug_memory_(false) {
  assert(pipeline);
 }
 
 void DebugNode::do_work(Timestamp cur_time) {
  // Process debug input commands
- while (char *input_cmd = read_line(debug_stream_)) {
+ 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);
- if (*hashed_words && !pipeline_->debug_cmd(hashed_words)) 
- debug_stream_.println("Unknown command.");
+ 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> ");
+ }
  }
 
  // Print current debug state
- if (throttle_ms(TimeDelta(1000, ms), cur_time, &debug_print_period_))
+ 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();
+}
+
 bool DebugNode::debug_cmd(HashedWord *input_words) {
  switch (*input_words++) {
  case "debug"_hash:
@@ -44,9 +60,16 @@ bool DebugNode::debug_cmd(HashedWord *input_words) {
  }
  break;
 
- case "!"_hash:
- settings.restart_in_configuration_mode();
- return true;
+ case "!"_hash: settings.restart_in_configuration_mode(); return true;
+ case "o"_hash: detachable_print_->set_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;
+ } else if (input_words->as_uint32(&val) && val >= 10 && val <= 100000) {
+ continuous_debug_print_ = val; debug_print_period_ = Timestamp::cur_time(); return true;
+ }
+ break;
  }
  return false;
 }

src/debug_node.h

@@ -1,7 +1,10 @@
 #pragma once
 #include "primitives/workers.h"
+#include <memory>
+#include <Print.h>
 
 class Stream;
+class DetachablePrint;
 
 // This node calls debug_cmd and debug_print for all pipeline nodes periodically,
 // provides some other debug facilities and blinks LED.
@@ -13,9 +16,36 @@ class DebugNode : public WorkerNode {
  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:
  Pipeline *pipeline_;
  Stream &debug_stream_;
+ std::unique_ptr<DetachablePrint> detachable_print_;
  Timestamp debug_print_period_, blinker_period_;
+ uint32_t continuous_debug_print_;
  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_;
+};

src/mavlink.cpp

@@ -90,8 +90,7 @@ void MavlinkGeometryOutput::consume(const ObjectGeometry& g) {
 }
 
 bool MavlinkGeometryOutput::debug_cmd(HashedWord *input_words) {
- if (*input_words == "mavlink#"_hash && input_words->idx == stream_idx_) {
- input_words++;
+ if (*input_words++ == "mavlink"_hash) {
  switch (*input_words++) {
  case "show"_hash: debug_print_state_ = true; return true;
  case "off"_hash: debug_print_state_ = false; return true;

src/message_logging.h

@@ -14,7 +14,7 @@ inline void print_value(Print &stream, const T& val);
 
 template<>
 inline void print_value<Pulse>(Print &stream, const Pulse& val) {
- stream.printf("inp %d, time %dus, len %d ", val.input_idx, val.start_time.get_value(usec), val.pulse_len.get_value(usec));
+ stream.printf("\ninp %d, time %dus, len %d ", val.input_idx, val.start_time.get_value(usec), val.pulse_len.get_value(usec));
 }
 
 template<>
@@ -89,10 +89,10 @@ class PrintingProducerLogger : public PrintableProduceLogger<T> {
  virtual void print_logs(Print &stream) {
  bool first = true;
  uint32_t has_idx = idx_ != (uint32_t)-1;
+ stream.printf("%s%.*u: ", name_, has_idx, has_idx && idx_);
  T val;
  while (log_.dequeue(&val)) {
  if (first) {
- stream.printf("%s%.*u: ", name_, has_idx, has_idx && idx_);
  first = false;
  } else {
  stream.printf("| ");
@@ -102,6 +102,8 @@ class PrintingProducerLogger : public PrintableProduceLogger<T> {
  if (!first) {
  stream.printf("(%d total)\n", counter_);
  counter_ = 0;
+ } else {
+ stream.printf("accumulating..\n");
  }
  }
 private:

src/primitives/timestamp.h

@@ -5,7 +5,7 @@
 #include <stdint.h>
 
 enum TimeUnit {
- usec = 96, // This will be increased to get better time resolution.
+ usec = 4, // This can be increased to get better time resolution.
  msec = usec * 1000,
  ms = msec,
  sec = msec * 1000,

src/pulse_processor.cpp

@@ -38,10 +38,8 @@ void PulseProcessor::consume(const Pulse& p) {
  // Ignore very long pulses.
  } else if (p.pulse_len >= min_long_pulse_len) { // Long pulse - likely sync pulse
  process_long_pulse(p);
- } else if (p.pulse_len >= min_short_pulse_len) { // Short pulse - likely laser sweep
+ } else { // Short pulse - likely laser sweep
  process_short_pulse(p);
- } else { 
- // Ignore very short pulses. TODO: Keep track of the number. 
  }
 }
 

src/vive_sensors_pipeline.cpp

@@ -18,7 +18,7 @@ std::unique_ptr<Pipeline> create_vive_sensor_pipeline(const PersistentSettings &
  auto pipeline = std::make_unique<Pipeline>();
 
  // Append Debug node to make it possible to print what's going on.
- pipeline->emplace_front(std::make_unique<DebugNode>(pipeline.get(), Serial));
+ auto debug_node = pipeline->emplace_front(std::make_unique<DebugNode>(pipeline.get(), Serial));
 
  // Create central element PulseProcessor.
  auto pulse_processor = pipeline->emplace_back(std::make_unique<PulseProcessor>(settings.inputs().size()));
@@ -46,7 +46,8 @@ std::unique_ptr<Pipeline> create_vive_sensor_pipeline(const PersistentSettings &
  }
 
  // Output Nodes
- // auto sensor_angles_output = pipeline->emplace_back(std::make_unique<SensorAnglesTextOutput>(Serial));
+ // auto sensor_angles_output = pipeline->emplace_back(
+ // std::make_unique<SensorAnglesTextOutput>(debug_node->stream()));
  // pulse_processor->Producer<SensorAnglesFrame>::connect(sensor_angles_output);
 
  if (geometry_builder) {
@@ -59,7 +60,7 @@ std::unique_ptr<Pipeline> create_vive_sensor_pipeline(const PersistentSettings &
  auto mavlink_output = pipeline->emplace_back(std::make_unique<MavlinkGeometryOutput>(Serial1));
  coord_converter->connect(mavlink_output);
 
- auto text_output = pipeline->emplace_back(std::make_unique<GeometryTextOutput>(Serial, 0));
+ auto text_output = pipeline->emplace_back(std::make_unique<GeometryTextOutput>(debug_node->stream(), 0));
  geometry_builder->connect(text_output);
  }