[modules] stereocam2state improvements/fixes

- changed description stereocam2state and replaced extern gps variable with abi message
- stereocam2state fixed syntax and added downlink message

closes #1490

conf/TUDELFT/tudelft_KM_control_panel.xml

@@ -78,6 +78,12 @@
         <arg flag="-ac" constant="@AIRCRAFT"/>
         <arg flag="hobbyking.xml"/>
       </program>
+    <program name="BluegigaUsbDongle" command="sw/tools/bluegiga_usb_dongle/bluegiga_usb_driver">
+      <arg flag="/dev/ttyACM2"/>
+      <arg flag="00:07:80:2d:e0:4b" />
+      <arg flag="4242" />
+      <arg flag="4252" />
+    </program>
     </session>
   </section>
 </control_panel>

conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml

@@ -214,6 +214,7 @@
    <load name="stereocam.xml">
      <configure name="STEREO_UART" value="UART1"/>
      <configure name="STEREO_BAUD" value="B115200"/>
+     <define name="SEND_STEREO" value="FALSE"/>
    </load>
    <load name="stereocam_stereocam2state.xml"/>
  </modules>

conf/modules/stereocam_stereocam2state.xml

@@ -12,7 +12,7 @@
     <file name="stereocam2state.h"/>
   </header>
   <init fun="stereo_to_state_init()"/>
-  <periodic fun="stereo_to_state_periodic()"  start="stereo_to_state_start()" stop="stereo_to_state_stop()" autorun="TRUE"/>
+  <periodic fun="stereo_to_state_periodic()" autorun="TRUE"/>
   <makefile>
     <file name="stereocam2state.c"/>
   </makefile>

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

@@ -11,19 +11,24 @@
  */
 
 #include "modules/stereocam/stereocam2state/stereocam2state.h"
-#include "modules/stereocam/stereocam.h"
 
 #include "subsystems/abi.h"
-#include "state.h"
+#include "subsystems/datalink/telemetry.h"
 
-#include "math/pprz_algebra_int.h"
-#include "math/pprz_algebra_float.h"
-#include "math/pprz_orientation_conversion.h"
-#include "subsystems/gps.h"
+//#include "subsystems/gps.h"
 
 #ifndef SENDER_ID
 #define SENDER_ID 1
 #endif
+
+/** ABI binding for gps messages*/
+#ifndef STEREOCAM_GPS_ID
+#define STEREOCAM_GPS_ID ABI_BROADCAST
+#endif
+static abi_event gps_ev;
+
+
+/** For extra functionality for derotation of velocity to state measurements*/
 #ifndef USE_DEROTATION_OPTICFLOW
 #define USE_DEROTATION_OPTICFLOW FALSE
 #endif
@@ -34,13 +39,29 @@
 static float prev_phi;
 static float prev_theta;
 
-
+struct GpsStereoCam gps_stereocam;
 
 void stereocam_to_state(float dphi, float dtheta);
 
-void stereo_to_state_init(void) {}
+static void stereocam_gps_cb(uint8_t sender_id __attribute__((unused)),
+                             uint32_t stamp __attribute__((unused)),
+                             struct GpsState *gps_s)
+{
+  gps_stereocam.ecef_vel.x = gps_s->ecef_vel.x;
+  gps_stereocam.ecef_vel.y = gps_s->ecef_vel.y;
+  gps_stereocam.ecef_vel.z = gps_s->ecef_vel.y;
+
+}
+
+void stereo_to_state_init(void)
+{
+  //subscribe to GPS abi-messages for state measurements
+  AbiBindMsgGPS(STEREOCAM_GPS_ID, &gps_ev, stereocam_gps_cb);
+
+}
 void stereo_to_state_periodic(void)
 {
+
   if (stereocam_data.fresh) {
     stereocam_data.fresh = 0;
     float phi = stateGetNedToBodyEulers_f()->phi;
@@ -54,9 +75,6 @@ void stereo_to_state_periodic(void)
     prev_phi = phi;
   }
 }
