Generic rotation matrix for mounted stereocamera (#1998)

This pull request adds a rotation matrix to the stereocam module group. The stereocam in combination with the lisa-s, the new lisa MXs, and the ardrone are usually mounted on there in different direction (facing backwards, forwards and downwards). Before, we had to constantly double check the direction of the velocities to transform that manually to body fixed coordinates, which was very prone to mistakes. Same goes with the derotation of optical flow on the stereocam.

This addition should make it easier and more generic for platforms with the mounted stereocamera.

sw/airborne/modules/stereocam/state2camera/state2camera.c

@@ -24,20 +24,30 @@
  */
 
 #include "modules/stereocam/state2camera/state2camera.h"
+#include "modules/stereocam/stereocam.h"
 #include "modules/stereocam/stereoprotocol.h"
 #include "subsystems/abi.h"
 #include "state.h"
 #include "mcu_periph/uart.h"
-static int frame_number_sending = 0;
 float lastKnownHeight = 0.0;
 int pleaseResetOdroid = 0;
 
 #ifndef STATE2CAMERA_SEND_DATA_TYPE
 #define STATE2CAMERA_SEND_DATA_TYPE 0
 #endif
+PRINT_CONFIG_VAR(STATE2CAMERA_SEND_DATA_TYPE)
+
+#if STATE2CAMERA_SEND_DATA_TYPE == 0
+static int frame_number_sending = 0;
+#endif
+#if STATE2CAMERA_SEND_DATA_TYPE == 1
+uint8_t stereocam_derotation_on = 1;
+#endif
+
 
 void write_serial_rot()
 {
+// 0 = send rotation matrix to stereocam
 #if STATE2CAMERA_SEND_DATA_TYPE == 0
 
   struct Int32RMat *ltp_to_body_mat = stateGetNedToBodyRMat_i();
@@ -52,15 +62,31 @@ void write_serial_rot()
   stereoprot_sendArray(&((UART_LINK).device), ar, lengthArrayInformation, 1);
 #endif
 
+  // 0 = send euler angles to stereocam (EdgeFlow)
 #if STATE2CAMERA_SEND_DATA_TYPE == 1
+
+
+  // rotate body angles to camera reference frame
+  // TODO: When available in paparazzi for matrix multiplications, use FloatEulers
+
+  struct FloatVect3 body_state;
+  body_state.x = stateGetNedToBodyEulers_f()->phi;
+  body_state.y = stateGetNedToBodyEulers_f()->theta;
+  body_state.z = stateGetNedToBodyEulers_f()->psi;
+
+  struct FloatVect3 cam_angles;
+  float_rmat_vmult(&cam_angles, &body_to_stereocam, &body_state);
+
   static int16_t lengthArrayInformation = 6 * sizeof(int16_t);
   uint8_t ar[lengthArrayInformation];
   int16_t *pointer = (int16_t *) ar;
-  pointer[0] =   (int16_t)(stateGetNedToBodyEulers_f()->theta*100);
-  pointer[1] =    (int16_t)(stateGetNedToBodyEulers_f()->phi*100);
-  pointer[2] =    (int16_t)(stateGetNedToBodyEulers_f()->psi*100);
+  pointer[0] = (int16_t)(cam_angles.x * 100);   // Roll
+  pointer[1] = (int16_t)(cam_angles.y * 100);   // Pitch
+  pointer[2] = (int16_t)(cam_angles.z * 100);   // Yaw
+  pointer[3] = (int16_t)(stereocam_derotation_on);   // derotation boolean
+
+  stereoprot_sendArray(&((UART_LINK).device), ar, lengthArrayInformation, 1);
 
-  stereoprot_sendArray(&((UART_LINK).device), ar,lengthArrayInformation, 1);
 
 #endif
 

sw/airborne/modules/stereocam/state2camera/state2camera.h

@@ -28,6 +28,6 @@
 
 #include <inttypes.h>
 extern void write_serial_rot(void);
-
+extern uint8_t stereocam_derotation_on;
 #endif
 

sw/airborne/modules/stereocam/stereocam.c

@@ -35,6 +35,42 @@
 #define SEND_STEREO TRUE
 #endif
 
+
+/* This defines the location of the stereocamera with respect to the body fixed coordinates.
+ *
+ *    Coordinate system stereocam (looking into the lens)
+ *    x      z
+ * <-----(( (*) ))              ((     )) = camera lens
+ *           |                     (*)    = arrow pointed outwards
+ *           | y                              (towards your direction)
+ *           V
+ *
+ * The conversion order in euler angles is psi (yaw) -> theta (pitch) -> phi (roll)
+ *
+ * Standard rotations: MAV NED body to stereocam in Deg:
+ * - facing forward:   90 -> 0 -> 90
+ * - facing backward: -90 -> 0 -> 90
+ * - facing downward:  90 -> 0 -> 0
+ */
+
+#if !defined(STEREO_BODY_TO_STEREO_PHI) || !defined(STEREO_BODY_TO_STEREO_THETA) || !defined(STEREO_BODY_TO_STEREO_PSI)
+#warning "STEREO_BODY_TO_STEREO_XXX not defined. Using default Euler rotation angles (0,0,0)"
+#endif
+
+#ifndef STEREO_BODY_TO_STEREO_PHI
+#define STEREO_BODY_TO_STEREO_PHI 0
+#endif
+
+#ifndef STEREO_BODY_TO_STEREO_THETA
+#define STEREO_BODY_TO_STEREO_THETA 0
+#endif
+
+#ifndef STEREO_BODY_TO_STEREO_PSI
+#define STEREO_BODY_TO_STEREO_PSI 0
+#endif
+
+struct FloatRMat body_to_stereocam;
+
 // define coms link for stereocam
 #define STEREO_PORT   (&((UART_LINK).device))
 struct link_device *linkdev = STEREO_PORT;
@@ -43,7 +79,6 @@ struct link_device *linkdev = STEREO_PORT;
 // pervasive local variables
 MsgProperties msgProperties;
 
-
 uint16_t freq_counter = 0;
 uint8_t frequency = 0;
 uint32_t previous_time = 0;
@@ -76,6 +111,9 @@ extern void stereocam_disparity_to_meters(uint8_t *disparity, float *distancesMe
 
 extern void stereocam_start(void)
 {
+  struct FloatEulers euler = {STEREO_BODY_TO_STEREO_PHI, STEREO_BODY_TO_STEREO_THETA, STEREO_BODY_TO_STEREO_PSI};
+  float_rmat_of_eulers(&body_to_stereocam, &euler);
+
   // initialize local variables
   msgProperties = (MsgProperties) {0, 0, 0};
 
@@ -110,11 +148,9 @@ extern void stereocam_periodic(void)
 #if SEND_STEREO
       if (stereocam_data.len > 100) {
         DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &frequency, &(stereocam_data.len), 100, stereocam_data.data);
-
       } else {
         DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &frequency, &(stereocam_data.len), stereocam_data.len,
                                  stereocam_data.data);
-
       }
 #endif
     }

sw/airborne/modules/stereocam/stereocam.h

@@ -38,6 +38,7 @@ typedef struct {
 } uint8array;
 
 extern uint8array stereocam_data;
+extern struct FloatRMat body_to_stereocam;
 
 extern void stereocam_disparity_to_meters(uint8_t *, float *, int);
 extern void stereocam_start(void);

sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c

@@ -22,6 +22,18 @@
 #ifndef STEREOCAM2STATE_RECEIVED_DATA_TYPE
 #define STEREOCAM2STATE_RECEIVED_DATA_TYPE 0
 #endif
+PRINT_CONFIG_VAR(STEREOCAM2STATE_RECEIVED_DATA_TYPE)
+
+#if STEREOCAM2STATE_RECEIVED_DATA_TYPE == 0
+#ifndef STEREOCAM2STATE_EDGEFLOW_PIXELWISE
+#define STEREOCAM2STATE_EDGEFLOW_PIXELWISE FALSE
+PRINT_CONFIG_VAR(STEREOCAM2STATE_EDGEFLOW_PIXELWISE)
+#endif
+uint8_t stereocam_medianfilter_on = 1;
+#endif
+
+#include "filters/median_filter.h"
+struct MedianFilterInt medianfilter_x, medianfilter_y, medianfilter_z;
 
 #include "subsystems/datalink/telemetry.h"
 
@@ -30,23 +42,28 @@ void stereocam_to_state(void);
 void stereo_to_state_init(void)
 {
 
+  init_median_filter(&medianfilter_x);
+  init_median_filter(&medianfilter_y);
+  init_median_filter(&medianfilter_z);
 }
 
 void stereo_to_state_periodic(void)
 {
   if (stereocam_data.fresh) {
-	  stereocam_to_state();
+    stereocam_to_state();
     stereocam_data.fresh = 0;
   }
 }
 
 void stereocam_to_state(void)
 {
+  int16_t RES = 100;
+
+  // Sort the info from stereocam data from UART
 
-  // Get info from stereocam data
   // 0 = stereoboard's #define SEND_EDGEFLOW
 #if STEREOCAM2STATE_RECEIVED_DATA_TYPE == 0
-  // opticflow
+  // opticflow and divergence (unscaled with depth)
   int16_t div_x = (int16_t)stereocam_data.data[0] << 8;
   div_x |= (int16_t)stereocam_data.data[1];
   int16_t flow_x = (int16_t)stereocam_data.data[2] << 8;
@@ -56,48 +73,74 @@ void stereocam_to_state(void)
   int16_t flow_y = (int16_t)stereocam_data.data[6] << 8;
   flow_y |= (int16_t)stereocam_data.data[7];
 
+// uint8_t agl = stereocam_data.data[8]; // in cm //TODO: use agl for in a guided obstacle avoidance.
   float fps = (float)stereocam_data.data[9];
-  //int8_t agl = stereocam_data.data[8]; // in cm
 
-  // velocity
-  int16_t vel_x_int = (int16_t)stereocam_data.data[10] << 8;
-  vel_x_int |= (int16_t)stereocam_data.data[11];
-  int16_t vel_y_int = (int16_t)stereocam_data.data[12] << 8;
-  vel_y_int |= (int16_t)stereocam_data.data[13];
+  // velocity global
+  int16_t vel_x_global_int = (int16_t)stereocam_data.data[10] << 8;
+  vel_x_global_int |= (int16_t)stereocam_data.data[11];
+  int16_t vel_y_global_int = (int16_t)stereocam_data.data[12] << 8;
+  vel_y_global_int |= (int16_t)stereocam_data.data[13];
+  int16_t vel_z_global_int = (int16_t)stereocam_data.data[14] << 8;
+  vel_z_global_int |= (int16_t)stereocam_data.data[15];
 
-  int16_t RES = 100;
+  // Velocity Pixelwise
+  int16_t vel_x_pixelwise_int = (int16_t)stereocam_data.data[16] << 8;
+  vel_x_pixelwise_int |= (int16_t)stereocam_data.data[17];
+  int16_t vel_z_pixelwise_int = (int16_t)stereocam_data.data[18] << 8;
+  vel_z_pixelwise_int |= (int16_t)stereocam_data.data[19];
 
-  float vel_x = (float)vel_x_int / RES;
-  float vel_y = (float)vel_y_int / RES;
+// Select what type of velocity estimate fom edgeflow is wanted
+#if STEREOCAM2STATE_EDGEFLOW_PIXELWISE == TRUE
+  struct FloatVect3 camera_frame_vel;
+  camera_frame_vel.x = (float)vel_x_pixelwise_int / RES;
+  camera_frame_vel.y = (float)vel_y_global_int / RES;
+  camera_frame_vel.z = (float)vel_z_pixelwise_int / RES;
 
-  // Derotate velocity and transform from frame to body coordinates
-  // TODO: send resolution directly from stereocam
+#else
+  struct FloatVect3 camera_frame_vel;
+  camera_frame_vel.x = (float)vel_x_global_int / RES;
+  camera_frame_vel.y = (float)vel_y_global_int / RES;
+  camera_frame_vel.z = (float)vel_z_global_int / RES;
 
-  float vel_body_x = - vel_x;
-  float vel_body_y = vel_y;
+#endif
+
+//Rotate velocity back to quad's frame
+  struct FloatVect3 quad_body_vel;
+  float_rmat_transp_vmult(&quad_body_vel, &body_to_stereocam, &camera_frame_vel);
 
   //Send velocity estimate to state
   //TODO:: Make variance dependable on line fit error, after new horizontal filter is made
   uint32_t now_ts = get_sys_time_usec();
 
-  if (!(abs(vel_body_x) > 0.5 || abs(vel_body_x) > 0.5))
-  {
-    AbiSendMsgVELOCITY_ESTIMATE(STEREOCAM2STATE_SENDER_ID, now_ts,
-                                vel_body_x,
-                                vel_body_y,
-                                0.0f,
-                                0.3f
-                               );
+  float vel_body_x_processed = quad_body_vel.x;
+  float vel_body_y_processed = quad_body_vel.y;
+  float vel_body_z_processed = quad_body_vel.z;
+
+  if (stereocam_medianfilter_on == 1) {
+    // Use a slight median filter to filter out the large outliers before sending it to state
+    // TODO: if a float median filter exist, replace this version
+    vel_body_x_processed = (float)update_median_filter(&medianfilter_x, (int32_t)(quad_body_vel.x * 100)) / 100;
+    vel_body_y_processed = (float)update_median_filter(&medianfilter_y, (int32_t)(quad_body_vel.y * 100)) / 100;
+    vel_body_z_processed = (float)update_median_filter(&medianfilter_z, (int32_t)(quad_body_vel.z * 100)) / 100;
   }
 
-  // Reusing the OPTIC_FLOW_EST telemetry messages, with some values replaced by 0
+  //Send velocities to state
+  AbiSendMsgVELOCITY_ESTIMATE(STEREOCAM2STATE_SENDER_ID, now_ts,
+                              vel_body_x_processed,
+                              vel_body_y_processed,
+                              vel_body_z_processed,
+                              0.3f
+                             );
 
+  // Reusing the OPTIC_FLOW_EST telemetry messages, with some values replaced by 0
   uint16_t dummy_uint16 = 0;
   int16_t dummy_int16 = 0;
   float dummy_float = 0;
 
-  DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice, &fps, &dummy_uint16, &dummy_uint16, &flow_x, &flow_y, &dummy_int16, &dummy_int16,
-		  &vel_x, &vel_y,&dummy_float, &dummy_float, &dummy_float);
+  DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice, &fps, &dummy_uint16, &dummy_uint16, &flow_x, &flow_y,
+                               &dummy_int16, &dummy_int16, &vel_body_x_processed, &vel_body_y_processed,
+                               &dummy_float, &dummy_float, &dummy_float);
 
 #endif
 

sw/airborne/modules/stereocam/stereocam2state/stereocam2state.h

@@ -16,6 +16,8 @@
 #include <std.h>
 #include "modules/stereocam/stereocam.h"
 
+extern uint8_t stereocam_medianfilter_on;
+
 extern void stereo_to_state_init(void);
 extern void stereo_to_state_periodic(void);