#570 Using constant for nr of base stations

bitcraze · Apr 18, 2020 · 0387a08 · 0387a08
1 parent df7d81d
commit 0387a08
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 11 deletions.
diff --git a/src/modules/interface/lighthouse/lighthouse_position_est.h b/src/modules/interface/lighthouse/lighthouse_position_est.h
@@ -31,7 +31,7 @@
 #include "lighthouse_geometry.h"
 
 
-extern baseStationGeometry_t lighthouseBaseStationsGeometry[2];
+extern baseStationGeometry_t lighthouseBaseStationsGeometry[PULSE_PROCESSOR_N_BASE_STATIONS];
 
 /**
  * @brief Call when the lighthouseBaseStationsGeometry data has been updated

diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c
@@ -64,7 +64,7 @@ static STATS_CNT_RATE_DEFINE(cycleRate, ONE_SECOND);
 
 static STATS_CNT_RATE_DEFINE(bs0Rate, HALF_SECOND);
 static STATS_CNT_RATE_DEFINE(bs1Rate, HALF_SECOND);
-static statsCntRateLogger_t* bsRates[2] = {&bs0Rate, &bs1Rate};
+static statsCntRateLogger_t* bsRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&bs0Rate, &bs1Rate};
 
 static uint16_t pulseWidth[PULSE_PROCESSOR_N_SENSORS];
 static pulseProcessor_t ppState = {};

diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c
@@ -35,7 +35,7 @@
 
 #include "lighthouse_position_est.h"
 
-baseStationGeometry_t lighthouseBaseStationsGeometry[2]  = {
+baseStationGeometry_t lighthouseBaseStationsGeometry[PULSE_PROCESSOR_N_BASE_STATIONS]  = {
 {.origin = {-1.958483,  0.542299,  3.152727, }, .mat = {{0.79721498, -0.004274, 0.60368103, }, {0.0, 0.99997503, 0.00708, }, {-0.60369599, -0.005645, 0.79719502, }, }},
 {.origin = {1.062398, -2.563488,  3.112367, }, .mat = {{0.018067, -0.999336, 0.031647, }, {0.76125097, 0.034269, 0.64755201, }, {-0.648206, 0.012392, 0.76136398, }, }},
 };
@@ -46,19 +46,18 @@ baseStationGeometry_t lighthouseBaseStationsGeometry[2]  = {
 static STATS_CNT_RATE_DEFINE(positionRate, ONE_SECOND);
 static STATS_CNT_RATE_DEFINE(estBs0Rate, HALF_SECOND);
 static STATS_CNT_RATE_DEFINE(estBs1Rate, HALF_SECOND);
-static statsCntRateLogger_t* bsEstRates[2] = {&estBs0Rate, &estBs1Rate};
+static statsCntRateLogger_t* bsEstRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&estBs0Rate, &estBs1Rate};
 
-baseStationEulerAngles_t lighthouseBaseStationAngles[2];
-static mat3d baseStationInvertedRotationMatrixes[2];
+baseStationEulerAngles_t lighthouseBaseStationAngles[PULSE_PROCESSOR_N_BASE_STATIONS];
+static mat3d baseStationInvertedRotationMatrixes[PULSE_PROCESSOR_N_BASE_STATIONS];
 
 static void invertRotationMatrix(mat3d rot, mat3d inverted);
 
 void lightHousePositionGeometryDataUpdated() {
-  lighthouseGeometryCalculateAnglesFromRotationMatrix(&lighthouseBaseStationsGeometry[0], &lighthouseBaseStationAngles[0]);
-  lighthouseGeometryCalculateAnglesFromRotationMatrix(&lighthouseBaseStationsGeometry[1], &lighthouseBaseStationAngles[1]);
-
-  invertRotationMatrix(lighthouseBaseStationsGeometry[0].mat, baseStationInvertedRotationMatrixes[0]);
-  invertRotationMatrix(lighthouseBaseStationsGeometry[1].mat, baseStationInvertedRotationMatrixes[1]);
+  for (int i = 0; i < PULSE_PROCESSOR_N_BASE_STATIONS; i++) {
+    lighthouseGeometryCalculateAnglesFromRotationMatrix(&lighthouseBaseStationsGeometry[i], &lighthouseBaseStationAngles[i]);
+    invertRotationMatrix(lighthouseBaseStationsGeometry[i].mat, baseStationInvertedRotationMatrixes[i]);
+  }
 }