#461 added functions to lighthouse to get the eulerangles from basest…

…ation

src/deck/drivers/src/lighthouse.c

@@ -67,6 +67,8 @@ baseStationGeometry_t lighthouseBaseStationsGeometry[2]  = {
 {.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, }, }},
 };
 
+baseStationEulerAngles_t lighthouseBaseStationAngles[2];
+
 // Uncomment if you want to force the Crazyflie to reflash the deck at each startup
 // #define FORCE_FLASH true
 
@@ -284,6 +286,10 @@ static void lighthouseTask(void *param)
   int basestation;
   int axis;
 
+  // Get the eulerangles from the rotation matrix of the basestations
+  lighthouseGeometryCalculateAnglesFromRotationMatrix(&lighthouseBaseStationsGeometry[0],&lighthouseBaseStationAngles[0]);
+  lighthouseGeometryCalculateAnglesFromRotationMatrix(&lighthouseBaseStationsGeometry[1],&lighthouseBaseStationAngles[1]);
+
   systemWaitStart();
 
 #ifdef LH_FLASH_DECK

src/utils/interface/lighthouse/lighthouse_geometry.h

@@ -10,11 +10,19 @@ typedef float vec3d[vec3d_size];
 typedef struct baseStationGeometry_s {
   float origin[3];
   float mat[3][3];
+  float eulerAngles[3];
 } __attribute__((packed)) baseStationGeometry_t;
 
+typedef struct baseStationEulerAngles_s {
+  float roll;
+  float pitch;
+  float yaw;
+} baseStationEulerAngles_t;
+
 bool lighthouseGeometryGetPositionFromRayIntersection(baseStationGeometry_t baseStations[2], float angles[4], vec3d position, float *position_delta);
 void lighthouseGeometryGetBaseStationPosition(baseStationGeometry_t* baseStationGeometry, vec3d baseStationPos);
 void lighthouseGeometryGetRay(const baseStationGeometry_t* baseStationGeometry, const float angle1, const float angle2, vec3d ray);
 bool lighthouseGeometryIntersectionPlaneVector(const vec3d linePoint, const vec3d lineVec, const vec3d planePoint, const vec3d PlaneNormal, vec3d intersectionPoint);
 void lighthouseGeometryGetSensorPosition(const vec3d cfPos, const arm_matrix_instance_f32 *R, const int sensor, vec3d pos);
 bool lighthouseGeometryYawDelta(const vec3d ipv, const vec3d spv, const vec3d n, float* yawDelta);
+void lighthouseGeometryCalculateAnglesFromRotationMatrix(baseStationGeometry_t* baseStationGeometry, baseStationEulerAngles_t* baseStationEulerAngles);

src/utils/src/lighthouse/lighthouse_geometry.c

@@ -263,3 +263,36 @@ bool lighthouseGeometryYawDelta(const vec3d ipv, const vec3d spv, const vec3d n,
     *yawDelta = delta;
     return true;
 }
+
+/**
+ * @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) {
+
+   /*
+    * 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;
+
+}