Added fix_level; Renamed ObjectPosition; Converted enums to scoped.

src/common.h

@@ -5,8 +5,8 @@
 #include <stdint.h>
 
 // Tunable
-constexpr int max_num_inputs = 8; // Number of concurrent sensors we support.
-constexpr int max_bytes_in_data_frame = 64;
+constexpr int max_num_inputs = 8;  // Number of concurrent sensors we support.
+constexpr int max_bytes_in_data_frame = 64; // Current DataFrame length is 33. This param should be larger.
 constexpr int max_bytes_in_data_chunk = 64;
 
 // Not tunable: constant for Lighthouse system.
@@ -20,6 +20,14 @@ struct Pulse {
  TimeDelta pulse_len;
 };
 
+enum class FixLevel {
+ kNoSignals = 0, // No signals visible at all.
+ kCycleSyncing = 100, // Base station sync pulses are visible and we're syncing to them.
+ kCycleSynced = 200, // We're synced to the base station sync pulses.
+ kPartialVis = 500, // Some sensors/base stations are covered. Not enough info to get position.
+ kFullFix = 1000, // Base station visibility is enough to extract full position.
+};
+
 struct SensorAngles {
  float angles[num_cycle_phases]; // Angles of base stations to sensor, -1/3 Pi to 1/3 Pi
  uint32_t updated_cycles[num_cycle_phases]; // Cycle id when this angle was last updated.
@@ -45,10 +53,14 @@ struct DataFrame {
  Vector<uint8_t, max_bytes_in_data_frame> bytes;
 };
 
-struct ObjectGeometry {
+// Position of an object.
+struct ObjectPosition {
  Timestamp time;
- float pos[3]; // Object position
- float q[4]; // Rotation quaternion
+ uint32_t object_idx; // Index of the object.
+ FixLevel fix_level;
+ float pos[3]; // 3d object position
+ float pos_delta; // Distance between base station rays. Can be used as a measure of position uncertainty.
+ float q[4]; // Rotation quaternion (unit if no rotation information available)
 };
 
 // DataChunk is used to send raw data to outputs.
@@ -59,10 +71,13 @@ struct DataChunk {
  bool last_chunk; // True if this is the last chunk in a "packet". Useful for polling mode.
 };
 
+
+enum class OutputCommandType {
+ kMakeExclusive, // Make given stream_idx exclusive and don't accept data chunks from other streams.
+ kMakeNonExclusive, // Remove exclusivity.
+};
+
 struct OutputCommand {
- enum OutputCommandType {
- kMakeExclusive, // Make given stream_idx exclusive and don't accept data chunks from other streams.
- kMakeNonExclusive, // Remove exclusivity.
- } type;
+ OutputCommandType type;
  uint32_t stream_idx;
 };

src/cycle_phase_classifier.cpp

@@ -1,7 +1,7 @@
 #include "cycle_phase_classifier.h"
 #include <Arduino.h>
 
-enum PhaseFixLevels {
+enum PhaseFixLevels { // Unscoped enum because we use it more like set of constants.
  kPhaseFixNone = 0,
  kPhaseFixCandidate = 1,
  kPhaseFixAcquired = 4,

src/debug_node.cpp

@@ -55,8 +55,8 @@ void DebugNode::set_output_attached(bool attached) {
 
  // 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_});
+ ? OutputCommand{.type = OutputCommandType::kMakeNonExclusive} 
+ : OutputCommand{.type = OutputCommandType::kMakeExclusive, .stream_idx = stream_idx_});
 
  output_attached_ = attached;
 }

src/formatters.cpp

@@ -46,14 +46,14 @@ void SensorAnglesTextFormatter::consume(const SensorAnglesFrame& f) {
 // ====== GeometryFormatter =================================================
 std::unique_ptr<GeometryFormatter> GeometryFormatter::create(uint32_t idx, const FormatterDef &def) {
  switch (def.formatter_subtype) {
- case kFormatGeoText: return std::make_unique<GeometryTextFormatter>(idx, def);
- case kFormatGeoMavlink: return std::make_unique<GeometryMavlinkFormatter>(idx, def);
+ case FormatterSubtype::kPosText: return std::make_unique<GeometryTextFormatter>(idx, def);
+ case FormatterSubtype::kPosMavlink: return std::make_unique<GeometryMavlinkFormatter>(idx, def);
  default: throw_printf("Unknown geometry formatter subtype: %d", def.formatter_subtype);
  }
 }
 
 // ====== GeometryTextFormatter =============================================
-void GeometryTextFormatter::consume(const ObjectGeometry& f) {
+void GeometryTextFormatter::consume(const ObjectPosition& f) {
  auto time = f.time.get_value(msec);
  DataChunkPrint printer(this, f.time, node_idx_);
  printer.printf("OBJ%d\t%u\t%.4f\t%.4f\t%.4f", def_.input_idx, time, f.pos[0], f.pos[1], f.pos[2]);
@@ -71,32 +71,32 @@ void GeometryTextFormatter::consume(const ObjectGeometry& f) {
 // stream2 position object0 > usb_serial
 
 HashedWord formatter_types[] = {
- {"angles", "angles"_hash, kFormatAngles << 16 },
- {"dataframe", "dataframe"_hash, kFormatDataFrame << 16 },
- {"position", "position"_hash, kFormatGeometry << 16 | kFormatGeoText},
- {"mavlink", "mavlink"_hash, kFormatGeometry << 16 | kFormatGeoMavlink},
+ {"angles", "angles"_hash, (int)FormatterType::kAngles << 16 },
+ {"dataframe", "dataframe"_hash, (int)FormatterType::kDataFrame << 16 },
+ {"position", "position"_hash, (int)FormatterType::kPosition << 16 | (int)FormatterSubtype::kPosText},
+ {"mavlink", "mavlink"_hash, (int)FormatterType::kPosition << 16 | (int)FormatterSubtype::kPosMavlink},
 };
 
 
 void FormatterDef::print_def(uint32_t idx, Print &stream) {
  stream.printf("stream%d ", idx);
 
  // Print type & subtype.
- uint32_t packed_type = formatter_type << 16 | formatter_subtype;
+ uint32_t packed_type = (int)formatter_type << 16 | (int)formatter_subtype;
  for (uint32_t i = 0; i < sizeof(formatter_types) / sizeof(formatter_types[0]); i++)
  if (packed_type == formatter_types[i].idx) {
  stream.printf("%s ", formatter_types[i].word);
  }
 
  // Print settings for the type.
  switch (formatter_type) {
- case kFormatAngles: break;
- case kFormatDataFrame: break;
- case kFormatGeometry: {
+ case FormatterType::kAngles: break;
+ case FormatterType::kDataFrame: break;
+ case FormatterType::kPosition: {
  stream.printf("object%d ", input_idx);
  switch (coord_sys_type) {
- case kDefaultCoordSys: break; // Nothing
- case kNED: {
+ case CoordSysType::kDefault: break; // Nothing
+ case CoordSysType::kNED: {
  stream.printf("ned %.1f ", coord_sys_params.ned.north_angle);
  break;
  }
@@ -129,19 +129,19 @@ bool FormatterDef::parse_def(uint32_t idx, HashedWord *input_words, Print &err_s
 
  // Parse settings for type/subtype
  switch (formatter_type) {
- case kFormatAngles: break;
- case kFormatDataFrame: break;
- case kFormatGeometry: {
+ case FormatterType::kAngles: break;
+ case FormatterType::kDataFrame: break;
+ case FormatterType::kPosition: {
  if (*input_words != "object#"_hash) {
  err_stream.printf("Need object for position stream type.\n");
  return false;
  }
  input_idx = input_words->idx;
  input_words++;
 
- coord_sys_type = kDefaultCoordSys;
+ coord_sys_type = CoordSysType::kDefault;
  if (*input_words == "ned"_hash) {
- coord_sys_type = kNED;
+ coord_sys_type = CoordSysType::kNED;
  input_words++;
  if (!input_words->as_float(&coord_sys_params.ned.north_angle)) {
  err_stream.printf("Expected north angle after 'ned' keyword.\n");

src/formatters.h

@@ -4,14 +4,14 @@
 #include "common.h"
 #include "geometry.h"
 
-enum FormatterType {
- kFormatAngles,
- kFormatDataFrame,
- kFormatGeometry,
+enum class FormatterType {
+ kAngles,
+ kDataFrame,
+ kPosition,
 };
-enum FormatterSubtype {
- kFormatGeoText,
- kFormatGeoMavlink,
+enum class FormatterSubtype {
+ kPosText,
+ kPosMavlink,
 };
 
 // Stored definition of a FormatterNode
@@ -56,7 +56,7 @@ class SensorAnglesTextFormatter
 // Base class for geometry formatters.
 class GeometryFormatter 
  : public FormatterNode
- , public Consumer<ObjectGeometry> {
+ , public Consumer<ObjectPosition> {
 public:
  static std::unique_ptr<GeometryFormatter> create(uint32_t idx, const FormatterDef &def);
 
@@ -68,21 +68,21 @@ class GeometryFormatter
 class GeometryTextFormatter : public GeometryFormatter {
 public:
  GeometryTextFormatter(uint32_t idx, const FormatterDef &def) : GeometryFormatter(idx, def) {}
- virtual void consume(const ObjectGeometry& f);
+ virtual void consume(const ObjectPosition& f);
 };
 
 
 // Format object geometry in Mavlink format.
 class GeometryMavlinkFormatter : public GeometryFormatter {
 public:
  GeometryMavlinkFormatter(uint32_t idx, const FormatterDef &def);
- virtual void consume(const ObjectGeometry& f);
+ virtual void consume(const ObjectPosition& f);
 
  virtual bool debug_cmd(HashedWord *input_words);
  virtual void debug_print(Print& stream);
 
 private:
- bool position_valid(const ObjectGeometry& g);
+ bool position_valid(const ObjectPosition& g);
  void send_message(uint32_t msgid, const char *packet, Timestamp cur_time, uint8_t min_length, uint8_t length, uint8_t crc_extra);
 
  uint32_t current_tx_seq_;

src/geometry.cpp

@@ -35,13 +35,14 @@ 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 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) 
  return;
 
+ const SensorLocalGeometry &sens_def = def_.sensors[0];
+ const SensorAngles &sens = f.sensors[sens_def.input_idx];
+
  // Check all angles are fresh.
  for (int i = 0; i < num_cycle_phases; i++)
  if (sens.updated_cycles[i] < f.cycle_idx - 8) {
@@ -59,12 +60,17 @@ void PointGeometryBuilder::consume(const SensorAnglesFrame& f) {
  calc_ray_vec(base_stations_[1], angles[2], angles[3], ray2);
  //Serial.printf("Ray2: %f %f %f\n", ray2[0], ray2[1], ray2[2]);
 
+ ObjectPosition geo = {
+ .time = f.time,
+ .object_idx = geo_builder_idx_,
+ .fix_level = FixLevel::kFullFix,
+ .pos = {0.f, 0.f, 0.f},
+ .pos_delta = 0.0,
+ .q = {1.f, 0.f, 0.f, 0.f}
+ };
 
- ObjectGeometry geo = {.time = f.time, .pos = {}, .q = {1.f, 0.f, 0.f, 0.f}};
-
- float dist;
  intersect_lines(base_stations_[0].origin, ray1, 
- base_stations_[1].origin, ray2, &geo.pos, &dist);
+ base_stations_[1].origin, ray2, &geo.pos, &geo.pos_delta);
 
  last_success_ = f.time;
  for (int i = 0; i < vec3d_size; i++)
@@ -76,13 +82,13 @@ void PointGeometryBuilder::consume(const SensorAnglesFrame& f) {
 void PointGeometryBuilder::do_work(Timestamp cur_time) {
  // TODO: Make compatible with multiple geometry objects.
  bool has_fix = cur_time - last_success_ < TimeDelta(80, ms);
- set_led_state(has_fix ? kFixFound : kNoFix);
+ set_led_state(has_fix ? LedState::kFixFound : LedState::kNoFix);
 }
 
 bool PointGeometryBuilder::debug_cmd(HashedWord *input_words) {
  if (*input_words == "geom#"_hash && input_words->idx == geo_builder_idx_) {
  input_words++;
- return producer_debug_cmd(this, input_words, "ObjectGeometry", geo_builder_idx_);
+ return producer_debug_cmd(this, input_words, "ObjectPosition", geo_builder_idx_);
  }
  return false;
 }
@@ -170,8 +176,8 @@ CoordinateSystemConverter::CoordinateSystemConverter(float mat[9]) {
 
 std::unique_ptr<CoordinateSystemConverter> CoordinateSystemConverter::create(CoordSysType type, const CoordSysDef& def) {
  switch (type) {
- case kDefaultCoordSys: return nullptr; // Do nothing.
- case kNED: return CoordinateSystemConverter::NED(def.ned);
+ case CoordSysType::kDefault: return nullptr; // Do nothing.
+ case CoordSysType::kNED: return CoordinateSystemConverter::NED(def.ned);
  default: throw_printf("Unknown coord sys type: %d", type);
  }
 }
@@ -187,24 +193,24 @@ std::unique_ptr<CoordinateSystemConverter> CoordinateSystemConverter::NED(const
  return std::make_unique<CoordinateSystemConverter>(mat);
 }
 
-void CoordinateSystemConverter::consume(const ObjectGeometry& geo) {
- ObjectGeometry res(geo);
+void CoordinateSystemConverter::consume(const ObjectPosition& pos) {
+ ObjectPosition out_pos(pos);
 
  // Convert position
- arm_matrix_instance_f32 src_mat = {3, 1, const_cast<float*>(geo.pos)};
- arm_matrix_instance_f32 dest_mat = {3, 1, res.pos};
+ arm_matrix_instance_f32 src_mat = {3, 1, const_cast<float*>(pos.pos)};
+ arm_matrix_instance_f32 dest_mat = {3, 1, out_pos.pos};
  arm_matrix_instance_f32 rotation_mat = {3, 3, mat_};
  arm_mat_mult_f32(&rotation_mat, &src_mat, &dest_mat);
 
  // TODO: Convert quaternion.
- assert(geo.q[0] == 1.0f);
+ assert(pos.q[0] == 1.0f);
 
- produce(res);
+ produce(out_pos);
 }
 
 bool CoordinateSystemConverter::debug_cmd(HashedWord *input_words) {
  if (*input_words++ == "coord"_hash) {
- return producer_debug_cmd(this, input_words, "ObjectGeometry");
+ return producer_debug_cmd(this, input_words, "ObjectPosition");
  }
  return false;
 }

src/geometry.h

@@ -36,7 +36,7 @@ struct GeometryBuilderDef {
 class GeometryBuilder
  : public WorkerNode
  , public Consumer<SensorAnglesFrame>
- , public Producer<ObjectGeometry> {
+ , public Producer<ObjectPosition> {
 public:
  GeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def,
  const Vector<BaseStationGeometryDef, num_base_stations> &base_stations);
@@ -63,9 +63,9 @@ class PointGeometryBuilder : public GeometryBuilder {
 };
 
 
-enum CoordSysType {
- kDefaultCoordSys,
- kNED,
+enum class CoordSysType {
+ kDefault, // No conversion.
+ kNED, // North-East-Down.
 };
 
 struct NEDCoordDef {
@@ -80,8 +80,8 @@ union CoordSysDef {
 // Helper node to convert coordinates to a different coordinate system.
 class CoordinateSystemConverter
  : public WorkerNode
- , public Consumer<ObjectGeometry>
- , public Producer<ObjectGeometry> {
+ , public Consumer<ObjectPosition>
+ , public Producer<ObjectPosition> {
 public:
  CoordinateSystemConverter(float m[9]);
 
@@ -91,7 +91,7 @@ class CoordinateSystemConverter
  // Needs angle between North and X axis, in degrees.
  static std::unique_ptr<CoordinateSystemConverter> NED(const NEDCoordDef &def);
 
- virtual void consume(const ObjectGeometry& geo);
+ virtual void consume(const ObjectPosition& geo);
  virtual bool debug_cmd(HashedWord *input_words);
  virtual void debug_print(Print& stream);