Added configuration for geometry builder nodes.

notes.md

@@ -2,11 +2,11 @@
 ### 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.
  * [ ] Add FTM input
  * [ ] Rework docs.
- * [ ] Add outputs, geo objects and coord conversions to settings
- * [ ] Output point distance.
  * [ ] Make USB Serial switchable between debug io and regular mode.
  * [ ] Assertion/termination system.
 

src/geometry.cpp

@@ -1,29 +1,41 @@
 #include "geometry.h"
 #include <arm_math.h>
-#include "message_logging.h"
-#include "led_state.h"
 #include <assert.h>
 
+#include "primitives/string_utils.h"
+#include "Print.h"
+#include "message_logging.h"
+#include "led_state.h"
 
-constexpr int vec3d_size = 3;
-typedef float vec3d[vec3d_size];
 
 bool intersect_lines(const vec3d &orig1, const vec3d &vec1, const vec3d &orig2, const vec3d &vec2, vec3d *res, float *dist);
-void calc_ray_vec(const BaseStationGeometry &bs, float angle1, float angle2, vec3d &res);
+void calc_ray_vec(const BaseStationGeometryDef &bs, float angle1, float angle2, vec3d &res);
+
+
+GeometryBuilder::GeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def,
+ const Vector<BaseStationGeometryDef, num_base_stations> &base_stations) 
+ : geo_builder_idx_(idx)
+ , base_stations_(base_stations)
+ , def_(geo_def) {
+ assert(idx < max_num_inputs);
+ assert(base_stations.size() == 2);
+ assert(geo_def.sensors.size() > 0);
+}
 
 
-PointGeometryBuilder::PointGeometryBuilder(const Vector<BaseStationGeometry, num_base_stations> &base_stations, uint32_t input_idx)
- : base_stations_(base_stations)
- , input_idx_(input_idx) {
- assert(base_stations.size() >= 2);
+PointGeometryBuilder::PointGeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def,
+  const Vector<BaseStationGeometryDef, num_base_stations> &base_stations )
+ : GeometryBuilder(idx, geo_def, base_stations) {
+ assert(geo_def.sensors.size() == 1);
 }
 
 
 void PointGeometryBuilder::consume(const SensorAnglesFrame& f) {
  // First 2 angles - x, y of station B; second 2 angles - x, y of station C.
  // Y - Up; X -> Z v
  // Station ray is inverse Z axis.
- const SensorAngles &sens = f.sensors[input_idx_];
+ const SensorLocalGeometry &sens_def = def_.sensors[0];
+ const SensorAngles &sens = f.sensors[sens_def.input_idx];
 
  // Use only full frames (30Hz). In future, we can make this check configurable if 120Hz rate needed.
  if (f.phase_id != 3) 
@@ -54,6 +66,9 @@ void PointGeometryBuilder::consume(const SensorAnglesFrame& f) {
  base_stations_[1].origin, ray2, &geo.xyz, &dist);
 
  last_success_ = f.time;
+ for (int i = 0; i < vec3d_size; i++)
+ geo.xyz[i] -= sens_def.pos[i];
+
  produce(geo);
 }
 
