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_);
+        }
     }
 }