Added FixLevel and dist values.

notes.md

@@ -1,7 +1,6 @@
 
 ### TODO
 
- * [ ] Output point distance & fix level
  * [ ] Re-check all last-success timestamps (LongTimestamp) - they don't survive the overflow.
  * [ ] Add FTM input
  * [ ] Rework docs.

src/common.h

@@ -24,8 +24,9 @@ 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.
+ kPartialVis = 500, // Some sensors/base stations don't have visibility and angles are stale. Position is invalid.
+ kStaleFix = 800, // Position fix is valid, but uses angles from previous 1-2 cycles.
+ kFullFix = 1000, // Position fix is valid and fresh.
 };
 
 struct SensorAngles {
@@ -35,6 +36,7 @@ struct SensorAngles {
 
 struct SensorAnglesFrame {
  Timestamp time;
+ FixLevel fix_level;
  uint32_t cycle_idx;
  int32_t phase_id;
  Vector<SensorAngles, max_num_inputs> sensors;

src/formatters.cpp

@@ -23,24 +23,20 @@ void FormatterNode::debug_print(Print& stream) {
 
 // ====== SensorAnglesTextFormatter =========================================
 void SensorAnglesTextFormatter::consume(const SensorAnglesFrame& f) {
- // Only print on the last phase.
- if (f.phase_id != 3)
- return;
+ uint32_t time = f.time.get_value(msec);
 
- auto time = f.time.get_value(msec);
-
- DataChunkPrint printer(this, f.time, node_idx_);
- printer.printf("ANG\t%d", time);
+ // Print each sensor on its own line.
  for (uint32_t i = 0; i < f.sensors.size(); i++) {
+ DataChunkPrint printer(this, f.time, node_idx_);
  const SensorAngles &angles = f.sensors[i];
+ printer.printf("ANG%d\t%u\t%d", i, time, f.fix_level);
  for (uint32_t j = 0; j < num_cycle_phases; j++) {
- if (angles.updated_cycles[j] == f.cycle_idx - f.phase_id + j)
- printer.printf("\t%.4f", angles.angles[j]);
- else
- printer.printf("\t");
+ printer.printf("\t");
+ if (f.fix_level == FixLevel::kCycleSynced && angles.updated_cycles[j] == f.cycle_idx - f.phase_id + j)
+ printer.printf("%.4f", angles.angles[j]);
  }
+ printer.printf("\n");
  }
- printer.printf("\n");
 }
 
 // ====== GeometryFormatter =================================================
@@ -54,12 +50,13 @@ std::unique_ptr<GeometryFormatter> GeometryFormatter::create(uint32_t idx, const
 
 // ====== GeometryTextFormatter =============================================
 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]);
- if (f.q[0] != 1.0f) {
- // Output quaternion if available.
- printer.printf("\t%.4f\t%.4f\t%.4f\t%.4f", f.q[0], f.q[1], f.q[2], f.q[3]);
+ printer.printf("OBJ%d\t%u\t%d", f.object_idx, f.time.get_value(msec), f.fix_level);
+ if (f.fix_level >= FixLevel::kStaleFix) {
+ printer.printf("\t%.4f\t%.4f\t%.4f\t%.4f", f.pos[0], f.pos[1], f.pos[2], f.pos_delta);
+ if (f.q[0] != 1.0f) { // Output quaternion if available.
+ printer.printf("\t%.4f\t%.4f\t%.4f\t%.4f", f.q[0], f.q[1], f.q[2], f.q[3]);
+ }
  }
  printer.printf("\n");
 }

src/geometry.cpp

@@ -14,7 +14,7 @@ void calc_ray_vec(const BaseStationGeometryDef &bs, float angle1, float angle2,
 
 GeometryBuilder::GeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def,
  const Vector<BaseStationGeometryDef, num_base_stations> &base_stations) 
- : geo_builder_idx_(idx)
+ : object_idx_(idx)
  , base_stations_(base_stations)
  , def_(geo_def) {
  assert(idx < max_num_inputs);
@@ -26,69 +26,61 @@ GeometryBuilder::GeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def
 
 PointGeometryBuilder::PointGeometryBuilder(uint32_t idx, const GeometryBuilderDef &geo_def,
  const Vector<BaseStationGeometryDef, num_base_stations> &base_stations )
- : GeometryBuilder(idx, geo_def, base_stations) {
+ : GeometryBuilder(idx, geo_def, base_stations)
+ , pos_{Timestamp(), idx, FixLevel::kNoSignals, {0.f, 0.f, 0.f}, 0.f, {1.f, 0.f, 0.f, 0.f}} {
  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.
-
- // 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) {
- return; // Angles too stale.
+ // Coordinate system: Y - Up; X -> Z v (to the viewer)
+ // Station 'looks' to inverse Z axis (vector 0;0;-1).
+ pos_.time = f.time;
+ pos_.fix_level = f.fix_level;
+
+ if (f.fix_level >= FixLevel::kCycleSynced) {
+ const SensorLocalGeometry &sens_def = def_.sensors[0];
+ const SensorAngles &sens = f.sensors[sens_def.input_idx];
+
+ // Check angles are fresh enough.
+ uint32_t max_stale = 0;
+ for (int i = 0; i < num_cycle_phases; i++)
+ max_stale = max(max_stale, f.cycle_idx - sens.updated_cycles[i]);
+
+ if (max_stale < num_cycle_phases * 3) { // We tolerate stale angles up to 2 cycles old.
+ pos_.fix_level = (max_stale < num_cycle_phases) 
+ ? FixLevel::kFullFix : FixLevel::kStaleFix;
+
+ vec3d ray1{}, ray2{};
+ calc_ray_vec(base_stations_[0], sens.angles[0], sens.angles[1], ray1);
+ calc_ray_vec(base_stations_[1], sens.angles[2], sens.angles[3], ray2);
+
+ intersect_lines(base_stations_[0].origin, ray1, 
+ base_stations_[1].origin, ray2, &pos_.pos, &pos_.pos_delta);
+
+ // Translate object position depending on the position of sensor relative to object.
+ for (int i = 0; i < vec3d_size; i++)
+ pos_.pos[i] -= sens_def.pos[i];
+
+ } else {
+ // Angles too stale - cannot calculate position anymore.
+ pos_.fix_level = FixLevel::kPartialVis;
  }
+ }
 
- const float *angles = sens.angles;
- //Serial.printf("Angles: %.4f %.4f %.4f %.4f\n", angles[0], angles[1], angles[2], angles[3]);
-
- vec3d ray1 = {};
- calc_ray_vec(base_stations_[0], angles[0], angles[1], ray1);
- //Serial.printf("Ray1: %f %f %f\n", ray1[0], ray1[1], ray1[2]);
-
- vec3d ray2 = {};
- 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}
- };
-
- intersect_lines(base_stations_[0].origin, ray1, 
- base_stations_[1].origin, ray2, &geo.pos, &geo.pos_delta);
-
- last_success_ = f.time;
- for (int i = 0; i < vec3d_size; i++)
- geo.pos[i] -= sens_def.pos[i];
-
- produce(geo);
+ produce(pos_);
 }
 
 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 ? LedState::kFixFound : LedState::kNoFix);
+ set_led_state(pos_.fix_level >= FixLevel::kStaleFix ? LedState::kFixFound : LedState::kNoFix);
 }
 
 bool PointGeometryBuilder::debug_cmd(HashedWord *input_words) {
- if (*input_words == "geom#"_hash && input_words->idx == geo_builder_idx_) {
+ if (*input_words == "geom#"_hash && input_words->idx == object_idx_) {
  input_words++;
- return producer_debug_cmd(this, input_words, "ObjectPosition", geo_builder_idx_);
+ return producer_debug_cmd(this, input_words, "ObjectPosition", object_idx_);
  }
  return false;
 }

src/geometry.h

@@ -42,7 +42,7 @@ class GeometryBuilder
  const Vector<BaseStationGeometryDef, num_base_stations> &base_stations);
 
 protected:
- uint32_t geo_builder_idx_;
+ uint32_t object_idx_;
  const Vector<BaseStationGeometryDef, num_base_stations> &base_stations_;
  GeometryBuilderDef def_;
 };