-void stereo_to_state_start(void) {}
-void stereo_to_state_stop(void) {}
-
 
 void stereocam_to_state(float dphi, float dtheta)
 {
@@ -67,6 +85,7 @@ void stereocam_to_state(float dphi, float dtheta)
   float vel_x = 0;
   float vel_y = 0;
 
+
   // Calculate derotated velocity
 #if USE_DEROTATION_OPTICFLOW
   float agl_stereo = (float)(stereocam_data.data[4]) / 10;
@@ -95,9 +114,9 @@ void stereocam_to_state(float dphi, float dtheta)
   coordinates_speed_state.z = stateGetSpeedNed_f()->z;
 
   struct NedCoor_f opti_state;
-  opti_state.x = (float)(gps.ecef_vel.y) / 100;
-  opti_state.y = (float)(gps.ecef_vel.x) / 100;
-  opti_state.z = -(float)(gps.ecef_vel.z) / 100;
+  opti_state.x = (float)(gps_stereocam.ecef_vel.y) / 100;
+  opti_state.y = (float)(gps_stereocam.ecef_vel.x) / 100;
+  opti_state.z = -(float)(gps_stereocam.ecef_vel.z) / 100;
 
   struct FloatVect3 velocity_rot_state;
   struct FloatVect3 velocity_rot_gps;
@@ -113,14 +132,19 @@ void stereocam_to_state(float dphi, float dtheta)
   float vel_y_error = vel_y_opti - vel_y;
 
 //TODO:: Check out why vel_x_opti is 10 x big as stereocamera's output
-  stereocam_data.data[8] = (uint8_t)((vel_x * 10) + 127);
-  stereocam_data.data[9] = (uint8_t)((vel_y * 10) + 127);
+  stereocam_data.data[8] = (uint8_t)((vel_x * 10) + 127); // dm/s
+  stereocam_data.data[9] = (uint8_t)((vel_y * 10) + 127); // dm/s
   stereocam_data.data[19] = (uint8_t)((vel_x_opti) * 10 + 127); // dm/s
   stereocam_data.data[20] = (uint8_t)((vel_y_opti) * 10 + 127); // dm/s
-  stereocam_data.data[21] = (uint8_t)((vel_x_error) * 10 + 127); //dm/s
-  stereocam_data.data[22] = (uint8_t)((vel_y_error) * 10 + 127); //dm/s
-  stereocam_data.data[23] = (uint8_t)((velocity_rot_state.x) * 10 + 127); //dm/s
-  stereocam_data.data[24] = (uint8_t)((velocity_rot_state.y) * 10 + 127); //dm/s
+  stereocam_data.data[21] = (uint8_t)((vel_x_error) * 10 + 127); // dm/s
+  stereocam_data.data[22] = (uint8_t)((vel_y_error) * 10 + 127); // dm/s
+  stereocam_data.data[23] = (uint8_t)((velocity_rot_state.x) * 10 + 127); // dm/s
+  stereocam_data.data[24] = (uint8_t)((velocity_rot_state.y) * 10 + 127); // dm/s
+
+  //Send measurement values in same structure as stereocam message (make sure SEND_STEREO in stereocam.c is FALSE)
+  uint8_t frequency = 0;
+  DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &frequency, &(stereocam_data.len), stereocam_data.len,
+                           stereocam_data.data);
 #endif
 
   //Send velocity estimate to state

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

@@ -13,10 +13,23 @@
 #ifndef STEREOCAM2STATE_H
 #define STEREOCAM2STATE_H
 
+#include "state.h"
+
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_orientation_conversion.h"
+#include <std.h>
+#include "modules/stereocam/stereocam.h"
+
+struct GpsStereoCam {
+  struct EcefCoor_i ecef_vel;
+};
+
+extern struct GpsStereoCam gps_stereocam;
+
 extern void stereo_to_state_init(void);
 extern void stereo_to_state_periodic(void);
-extern void stereo_to_state_start(void);
-extern void stereo_to_state_stop(void);
+
 
 #endif