#641 Removed unused euler angles

bitcraze · Nov 10, 2020 · 364cb79 · 364cb79
1 parent 10dda04
commit 364cb79
Show file tree

Hide file tree

Showing 3 changed files with 0 additions and 44 deletions.
diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c
@@ -55,8 +55,6 @@ static STATS_CNT_RATE_DEFINE(estBs0Rate, HALF_SECOND);
 static STATS_CNT_RATE_DEFINE(estBs1Rate, HALF_SECOND);
 static statsCntRateLogger_t* bsEstRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&estBs0Rate, &estBs1Rate};
 
-baseStationEulerAngles_t lighthouseBaseStationAngles[PULSE_PROCESSOR_N_BASE_STATIONS];
-
 // The light planes in LH2 are tilted +- 30 degrees
 static const float t30 = M_PI / 6;
 
@@ -114,7 +112,6 @@ static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const
 
 static void lighthousePositionGeometryDataUpdated() {
   for (int i = 0; i < PULSE_PROCESSOR_N_BASE_STATIONS; i++) {
-    lighthouseGeometryCalculateAnglesFromRotationMatrix(&lighthouseBaseStationsGeometry[i], &lighthouseBaseStationAngles[i]);
     preProcessGeometryData(lighthouseBaseStationsGeometry[i].mat, baseStationInvertedRotationMatrixes[i], lh1Rotor2RotationMatrixes[i], lh1Rotor2InvertedRotationMatrixes[i]);
   }
 }

diff --git a/src/utils/interface/lighthouse/lighthouse_geometry.h b/src/utils/interface/lighthouse/lighthouse_geometry.h
@@ -9,12 +9,6 @@ typedef struct baseStationGeometry_s {
   mat3d mat;
 } __attribute__((packed)) baseStationGeometry_t;
 
-typedef struct baseStationEulerAngles_s {
-  float roll;
-  float pitch;
-  float yaw;
-} baseStationEulerAngles_t;
-
 /**
  * @brief Find closest point between rays from two bases stations.
  *
@@ -81,11 +75,3 @@ void lighthouseGeometryGetSensorPosition(const vec3d cfPos, const arm_matrix_ins
  * @return true if the angle could be calculated
 */
 bool lighthouseGeometryYawDelta(const vec3d ipv, const vec3d spv, const vec3d n, float* yawDelta);
-
-/**
- * @brief Get the roll, pitch, yaw angles of the basestation rotation matrix
- * Note: the origin of the Crazyflie is set in the center of the deck
- *
- * @param baseStation - Geometry data for the base station (position and orientation)
- */
-void lighthouseGeometryCalculateAnglesFromRotationMatrix(baseStationGeometry_t* baseStationGeometry, baseStationEulerAngles_t* baseStationEulerAngles);
diff --git a/src/utils/src/lighthouse/lighthouse_geometry.c b/src/utils/src/lighthouse/lighthouse_geometry.c
@@ -199,30 +199,3 @@ bool lighthouseGeometryYawDelta(const vec3d ipv, const vec3d spv, const vec3d n,
     *yawDelta = delta;
     return true;
 }
-
-void lighthouseGeometryCalculateAnglesFromRotationMatrix(baseStationGeometry_t* baseStationGeometry, baseStationEulerAngles_t* baseStationEulerAngles) {
-
-   /*
-    * roll pitch yaw Rotation matrix
-    *
-    R = | R00 R01 R02 |
-        | R10 R11 R12 |
-        | R20 R21 R22 |
-        */
-
-  float32_t R00 = (float32_t)baseStationGeometry->mat[0][0];
-  float32_t R01 = (float32_t)baseStationGeometry->mat[0][1];
-  float32_t R02 = (float32_t)baseStationGeometry->mat[0][2];
-  float32_t R12 = (float32_t)baseStationGeometry->mat[1][2];
-  float32_t R22 = (float32_t)baseStationGeometry->mat[2][2];
-
-
-  float pitchBaseStation = asin(R02);
-  float yawBaseStation = -1.0*atan2(R01,R00);
-  float rollBaseStation = -1.0*atan2(R12,R22);
-
-  baseStationEulerAngles->roll=rollBaseStation;
-  baseStationEulerAngles->pitch=pitchBaseStation;
-  baseStationEulerAngles->yaw=yawBaseStation;
-
-}