Fixed incorrect base station cycle phase classification.

src/cycle_phase_classifier.cpp

@@ -24,7 +24,7 @@ CyclePhaseClassifier::CyclePhaseClassifier()
 void CyclePhaseClassifier::process_pulse_lengths(uint32_t cycle_idx, const TimeDelta (&pulse_lens)[num_base_stations]) {
  int cur_phase_id = -1;
  if (pulse_lens[0] > TimeDelta(0, usec) && pulse_lens[1] > TimeDelta(0, usec)) {
- int cur_more = pulse_lens[0] < pulse_lens[1];
+ int cur_more = pulse_lens[0] > pulse_lens[1];
  if (cycle_idx == prev_full_cycle_idx_ + 1) {
  // To get current phase, we use simple fact that in phases 0 and 1, first pulse is shorter than the second,
  // and in phases 2, 3 it is longer. This allows us to estimate current phase using comparison between 
@@ -67,7 +67,7 @@ DataFrameBitPair CyclePhaseClassifier::get_data_bits(uint32_t cycle_idx, const T
  if (phase_id >= 0) {
  for (int b = 0; b < num_base_stations; b++) 
  if (pulse_lens[b] > TimeDelta(0, usec)) {
- bool skip = (phase_id >> 1) == b;
+ bool skip = (phase_id >> 1) != b;
  bool axis = phase_id & 0x1;
 
  // Get the middle value of pulse width between bits 0 and 1.
@@ -122,7 +122,7 @@ bool CyclePhaseClassifier::debug_cmd(HashedWord *input_words) {
 }
 void CyclePhaseClassifier::debug_print(Print &stream) {
  if (debug_print_state_) {
- stream.printf("CyclePhaseClassifier: fix %d, phase %d, pulse_len %f, history 0x%x, avg error %.1f us\n", 
+ stream.printf("CyclePhaseClassifier: fix %d, phase %d, pulse_base_len %f, history 0x%x, avg error %.1f us\n", 
  fix_level_, phase_shift_, pulse_base_len_, phase_history_, average_error_);
  }
 }

src/pulse_processor.cpp

@@ -140,10 +140,11 @@ void PulseProcessor::process_cycle_fix(Timestamp cur_time) {
  // From (potentially several) short pulses for the same input, we choose the longest one.
  Pulse *short_pulses[max_num_inputs] = {};
  TimeDelta short_pulse_timings[max_num_inputs] = {};
+ TimeDelta base_station_delta = long_pulse_starts[cycle_phase >> 1];
  for (uint32_t i = 0; i < cycle_short_pulses_.size(); i++) {
  Pulse *p = &cycle_short_pulses_[i];
  uint32_t input_idx = p->input_idx;
- TimeDelta pulse_timing = p->start_time - cycle_start_time_ + p->pulse_len / 2;
+ TimeDelta pulse_timing = p->start_time - cycle_start_time_ + p->pulse_len / 2 - base_station_delta;
  if (short_pulse_min_time < pulse_timing && pulse_timing < short_pulse_max_time)
  if (!short_pulses[input_idx] || short_pulses[input_idx]->pulse_len < p->pulse_len) {
  short_pulses[input_idx] = p;

src/settings.cpp

@@ -17,10 +17,10 @@ sensor0 pin 12 positive
 base0 origin -1.528180 2.433750 -1.969390 matrix -0.841840 0.332160 -0.425400 -0.046900 0.740190 0.670760 0.537680 0.584630 -0.607540
 base1 origin 1.718700 2.543170 0.725060 matrix 0.458350 -0.649590 0.606590 0.028970 0.693060 0.720300 -0.888300 -0.312580 0.336480
 object0 sensor0
-serial1 57600
-stream0 mavlink object0 ned 110 > serial1
+stream0 position object0 > usb_serial
 stream1 angles > usb_serial
-stream2 position object0 > usb_serial
+serial1 57600
+stream2 mavlink object0 ned 110 > serial1
 */
 
 PersistentSettings settings;