WIP

CMakeLists.txt

@@ -8,18 +8,19 @@ project(vive-diy-position-sensor)
 
 set(SOURCE_FILES
  src/main.cpp
- src/setup.cpp
  src/settings.cpp
-
+ 
  src/cycle_phase_classifier.cpp
  src/data_frame_decoder.cpp
+ src/debug_node.cpp
  src/geometry.cpp
  src/input.cpp
  src/input_cmp.cpp
  src/mavlink.cpp
  src/outputs.cpp
- src/pulse_processor.cpp
  src/platform.cpp
+ src/pulse_processor.cpp
+ src/vive_sensors_pipeline.cpp
 
  src/primitives/timestamp.cpp
  src/primitives/string_utils.cpp

notes.md

@@ -25,7 +25,8 @@ Later:
  * [ ] Create multi-sensor geometry processing unit
  * [ ] Add unit testing
  * [ ] DataFrame: Check CRC32; Decode values.
-
+ * [ ] Split PersistentSettings to Settings and Persistent<>
+ * [ ] (Maybe) Introduce EASTL library and all its niceties like fixed_vector. Tried it and it looks problematic (platform not supported + threading issues).
 
 ### Style guide
 https://google.github.io/styleguide/cppguide.html

src/cycle_phase_classifier.cpp

@@ -8,6 +8,18 @@ enum PhaseFixLevels {
  kPhaseFixFinal = 16,
 };
 
+CyclePhaseClassifier::CyclePhaseClassifier()
+ : prev_full_cycle_idx_(0)
+ , phase_history_(0)
+ , fix_level_(kPhaseFixNone)
+ , phase_shift_(0)
+ , pulse_base_len_(0.0)
+ , bits_()
+ , debug_print_state_(false) {
+ reset();
+}
+
+
 void CyclePhaseClassifier::process_pulse_lengths(uint32_t cycle_idx, const TimeDelta (&pulse_lens)[num_base_stations]) {
  int cur_phase_id = -1;
  if (pulse_lens[0] > TimeDelta(0, usec) && pulse_lens[1] > TimeDelta(0, usec)) {

src/cycle_phase_classifier.h

@@ -14,6 +14,8 @@ typedef DataFrameBit (&DataFrameBitPair)[num_base_stations];
 // 3) Base 2 (C), vertical sweep
 class CyclePhaseClassifier {
 public:
+ CyclePhaseClassifier();
+
  // Process the pulse lengths for current cycle (given by incrementing cycle_idx).
  void process_pulse_lengths(uint32_t cycle_idx, const TimeDelta (&pulse_lens)[num_base_stations]);
 

src/data_frame_decoder.cpp

@@ -2,7 +2,15 @@
 #include "message_logging.h"
 
 DataFrameDecoder::DataFrameDecoder(uint32_t base_station_idx)
- : base_station_idx_(base_station_idx) {
+ : base_station_idx_(base_station_idx)
+ , prev_cycle_idx_(0)
+ , preamble_len_(0)
+ , skip_one_set_bit_(false)
+ , cur_byte_(0)
+ , cur_bit_idx_(0)
+ , data_idx_(0)
+ , data_frame_len_(0)
+ , data_frame_() {
 }
 
 void DataFrameDecoder::consume(const DataFrameBit& frame_bit) {

src/debug_node.cpp

@@ -0,0 +1,76 @@
+#include "debug_node.h"
+#include <assert.h>
+#include <malloc.h>
+
+#include "settings.h"
+
+#include <Stream.h>
+#include <core_pins.h>
+#include <pins_arduino.h> // For blinker
+
+
+DebugNode::DebugNode(Pipeline *pipeline, Stream &debug_stream) 
+ : pipeline_(pipeline)
+ , debug_stream_(debug_stream)
+ , 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_)) {
+ HashedWord* hashed_words = hash_words(input_cmd);
+ if (*hashed_words && !pipeline_->debug_cmd(hashed_words)) 
+ debug_stream_.println("Unknown command.");
+ }
+
+ // Print current debug state
+ if (throttle_ms(TimeDelta(1000, ms), cur_time, &debug_print_period_))
+ pipeline_->debug_print(debug_stream_);
+
+ // Blink once a second in normal mode without a fix.
+ if (throttle_ms(TimeDelta(1000, ms), cur_time, &blinker_period_)) {
+ digitalWriteFast(LED_BUILTIN, (uint8_t)(!digitalReadFast(LED_BUILTIN)));
+ }
+}
+
+bool DebugNode::debug_cmd(HashedWord *input_words) {
+ switch (*input_words++) {
+ case "debug"_hash:
+ switch (*input_words++) {
+ case "mem"_hash:
+ case "memory"_hash:
+ print_debug_memory_ = !print_debug_memory_;
+ return true;
+ }
+ break;
+
+ case "!"_hash:
+ settings.restart_in_configuration_mode();
+ return true;
+ }
+ return false;
+}
+
+// Link-time constant markers. Note, you need the *address* of these.
+extern char _sdata; // start of static data
+extern char _edata;
+extern char _sbss;
+extern char _ebss; // end of static data; bottom of heap
+extern char _estack; // bottom of stack, top of ram: stack grows down towards heap
+
+// Top of heap from mk20dx128.c
+extern char *__brkval; // top of heap (dynamic ram): grows up towards stack
+
+void DebugNode::debug_print(Print &stream) {
+ if (print_debug_memory_) {
+ uint32_t static_data_size = &_ebss - (char*)((uint32_t)&_sdata & 0xFFFFF000);
+ uint32_t allocated_heap = __brkval - &_ebss;
+ char c, *top_stack = &c;
+ int32_t heap_to_stack_distance = top_stack - __brkval;
+ uint32_t stack_size = &_estack - top_stack;
+ struct mallinfo m = mallinfo();
+ stream.printf("RAM: static %d, heap %d (used %d, free %d), unalloc %d, stack %d\n", 
+ static_data_size, allocated_heap, m.uordblks, m.fordblks, heap_to_stack_distance, stack_size);
+ }
+}

src/debug_node.h

@@ -0,0 +1,21 @@
+#pragma once
+#include "primitives/workers.h"
+
+class Stream;
+
+// 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 {
+public:
+ DebugNode(Pipeline *pipeline, Stream &debug_stream);
+
+ virtual void do_work(Timestamp cur_time);
+ virtual bool debug_cmd(HashedWord *input_words);
+ virtual void debug_print(Print &stream);
+
+private:
+ Pipeline *pipeline_;
+ Stream &debug_stream_;
+ Timestamp debug_print_period_, blinker_period_;
+ bool print_debug_memory_;
+};

src/geometry.cpp

@@ -1,6 +1,7 @@
 #include "geometry.h"
 #include <arm_math.h>
 #include "message_logging.h"
+#include <assert.h>
 
 /*
 // NE angle = Angle(North - X axis).
@@ -14,8 +15,9 @@ static float ned_rotation[9] = {
 static arm_matrix_instance_f32 ned_rotation_mat = {3, 3, ned_rotation};
 */
 PointGeometryBuilder::PointGeometryBuilder(const Vector<BaseStationGeometry, num_base_stations> &base_stations, uint32_t input_idx)
- : base_stations_(base_stations), input_idx_(input_idx) {
- // TODO: Assert that base_stations->size() == 2.
+ : base_stations_(base_stations)
+ , input_idx_(input_idx) {
+ assert(base_stations.size() >= 2);
 }
 
 

src/main.cpp

@@ -1,10 +1,6 @@
-#include <Arduino.h>
-#include "common.h"
-#include "primitives/workers.h"
-//#include "settings.h"
+#include "vive_sensors_pipeline.h"
+#include "settings.h"
 
-// Global static data
-Workforce workforce;
 /*
  // 3. Calculate geometry & output it.
  if (d.fix_acquired && have_valid_input_point(d, cur_time)) {
@@ -50,30 +46,30 @@ void output_position(uint32_t input_idx, InputData &d, const float pos[3], float
 }
 */
 
+#include <avr_emulation.h>
+#include <usb_serial.h>
 
-// Main loop. All asynchronous calculations happen here.
-void loop() {
- Timestamp cur_time = Timestamp::cur_time();
 
- // Process debug I/O
- Stream &debug_stream = Serial;
- while (char *input_cmd = read_line(debug_stream)) {
- char **input_words = parse_words(input_cmd);
- if (input_words[0] && !workforce.debug_cmd(hash_words(input_words)))
- debug_stream.println("Unknown command.");
- }
-
- static Timestamp debug_next_run(cur_time);
- if (throttle_ms(TimeDelta(1000, ms), cur_time, &debug_next_run))
- workforce.debug_print(debug_stream);
+extern "C" int main() {
+ // Initialize core devices.
+ Serial.begin(9600);
+ pinMode(LED_BUILTIN, OUTPUT);
 
- // Blink once a second in normal mode without a fix. TODO: Change it when fix is achieved.
- static Timestamp blink_next_run(cur_time);
- if (throttle_ms(TimeDelta(1, sec), cur_time, &blink_next_run)) {
- digitalWriteFast(LED_BUILTIN, (uint8_t)(!digitalReadFast(LED_BUILTIN)));
+ // Initialize persistent settings interactively from user input, if needed.
+ if (settings.needs_configuration()) {
+  digitalWrite(LED_BUILTIN, HIGH);
+ settings.initialize_from_user_input(Serial);
  }
 
- // Process pulses, cycles and output data.
- workforce.do_work(cur_time);
+ // Create worker node pipeline from settings.
+ auto pipeline = create_vive_sensor_pipeline(settings);
 
+ // Register & start input nodes' interrupts.
+ pipeline->start();
+
+ // Main loop.
+ while (true) {
+ // Process pulses, cycles and output data.
+ pipeline->do_work(Timestamp::cur_time());
+ }
 }

src/mavlink.cpp

@@ -19,7 +19,9 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *chars, unsig
 #include <common/mavlink.h>
 
 // Insanity ends here. Back to normal.
-#include <Arduino.h> // Only for Print class.
+#include <assert.h>
+#include <Print.h>
+#include <avr_emulation.h>
 #include "primitives/vector.h"
 
 // Static list of streams we will output to.
@@ -30,13 +32,12 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *chars, unsig
  output_streams[chan]->write(chars, length);
 }
 
-MavlinkGeometryOutput::MavlinkGeometryOutput(Print &stream) {
- if (!output_streams.full()) {
- stream_idx_ = output_streams.size();
- output_streams.push(&stream);
- } else {
- // Assert: full.
- }
+MavlinkGeometryOutput::MavlinkGeometryOutput(Print &stream)
+ : stream_idx_(output_streams.size())
+ , debug_print_state_(false)
+ , debug_last_ms_(0) {
+ assert(!output_streams.full());
+ output_streams.push(&stream);
 }
 
 void MavlinkGeometryOutput::reset_all() {
@@ -70,7 +71,7 @@ void MavlinkGeometryOutput::debug_print(Print &stream) {
  if (debug_print_state_) {
  uint32_t now_ms = millis();
  if (now_ms - debug_last_ms_ > 50)
- Serial.printf("Late Mavlink message: %dms\n", now_ms - debug_last_ms_);
+ stream.printf("Late Mavlink message: %dms\n", now_ms - debug_last_ms_);
  debug_last_ms_ = now_ms;
  }
 }

src/message_logging.h

@@ -52,7 +52,7 @@ class CountingProducerLogger : public PrintableProduceLogger<T> {
  }
  virtual void print_logs(Print &stream) {
  uint32_t has_idx = idx_ != (uint32_t)-1;
- stream.printf("%s%.*u produced %d items", name_, has_idx, has_idx && idx_, counter_);
+ stream.printf("%s%.*u produced %d items\n", name_, has_idx, has_idx && idx_, counter_);
  counter_ = 0;
  }
 private:

src/outputs.cpp

@@ -30,7 +30,7 @@ GeometryTextOutput::GeometryTextOutput(Print &stream, uint32_t object_idx)
 }
 
 void GeometryTextOutput::consume(const ObjectGeometry& f) {
- const char* time = "<time>"; // TODO.
- stream_->printf("GEO\t%d\t%s\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", object_idx_, time,
+ auto time = f.time.get_value_unsafe(msec); // TODO.
+ stream_->printf("GEO%d\t%u\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f\n", object_idx_, time,
  f.xyz[0], f.xyz[1], f.xyz[2], f.q[0], f.q[1], f.q[2], f.q[3]);
 }