Merge pull request #1313 from bitcraze/krichardsson/lh-gt

Fix LH as ground truth for LH1
bitcraze · Sep 12, 2023 · 4e8568a · 4e8568a
2 parents e176751 + 564d772
commit 4e8568a
Showing 1 changed file with 15 additions and 10 deletions.
diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c
@@ -301,9 +301,12 @@ static void estimatePositionSweepsLh1(const pulseProcessor_t* appState, pulsePro
         sweepInfo.calib = &bsCalib->sweep[0];
         sweepInfo.sweepId = 0;
 
-        estimatorEnqueueSweepAngles(&sweepInfo);
-        STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
-        STATS_CNT_RATE_EVENT(&positionRate);
+        #ifndef CONFIG_DECK_LIGHTHOUSE_AS_GROUNDTRUTH
+          estimatorEnqueueSweepAngles(&sweepInfo);
+
+          STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
+          STATS_CNT_RATE_EVENT(&positionRate);
+        #endif
       }
 
       sweepInfo.measuredSweepAngle = measurement->angles[1];
@@ -315,10 +318,10 @@ static void estimatePositionSweepsLh1(const pulseProcessor_t* appState, pulsePro
 
         #ifndef CONFIG_DECK_LIGHTHOUSE_AS_GROUNDTRUTH
           estimatorEnqueueSweepAngles(&sweepInfo);
-        #endif
 
-        STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
-        STATS_CNT_RATE_EVENT(&positionRate);
+          STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
+          STATS_CNT_RATE_EVENT(&positionRate);
+        #endif
       }
     }
   }
@@ -347,9 +350,10 @@ static void estimatePositionSweepsLh2(const pulseProcessor_t* appState, pulsePro
         sweepInfo.sweepId = 0;
         #ifndef CONFIG_DECK_LIGHTHOUSE_AS_GROUNDTRUTH
           estimatorEnqueueSweepAngles(&sweepInfo);
+
+          STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
+          STATS_CNT_RATE_EVENT(&positionRate);
         #endif
-        STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
-        STATS_CNT_RATE_EVENT(&positionRate);
       }
 
       sweepInfo.measuredSweepAngle = measurement->angles[1];
@@ -359,9 +363,10 @@ static void estimatePositionSweepsLh2(const pulseProcessor_t* appState, pulsePro
         sweepInfo.sweepId = 1;
         #ifndef CONFIG_DECK_LIGHTHOUSE_AS_GROUNDTRUTH
           estimatorEnqueueSweepAngles(&sweepInfo);
+
+          STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
+          STATS_CNT_RATE_EVENT(&positionRate);
         #endif
-        STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
-        STATS_CNT_RATE_EVENT(&positionRate);
       }
     }
   }