@@ -64,9 +79,9 @@ void PointGeometryBuilder::do_work(Timestamp cur_time) {
 }
 
 bool PointGeometryBuilder::debug_cmd(HashedWord *input_words) {
- if (*input_words == "geom#"_hash && input_words->idx == input_idx_) {
+ if (*input_words == "geom#"_hash && input_words->idx == geo_builder_idx_) {
  input_words++;
- return producer_debug_cmd(this, input_words, "ObjectGeometry", input_idx_);
+ return producer_debug_cmd(this, input_words, "ObjectGeometry", geo_builder_idx_);
  }
  return false;
 }
@@ -89,7 +104,7 @@ float vec_length(vec3d &vec) {
  return res;
 }
 
-void calc_ray_vec(const BaseStationGeometry &bs, float angle1, float angle2, vec3d &res) {
+void calc_ray_vec(const BaseStationGeometryDef &bs, float angle1, float angle2, vec3d &res) {
  vec3d a = {arm_cos_f32(angle1), 0, -arm_sin_f32(angle1)}; // Normal vector to X plane
  vec3d b = {0, arm_cos_f32(angle2), arm_sin_f32(angle2)}; // Normal vector to Y plane
 
@@ -189,13 +204,10 @@ void CoordinateSystemConverter::debug_print(Print& stream) {
 }
 
 
-// ======= BaseStationGeometry I/O ===========================================
-#include "Print.h"
-#include "primitives/string_utils.h"
+// ======= BaseStationGeometryDef I/O ===========================================
+// Format: b<idx> origin <x> <y> <z> matrix <9x floats>
 
-// Format:
-// b<id> origin <x> <y> <z> matrix <9 floats>
-void BaseStationGeometry::print_def(uint32_t idx, Print &stream) {
+void BaseStationGeometryDef::print_def(uint32_t idx, Print &stream) {
  stream.printf("b%d origin", idx);
  for (int j = 0; j < 3; j++)
  stream.printf(" %f", origin[j]);
@@ -205,7 +217,7 @@ void BaseStationGeometry::print_def(uint32_t idx, Print &stream) {
  stream.println();
 }
 
-bool BaseStationGeometry::parse_def(HashedWord *input_words, Print &stream) {
+bool BaseStationGeometryDef::parse_def(uint32_t idx, HashedWord *input_words, Print &stream) {
  if (*input_words == "origin"_hash)
  input_words++;
  for (int i = 0; i < 3; i++, input_words++)
@@ -220,3 +232,47 @@ bool BaseStationGeometry::parse_def(HashedWord *input_words, Print &stream) {
  }
  return true;
 }
+
+// ======= GeometryBuilderDef I/O ===========================================
+// Format: g<idx> [ i<idx> <x> <y> <z> ]+
+
+void GeometryBuilderDef::print_def(uint32_t idx, Print &stream) {
+ stream.printf("g%d", idx);
+ for (uint32_t i = 0; i < sensors.size(); i++) {
+ const SensorLocalGeometry &sensor = sensors[i];
+ stream.printf(" i%d %.4f %.4f %.4f", sensor.input_idx, sensor.pos[0], sensor.pos[1], sensor.pos[2]);
+ }
+ stream.println();
+}
+
+bool GeometryBuilderDef::parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream) {
+ sensors.clear();
+ while (*input_words) {
+ SensorLocalGeometry sensor;
+ if (*input_words != "i#"_hash || !input_words->as_uint32(&sensor.input_idx) || sensor.input_idx >= max_num_inputs ) {
+ err_stream.printf("Input number (i<num>) required for geometry builder %d\n", idx); return false;
+ }
+ input_words++;
+ if (!*input_words && sensors.size() == 0) { // Allow skipping coordinates for one-sensor geometry builder.
+ sensor.pos[0] = sensor.pos[1] = sensor.pos[2] = 0.f;
+ sensors.push(sensor);
+ break;
+ }
+ for (int i = 0; i < vec3d_size; i++) {
+ if (!input_words++->as_float(&sensor.pos[i])) {
+ err_stream.printf("Invalid position coordinate in geometry builder %d\n", idx); return false;
+ }
+ }
+ if (sensors.full()) {
+ err_stream.printf("Too many inputs for geometry builder %d. Up to %d allowed.\n", idx, sensors.max_size()); return false;
+ }
+ sensors.push(sensor);
+ }
+ if (sensors.size() == 0) {
+ err_stream.printf("At least one sensor input should be defined for geometry builder %d\n", idx); return false;
+ }
+ if (sensors.size() > 1) {
+ err_stream.printf("Multi-sensor geometry builder is currently not supported.\n"); return false;
+ }
+ return true;
+}

src/geometry.h

@@ -4,34 +4,61 @@
 #include "primitives/vector.h"
 #include "common.h"
 
+constexpr int vec3d_size = 3;
+typedef float vec3d[vec3d_size];
+
 class Print;
 class HashedWord;
 
-struct BaseStationGeometry {
+// Stored definition of Base Stations
+struct BaseStationGeometryDef {
  float mat[9]; // Normalized rotation matrix.
- float origin[3]; // Origin point
+ vec3d origin; // Origin point
 
- void print_def(uint32_t idx, Print &err_stream);
- bool parse_def(HashedWord *input_words, Print &err_stream);
+ void print_def(uint32_t idx, Print &stream);
+ bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream);
 };
 
+struct SensorLocalGeometry {
+ uint32_t input_idx;
+ vec3d pos; // Position of the sensor relative to the object.
+};
 
-// Simple class for single-point sensors.
-class PointGeometryBuilder
+// Stored definition of GeometryBuilder
+struct GeometryBuilderDef {
+ Vector<SensorLocalGeometry, 4> sensors;
+
+ void print_def(uint32_t idx, Print &stream);
+ bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream);
+};
+
+// Parent, abstract class for GeometryBuilders.
+class GeometryBuilder
  : public WorkerNode
  , public Consumer<SensorAnglesFrame>
  , public Producer<ObjectGeometry> {
 public:
- PointGeometryBuilder(const Vector<BaseStationGeometry, num_base_stations> &base_stations, uint32_t input_idx);
+ GeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def,
+ const Vector<BaseStationGeometryDef, num_base_stations> &base_stations);
+
+protected:
+ uint32_t geo_builder_idx_;
+ const Vector<BaseStationGeometryDef, num_base_stations> &base_stations_;
+ GeometryBuilderDef def_;
+};
+
+// Simple class for single-point sensors.
+class PointGeometryBuilder : public GeometryBuilder {
+public:
+ PointGeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def,
+ const Vector<BaseStationGeometryDef, num_base_stations> &base_stations);
  virtual void consume(const SensorAnglesFrame& f);
  virtual void do_work(Timestamp cur_time);
 
  virtual bool debug_cmd(HashedWord *input_words);
  virtual void debug_print(Print& stream);
 
 private:
- const Vector<BaseStationGeometry, num_base_stations> &base_stations_;
- const uint32_t input_idx_;
  Timestamp last_success_;
 };
 

src/input.cpp

@@ -4,7 +4,7 @@
 
 // Multiplexer method to create input node of correct type.
 // Throws exceptions on incorrect values.
-std::unique_ptr<InputNode> InputNode::create(uint32_t input_idx, const InputDefinition &input_def) {
+std::unique_ptr<InputNode> InputNode::create(uint32_t input_idx, const InputDef &input_def) {
  switch (input_def.input_type) {
  case kCMP: return std::make_unique<InputCmpNode>(input_idx, input_def);
  case kPort: throw_printf("Port input type not implemented yet");
@@ -52,38 +52,38 @@ void InputNode::debug_print(Print& stream) {
 }
 
 
-// ==== InputDefinition I/O ================================
+// ==== InputDef I/O ================================
 #include "Print.h"
 #include "primitives/string_utils.h"
 
 uint32_t input_type_hashes[kMaxInputType] = {"cmp"_hash, "ftm"_hash, "port_irq"_hash};
 const char *input_type_names[kMaxInputType] = {"cmp", "ftm", "port_irq"};
 
-void InputDefinition::print_def(uint32_t idx, Print &stream) {
+void InputDef::print_def(uint32_t idx, Print &stream) {
  stream.printf("i%d pin %d %s %s", idx, pin, pulse_polarity ? "positive" : "negative", input_type_names[input_type]);
  if (input_type == kCMP)
  stream.printf(" %d", initial_cmp_threshold);
  stream.println();
 }
-bool InputDefinition::parse_def(HashedWord *input_words, Print &stream) {
+
+bool InputDef::parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream) {
  if (*input_words == "pin"_hash)
  input_words++; // Ignore "pin" word
 
  if (!input_words++->as_uint32(&pin) || pin >= CORE_NUM_TOTAL_PINS) {
- stream.printf("Invalid/missing pin number\n"); return false;
+ err_stream.printf("Invalid/missing pin number\n"); return false;
  }
 
  switch (*input_words++) {
- case 0: stream.printf("Missing polarity\n"); return false;
+ case 0: err_stream.printf("Missing polarity\n"); return false;
  case "positive"_hash: pulse_polarity = true; break;
  case "negative"_hash: pulse_polarity = false; break;
  default: 
- stream.printf("Unknown polarity. Only 'positive' and 'negative' supported.\n"); return false;
+ err_stream.printf("Unknown polarity. Only 'positive' and 'negative' supported.\n"); return false;
  }
 
  if (!*input_words) {
- // Use default input type: Comparator
- input_type = kCMP;
+ input_type = kCMP; // Default input type: Comparator
  } else {
  int i; 
  for (i = 0; i < kMaxInputType; i++)
@@ -92,7 +92,7 @@ bool InputDefinition::parse_def(HashedWord *input_words, Print &stream) {
  break;
  }
  if (i == kMaxInputType) {
- stream.printf("Unknown input type. Supported types: 'port_irq', 'ftm', 'cmp'.\n"); return false;
+ err_stream.printf("Unknown input type. Supported types: 'port_irq', 'ftm', 'cmp'.\n"); return false;
  }
  }
  input_words++;
@@ -102,10 +102,8 @@ bool InputDefinition::parse_def(HashedWord *input_words, Print &stream) {
  if (!*input_words) {
  initial_cmp_threshold = 20; // Default threshold level.
  } else if (!input_words->as_uint32(&initial_cmp_threshold) || initial_cmp_threshold >= 64) {
- stream.printf("Invalid threshold level. Supported values: 0-63.\n"); return false;
+ err_stream.printf("Invalid threshold level. Supported values: 0-63.\n"); return false;
  }
- } else {
- stream.printf("ftm and port_irq input types are not supported yet.\n"); return false;
  }
  return true;
 } 

src/input.h

@@ -16,14 +16,14 @@ enum InputType {
 class Print;
 class HashedWord;
 
-struct InputDefinition {
+struct InputDef {
  uint32_t pin; // Teensy PIN number
  bool pulse_polarity; // true = Positive, false = Negative.
  InputType input_type;
  uint32_t initial_cmp_threshold;
 
- void print_def(uint32_t idx, Print &err_stream);
- bool parse_def(HashedWord *input_words, Print &err_stream);
+ void print_def(uint32_t idx, Print &stream);
+ bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream);
 };
 
 
@@ -35,7 +35,7 @@ class InputNode
  , public Producer<Pulse> {
 public:
  // Create input node of needed type from given configuration.
- static std::unique_ptr<InputNode> create(uint32_t input_idx, const InputDefinition &def);
+ static std::unique_ptr<InputNode> create(uint32_t input_idx, const InputDef &def);
 
  virtual void do_work(Timestamp cur_time);
  virtual bool debug_cmd(HashedWord *input_words);

src/input_cmp.cpp

@@ -126,7 +126,7 @@ void dynamicThresholdAdjustment() {
 */
 
 
-InputCmpNode::InputCmpNode(uint32_t input_idx, const InputDefinition &input_def)
+InputCmpNode::InputCmpNode(uint32_t input_idx, const InputDef &input_def)
  : InputNode(input_idx)
  , rise_time_()
  , rise_valid_(false)

src/input_cmp.h

@@ -6,7 +6,7 @@ struct ComparatorInputPin;
 
 class InputCmpNode : public InputNode {
 public:
- InputCmpNode(uint32_t input_idx, const InputDefinition &def);
+ InputCmpNode(uint32_t input_idx, const InputDef &def);
  ~InputCmpNode();
 
  virtual void start();

src/settings.cpp

@@ -65,7 +65,7 @@ template<typename T, unsigned arr_len>
 void PersistentSettings::set_value(Vector<T, arr_len> &arr, uint32_t idx, HashedWord *input_words, Print& stream) {
  if (idx <= arr.size() && idx < arr_len) {
  T def;
- if (def.parse_def(input_words, stream)) {
+ if (def.parse_def(idx, input_words, stream)) {
  bool push_new = idx == arr.size();
  if (push_new)
  arr.push(def); 
@@ -107,13 +107,15 @@ void PersistentSettings::initialize_from_user_input(Stream &stream) {
  switch (*input_words++) {
  case "view"_hash:
  // Print all current settings.
- stream.printf("# Current configuration: %d inputs, %d base stations; copy/paste to save/restore.\n", 
+ stream.printf("# Current configuration. Copy/paste to save/restore.\n", 
  inputs_.size(), base_stations_.size());
  stream.printf("reset\n");
  for (uint32_t i = 0; i < inputs_.size(); i++)
  inputs_[i].print_def(i, stream);
  for (uint32_t i = 0; i < base_stations_.size(); i++)
  base_stations_[i].print_def(i, stream);
+ for (uint32_t i = 0; i < geo_builders_.size(); i++)
+ geo_builders_[i].print_def(i, stream);
  break;
 
  case "i#"_hash:
@@ -123,6 +125,9 @@ void PersistentSettings::initialize_from_user_input(Stream &stream) {
  case "b#"_hash:
  set_value(base_stations_, idx, input_words, stream);
  break;
+
+ case "g#"_hash:
+ set_value(geo_builders_, idx, input_words, stream);
 
  case "reset"_hash:
  inputs_.clear();