@@ -59,7 +59,7 @@ class PointGeometryBuilder : public GeometryBuilder {
  virtual void debug_print(Print& stream);
 
 private:
- Timestamp last_success_; // LongTimestamp
+ ObjectPosition pos_;
 };
 
 

src/mavlink.cpp

@@ -20,6 +20,9 @@ GeometryMavlinkFormatter::GeometryMavlinkFormatter(uint32_t idx, const Formatter
 }
 
 bool GeometryMavlinkFormatter::position_valid(const ObjectPosition& g) {
+ if (g.fix_level < FixLevel::kStaleFix)
+ return false;
+
  // Filter out outliers.
  constexpr float max_position_jump = 0.05; // meters
  bool is_valid = false;

src/message_logging.h

@@ -19,8 +19,8 @@ inline void print_value<Pulse>(Print &stream, const Pulse& val) {
 
 template<>
 inline void print_value<SensorAnglesFrame>(Print &stream, const SensorAnglesFrame& val) {
- stream.printf("\n%dms: cycle %u, angles ", 
- val.time.get_value(msec), val.cycle_idx, val.phase_id);
+ stream.printf("\n%dms: cycle %u, fix %02d, angles ", 
+ val.time.get_value(msec), val.cycle_idx, (int)val.fix_level / 100);
  for (uint32_t i = 0; i < val.sensors.size(); i++) {
  auto sens = val.sensors[i];
  for (int32_t phase = 0; phase < num_cycle_phases; phase++) {
@@ -49,9 +49,10 @@ inline void print_value<DataFrame>(Print &stream, const DataFrame& frame) {
 
 template<>
 inline void print_value<ObjectPosition>(Print &stream, const ObjectPosition& val) {
- stream.printf("\n%dms: pos %.3f %.3f %.3f ", val.time.get_value(msec), val.pos[0], val.pos[1], val.pos[2]);
+ stream.printf("\n%dms: fix %2d, pos %.4f %.4f %.4f, dist %.4f ", val.time.get_value(msec), 
+ (int)val.fix_level/100, val.pos[0], val.pos[1], val.pos[2], val.pos_delta);
  if (val.q[0] != 1.0f)
- stream.printf("q %.3f %.3f %.3f %.3f ", val.q[0], val.q[1], val.q[2], val.q[3]);
+ stream.printf(" Q %.4f %.4f %.4f %.4f ", val.q[0], val.q[1], val.q[2], val.q[3]);
 }
 
 template<>

src/pulse_processor.cpp

@@ -86,7 +86,7 @@ void PulseProcessor::process_long_pulse(const Pulse &p) {
 }
 
 void PulseProcessor::process_short_pulse(const Pulse &p) {
- if (cycle_fix_level_ >= kCycleFixAcquired && p.input_idx < num_inputs_) {
+ if (cycle_fix_level_ >= kCycleFixCandidate && p.input_idx < num_inputs_) {
  // TODO: Filter out pulses outside of current cycle.
  cycle_short_pulses_.push(p);
  }
@@ -158,18 +158,18 @@ void PulseProcessor::process_cycle_fix(Timestamp cur_time) {
  angles.angles[cycle_phase] = (short_pulse_timings[i] - angle_center_len) / cycle_period * (float)M_PI;
  angles.updated_cycles[cycle_phase] = cycle_idx_;
  }
-
- // Send the data down the pipeline every cycle. Consumers will be able to filter as needed.
+ }
+
+ // Send the data down the pipeline every 4th cycle (30Hz). Can be increased to 120Hz if needed.
+ if ((cycle_phase >= 0) ? (cycle_phase == 3) : (cycle_idx_ % 4 == 0)) {
  angles_frame_.time = cycle_start_time_;
+ angles_frame_.fix_level = (cycle_phase >= 0 && cycle_fix_level_ >= kCycleFixAcquired)
+ ? FixLevel::kCycleSynced : FixLevel::kCycleSyncing;
  angles_frame_.cycle_idx = cycle_idx_;
  angles_frame_.phase_id = cycle_phase;
  Producer<SensorAnglesFrame>::produce(angles_frame_);
-
- } else {
- angles_frame_.phase_id = cycle_phase;
- // We don't know the phase for now. Skip.
  }
-
+ 
  // Prepare for the next cycle.
  reset_cycle_pulses();
  cycle_start_time_ += cycle_period;
@@ -188,6 +188,15 @@ void PulseProcessor::do_work(Timestamp cur_time) {
  if (cur_time - cycle_start_time_ > cycle_processing_point) {
  process_cycle_fix(cur_time);
  }
+ } else { // No fix.
+ if (throttle_ms(TimeDelta(1000, ms), cur_time, &cycle_start_time_)) {
+ // Send frame showing we have no signals.
+ angles_frame_.time = cur_time;
+ angles_frame_.fix_level = FixLevel::kNoSignals;
+ angles_frame_.cycle_idx = 0;
+ angles_frame_.phase_id = 0;
+ Producer<SensorAnglesFrame>::produce(angles_frame_);
+ }
  }
 }