Add templated CircularBuffer; Rename structs to CamelCase.

CMakeLists.txt

@@ -14,7 +14,6 @@ set(SOURCE_FILES
  data_decoder.cpp
  geometry.cpp
  mavlink.cpp
- util.cpp
 )
 
 add_executable(vive-diy-position-sensor "${SOURCE_FILES}")

data_decoder.cpp

@@ -3,37 +3,34 @@
 // Frame is 33 bytes long, see description here: https://github.com/nairol/LighthouseRedox/blob/master/docs/Base%20Station.md
 // To decode float16, we can use ARM specific __fp16 type.
 
-void initialize_decoders(input_data &d) {
+void initialize_decoders(InputData &d) {
  // Initialize all decoders to middle value
  for (int i = 0; i < num_big_pulses_in_cycle; i++) {
  for (int j = 0; j < num_cycle_phases; j++)
  d.decoders[i].bit_decoders[j] = {100 << 4, 10}; // Use 100 uS as middle value - it'll be corrected later.
  }
 }
 
-inline int decode_bit(bit_decoder &dec, int pulse_len) {
+inline int decode_bit_and_adjust_decoder(BitDecoder &dec, uint32_t pulse_len) {
  if (pulse_len == 0)
  return -1;
 
- int delta_center = pulse_len - (dec.center_pulse_len >> 4);
- bool high = delta_center > 0;
+ bool high = pulse_len > dec.center_pulse_len;
 
- int assumed_center = pulse_len - (high ? dec.delta_width : -dec.delta_width);
- dec.center_pulse_len = (dec.center_pulse_len * 15 + (assumed_center << 4)) >> 4;
+ int assumed_center = int(pulse_len) - (high ? dec.delta_width : -dec.delta_width);
+ dec.center_pulse_len = (dec.center_pulse_len * 15.f + assumed_center) / 16.f;
 
  // In future: auto-adjust delta_width as well
 
- if (abs(assumed_center - (dec.center_pulse_len >> 4)) < 5) {
+ if (abs(assumed_center - int(dec.center_pulse_len)) < 5) {
  return int(high);
  } else {
  return -1; // Not accurate
  }
 }
 
-inline void decode_and_write_bit(decoder &dec, uint32_t phase_id, uint32_t pulse_len) {
- int bit = decode_bit(dec.bit_decoders[phase_id], int(pulse_len));
-
- data_frame &frame = dec.data_frames[dec.write_data_frames_idx];
+inline void decode_frame_bit(Decoder &dec, int bit) {
+ DataFrame &frame = dec.cur_frame;
  if (bit == -1) { // Not decoded -> reset frame.
  frame = {};
  return;
@@ -82,17 +79,21 @@ inline void decode_and_write_bit(decoder &dec, uint32_t phase_id, uint32_t pulse
 
  if (frame.data_idx == (frame.data_len|1) + 2) {
  // Received full frame - write it.
- INC_CONSTRAINED(dec.write_data_frames_idx, num_data_frames);
- dec.data_frames[dec.write_data_frames_idx] = {};
+ dec.data_frames.enqueue(frame);
+ frame = {};
  }
 }
 
-void extract_data_from_cycle(input_data &d, uint32_t first_pulse_len, uint32_t second_pulse_len, uint32_t id) {
+void extract_data_from_cycle(InputData &d, uint32_t first_pulse_len, uint32_t second_pulse_len, uint32_t id) {
  if (id == 0)
  initialize_decoders(d);
 
  uint32_t phase_id = id % 4;
+ uint32_t pulse_lens[num_big_pulses_in_cycle] = {first_pulse_len, second_pulse_len};
 
- decode_and_write_bit(d.decoders[0], phase_id, first_pulse_len);
- decode_and_write_bit(d.decoders[1], phase_id, second_pulse_len);
+ for (int i = 0; i < num_big_pulses_in_cycle; i++) {
+ Decoder &dec = d.decoders[i];
+ int bit = decode_bit_and_adjust_decoder(dec.bit_decoders[phase_id], pulse_lens[i]);
+ decode_frame_bit(dec, bit);
+ }
 }

geometry.cpp

@@ -1,5 +1,6 @@
 #include "main.h"
 #include "settings.h"
+#include <arm_math.h>
 
 static const int vec3d_size = 3;
 typedef float vec3d[vec3d_size];

main.cpp

@@ -1,8 +1,9 @@
 #include "main.h"
 #include "settings.h"
+#include "utils.h"
 
 // Input-specific data
-input_data global_input_data[max_num_inputs] = {};
+InputData global_input_data[max_num_inputs] = {};
 
 // Debugging state.
 uint32_t active_input_idx = 0; // Active input for which we print values.
@@ -43,17 +44,16 @@ void debug_io(Stream &stream) {
  }
 
  // Print different kinds of debug information.
- input_data &d = global_input_data[active_input_idx];
+ InputData &d = global_input_data[active_input_idx];
  if (printCountDelta) {
  stream.println();
- stream.printf("Pulses write idx: %d\n", d.pulses_write_idx);
- stream.printf("Cycles write idx: %d\n", d.cycles_write_idx);
+ // TODO: Write pulse & cycle queue progress
  stream.printf("Threshold level: %d (%d)\n", getCmpThreshold(active_input_idx), getCmpLevel(active_input_idx));
  }
 
  if (printCycles) {
- for (; d.cycles_read_idx != d.cycles_write_idx; INC_CONSTRAINED(d.cycles_read_idx, cycles_buffer_len)) {
-  cycle &c = d.cycles[d.cycles_read_idx];
+ Cycle c;
+ while (d.cycles.dequeue(&c)) {
  if (c.phase_id == 0) {
  stream.println("\n==================================");
  prevCycleId = -1;
@@ -74,21 +74,21 @@ void debug_io(Stream &stream) {
 
  if (printDecoders) {
  for (int i = 0; i < num_big_pulses_in_cycle; i++) {
- decoder &dec = d.decoders[i];
+ Decoder &dec = d.decoders[i];
  stream.printf("DECODER %d: ", i);
  for (int j = 0; j < num_cycle_phases; j++) {
- bit_decoder &bit_dec = dec.bit_decoders[j];
- stream.printf("%3d, ", bit_dec.center_pulse_len >> 4);
+ BitDecoder &bit_dec = dec.bit_decoders[j];
+ stream.printf("%3d, ", int(bit_dec.center_pulse_len));
  }
  stream.println();
  }
  }
 
  if (printFrames) {
  for (int i = 0; i < num_big_pulses_in_cycle; i++) {
- decoder &dec = d.decoders[i];
- for (; dec.read_data_frames_idx != dec.write_data_frames_idx; INC_CONSTRAINED(dec.read_data_frames_idx, num_data_frames)) {
-  data_frame &frame = dec.data_frames[dec.read_data_frames_idx];
+ Decoder &dec = d.decoders[i];
+ DataFrame frame;
+ while (dec.data_frames.dequeue(&frame)) {
  stream.printf("FRAME %d (%d bytes):", i, frame.data_len);
  for (int j = 0; j < frame.data_len; j++)
  stream.printf(" %02X", frame.data[j]);
@@ -99,15 +99,15 @@ void debug_io(Stream &stream) {
 }
 
 // Update whether fix is acquired for given input.
-void update_fix_acquired(input_data &d, uint32_t cur_millis) {
+void update_fix_acquired(InputData &d, uint32_t cur_millis) {
  // We say that we have a fix after 30 correct cycles and last correct cycle less than 100ms away.
  if (d.last_cycle_id > 30 && (cur_millis - d.last_cycle_time / 1000) < 100) {
  if (!d.fix_acquired) {
  bool invalid_pulse_lens = false;
  uint32_t min_idxes[2];
  for (int i = 0; i < num_big_pulses_in_cycle && !invalid_pulse_lens; i++) {
- decoder &dec = d.decoders[i];
-
+ Decoder &dec = d.decoders[i];
+ 
  // a. Find minimum len in bit_decoders
  uint32_t min_idx = 0;
  for (uint32_t j = 1; j < num_cycle_phases; j++)
@@ -117,13 +117,11 @@ void update_fix_acquired(input_data &d, uint32_t cur_millis) {
  min_idxes[i] = min_idx;
 
  // b. Check that delta lens are as expected (10 - 30 - 10)
- uint32_t cur_idx = min_idx;
- int last_len = dec.bit_decoders[cur_idx].center_pulse_len;
+ float last_len = dec.bit_decoders[min_idx].center_pulse_len;
  static const int valid_deltas[num_cycle_phases - 1] = {10, 30, 10};
  for (uint32_t j = 0; j < num_cycle_phases - 1; j++) {
- INC_CONSTRAINED(cur_idx, num_cycle_phases);
- int cur_len = dec.bit_decoders[cur_idx].center_pulse_len;
- if (abs(((cur_len - last_len) >> 4) - valid_deltas[j]) > 3) {
+ float cur_len = dec.bit_decoders[(min_idx+j+1) % num_cycle_phases].center_pulse_len;
+ if (abs(int(cur_len - last_len) - valid_deltas[j]) > 3) {
  invalid_pulse_lens = true;
  break;
  }
@@ -149,8 +147,8 @@ void update_fix_acquired(input_data &d, uint32_t cur_millis) {
 }
 
 void process_pulse(uint32_t input_idx, uint32_t start_time, uint32_t pulse_len) {
- input_data &d = global_input_data[input_idx];
- cycle &cur_cycle = d.cur_cycle;
+ InputData &d = global_input_data[input_idx];
+ Cycle &cur_cycle = d.cur_cycle;
 
  if (pulse_len >= max_big_pulse_len) {
  // Ignore it.
@@ -166,8 +164,7 @@ void process_pulse(uint32_t input_idx, uint32_t start_time, uint32_t pulse_len)
 
  // If there was a complete cycle before, we should write it and clean up.
  if (cur_cycle.start_time && time_from_cycle_start > (cycle_period-100)) {
- d.cycles[d.cycles_write_idx] = cur_cycle;
- INC_CONSTRAINED(d.cycles_write_idx, cycles_buffer_len);
+ d.cycles.enqueue(cur_cycle);
 
  d.last_cycle_time = cur_cycle.start_time;
  d.last_cycle_id = cur_cycle.phase_id;
@@ -225,19 +222,18 @@ void process_pulse(uint32_t input_idx, uint32_t start_time, uint32_t pulse_len)
 
 // This function is called by input methods when a new pulse is registered.
 void add_pulse(uint32_t input_idx, uint32_t start_time, uint32_t end_time) {
- input_data &d = global_input_data[input_idx];
- d.pulses[d.pulses_write_idx] = {start_time, end_time-start_time};
- INC_CONSTRAINED(d.pulses_write_idx, pulses_buffer_len);
+ InputData &d = global_input_data[input_idx];
+ d.pulses.enqueue({start_time, end_time-start_time});
 }
 
-bool have_valid_input_point(input_data &d, uint32_t cur_millis) {
+bool have_valid_input_point(InputData &d, uint32_t cur_millis) {
  for (int i = 0; i < num_cycle_phases; i++)
  if (d.angle_timestamps[i]/1000 < cur_millis - 100) // All angles were updated in the last 100ms.
  return false;
  return true;
 }
 
-void output_position(uint32_t input_idx, input_data &d, const float pos[3], float dist) {
+void output_position(uint32_t input_idx, InputData &d, const float pos[3], float dist) {
  if (d.angle_last_timestamp == d.angle_last_processed_timestamp)
  return;
 
@@ -286,11 +282,11 @@ void loop() {
  static uint32_t process_period = 0;
  if (throttle_ms(34, cur_millis, &process_period)) { // 33.33ms => 4 cycles
  for (uint32_t input_idx = 0; input_idx < settings.input_count; input_idx++) {
- input_data &d = global_input_data[input_idx];
+ InputData &d = global_input_data[input_idx];
 
  // 1. Process pulses to generate cycles.
- for (; d.pulses_read_idx != d.pulses_write_idx; INC_CONSTRAINED(d.pulses_read_idx, pulses_buffer_len)) {
-  pulse &p = d.pulses[d.pulses_read_idx];
+ Pulse p;
+ while (d.pulses.dequeue(&p)) {
  process_pulse(input_idx, p.start_time, p.pulse_len);
  }
 

main.h

@@ -1,8 +1,8 @@
-// Unfortunately, CMSIS and Arduino.h (kinetis.h) are conflicting on __enable_irq/__disable_irq, so we must
-// include arm_math.h first.
 #pragma once
-#include <arm_math.h>
 #include <Arduino.h>
+#undef __enable_irq
+#undef __disable_irq
+#include "utils.h"
 
 const static int max_num_inputs = 8;
 const static int max_num_base_stations = 2;
@@ -20,7 +20,7 @@ const static int angle_center_len = 4000; // uS
 const static int num_big_pulses_in_cycle = 2;
 const static int num_cycle_phases = 4;
 const static int decoded_data_max_len = 50;
-const static int num_data_frames = 10;
+const static int num_data_frames = 8;
 
 const static float max_position_jump = 0.05; // meters
 
@@ -31,12 +31,12 @@ enum InputType {
  kMaxInputType
 };
 
-struct bit_decoder {
- int center_pulse_len; // uS
+struct BitDecoder {
+ float center_pulse_len; // uS
  int delta_width; // uS
 };
 
-struct data_frame {
+struct DataFrame {
  int preamble_len;
  bool waiting_for_one;
  int data_len;
@@ -46,19 +46,19 @@ struct data_frame {
  byte data[decoded_data_max_len];
 };
 
-struct decoder {
- bit_decoder bit_decoders[num_cycle_phases];
+struct Decoder {
+ BitDecoder bit_decoders[num_cycle_phases];
 
- int read_data_frames_idx, write_data_frames_idx;
- data_frame data_frames[num_data_frames];
+ DataFrame cur_frame;
+ CircularBuffer<DataFrame, num_data_frames> data_frames;
 };
 
-struct pulse {
+struct Pulse {
  uint32_t start_time;
  uint32_t pulse_len;
 };
 
-struct cycle {
+struct Cycle {
  uint32_t start_time; // Microseconds of start of first pulse
  uint32_t phase_id; // 0..3 - id of this cycle
  uint32_t first_pulse_len;
@@ -68,9 +68,8 @@ struct cycle {
  uint32_t cmp_threshold;
 };
 
-struct input_data {
- int32_t pulses_write_idx, pulses_read_idx;
- pulse pulses[pulses_buffer_len];
+struct InputData {
+ CircularBuffer<Pulse, pulses_buffer_len> pulses;
 
  uint32_t num_pulses; // total number of pulses received
  uint32_t small_pulses; // number of small pulses (laser)
@@ -79,12 +78,11 @@ struct input_data {
 
  uint32_t last_cycle_time; // uS timestamp of start of last successful cycle
  uint32_t last_cycle_id; // 0..3
- cycle cur_cycle; // Currently constructed cycle
+ Cycle cur_cycle; // Currently constructed cycle
 
- int32_t cycles_write_idx, cycles_read_idx;
- cycle cycles[cycles_buffer_len];
+ CircularBuffer<Cycle, cycles_buffer_len> cycles;
 
- decoder decoders[num_big_pulses_in_cycle];
+ Decoder decoders[num_big_pulses_in_cycle];
 
  bool fix_acquired;
  uint32_t fix_cycle_offset;
@@ -110,7 +108,7 @@ void changeCmpThreshold(uint32_t input_idx, int delta);
 void add_pulse(uint32_t input_idx, uint32_t start_time, uint32_t end_time);
 
 // Data decoder
-void extract_data_from_cycle(input_data &d, uint32_t first_pulse_len, uint32_t second_pulse_len, uint32_t id);
+void extract_data_from_cycle(InputData &d, uint32_t first_pulse_len, uint32_t second_pulse_len, uint32_t id);
 
 // Geometry
 void calculate_3d_point(const uint32_t angle_lens[num_cycle_phases], float (*ned)[3], float *dist);
@@ -123,9 +121,3 @@ void send_ublox_ned_position(Stream &stream, bool fix_valid, float *pos, float *
 void process_incoming_mavlink_messages();
 void send_mavlink_position(const float ned[3]);
 
-
-// ==== Utils ====
-
-#define INC_CONSTRAINED(val, size) val = ((val) < (size)-1 ? ((val)+1) : 0)
-
-bool throttle_ms(uint32_t period_ms, uint32_t cur_time, uint32_t *prev_period, uint32_t *slips = NULL);