use pprzlink with stereocam (#2107)

paparazzi · Aug 29, 2017 · bd53a3c · bd53a3c
1 parent 290d47f
commit bd53a3c
Show file tree

Hide file tree

Showing 27 changed files with 522 additions and 840 deletions.
diff --git a/conf/airframes/KS/kirk_conf.xml b/conf/airframes/KS/kirk_conf.xml
@@ -1,4 +1,15 @@
 <conf>
+  <aircraft
+   name="Bebop2_stereo"
+   ac_id="204"
+   airframe="airframes/KS/ks_bebop2_stereo.xml"
+   radio="radios/dummy.xml"
+   telemetry="telemetry/default_rotorcraft.xml"
+   flight_plan="flight_plans/rotorcraft_optitrack.xml"
+   settings="settings/rotorcraft_basic.xml"
+   settings_modules="modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+   gui_color="#ffff00000000"
+  />
   <aircraft
    name="MAVTec1_optitrack"
    ac_id="203"

diff --git a/conf/airframes/KS/ks_bebop2_stereo.xml b/conf/airframes/KS/ks_bebop2_stereo.xml
@@ -0,0 +1,228 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<airframe name="bebop2_opticflow">
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="bebop2"/>
+
+    <module name="telemetry" type="transparent_udp"/>
+    <module name="radio_control" type="datalink"/>
+    <module name="motor_mixing"/>
+    <module name="actuators" type="bebop"/>
+    <module name="imu" type="bebop"/>
+    <module name="gps" type="datalink"/>
+    <module name="stabilization" type="indi_simple"/>
+    <module name="ahrs" type="int_cmpl_quat">
+      <configure name="USE_MAGNETOMETER" value="FALSE"/>
+      <define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
+      <define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
+    </module>
+    <module name="ins" type="extended"/>
+
+    <module name="stereocam">
+      <configure name="STEREO_UART" value="UART2"/>
+      <define name="STEREO_BODY_TO_STEREO_PHI" value="90."/>
+      <define name="STEREO_BODY_TO_STEREO_THETA" value="0."/>
+      <define name="STEREO_BODY_TO_STEREO_PSI" value="90."/>
+    </module>
+
+  </firmware>
+
+  <commands>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="YAW" failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="6000"/>
+  </commands>
+
+  <servos driver="Default">
+    <servo name="TOP_LEFT" no="0" min="2500" neutral="2500" max="12000"/>
+    <servo name="TOP_RIGHT" no="1" min="2500" neutral="2500" max="12000"/>
+    <servo name="BOTTOM_RIGHT" no="2" min="2500" neutral="2500" max="12000"/>
+    <servo name="BOTTOM_LEFT" no="3" min="2500" neutral="2500" max="12000"/>
+  </servos>
+
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
+    <define name="REVERSE" value="TRUE"/>
+    <define name="TYPE" value="QUAD_X"/>
+  </section>
+
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
+    <set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
+    <set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
+    <set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
+    <set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
+  </command_laws>
+
+  <section name="AIR_DATA" prefix="AIR_DATA_">
+    <define name="CALC_AIRSPEED" value="FALSE"/>
+    <define name="CALC_TAS_FACTOR" value="FALSE"/>
+    <define name="CALC_AMSL_BARO" value="FALSE"/>
+  </section>
+
+  <!-- Magnetometer still needs to be calibrated -->
+  <section name="IMU" prefix="IMU_">
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
+    <define name="MAG_X_SENS" value="7.28514789391" integer="16"/>
+    <define name="MAG_Y_SENS" value="7.33022132691" integer="16"/>
+    <define name="MAG_Z_SENS" value="7.57102035692" integer="16"/>
+  </section>
+
+  <!-- local magnetic field -->
+  <!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
+  <section name="AHRS" prefix="AHRS_">
+    <!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
+    <define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
+    <define name="GRAVITY_HEURISTIC_FACTOR" value="0"/> 
+    <!-- Delft -->
+    <define name="H_X" value="0.3892503"/>
+    <define name="H_Y" value="0.0017972"/>
+    <define name="H_Z" value="0.9211303"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="USE_GPS_ALT" value="1"/>
+    <!-- trust GPS a lot: -->
+    <define name="VFF_R_GPS" value="0.01"/> 
+    <define name="VFF_VZ_R_GPS" value="0.01"/>     
+    <define name="SONAR_MIN_RANGE" value="0.0"/>
+    <define name="SONAR_MAX_RANGE" value="2.2"/>
+    <define name="SONAR_UPDATE_ON_AGL" value="FALSE"/>
+
+  </section>
+
+
+  <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
+    <!-- setpoint limits for attitude stabilization rc flight -->
+    <define name="SP_MAX_PHI" value="45" unit="deg"/>
+    <define name="SP_MAX_THETA" value="45" unit="deg"/>
+    <define name="SP_MAX_R" value="300" unit="deg/s"/>
+    <define name="DEADBAND_A" value="0"/>
+    <define name="DEADBAND_E" value="0"/>
+    <define name="DEADBAND_R" value="50"/>
+  </section>
+
+  <section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
+    <!-- attitude reference generation model -->
+    <define name="REF_OMEGA_P" value="450" unit="deg/s"/>
+    <define name="REF_ZETA_P" value="0.9"/>
+    <define name="REF_MAX_P" value="600." unit="deg/s"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
+    <define name="REF_ZETA_Q" value="0.9"/>
+    <define name="REF_MAX_Q" value="600." unit="deg/s"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R" value="450" unit="deg/s"/>
+    <define name="REF_ZETA_R" value="0.9"/>
+    <define name="REF_MAX_R" value="600." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
+  </section>
+
+  <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
+    <!-- control effectiveness, can be obtained by switching on "adaptive" in the indi settings -->
+    <define name="G1_P" value="0.100"/>
+    <define name="G1_Q" value="0.050"/>
+    <define name="G1_R" value="0.0010"/>
+    <define name="G2_R" value="0.236"/>
+
+    <!-- Here it is assumed that your removed the damping from your bebop2!
+         The dampers do not really damp, but cause oscillation. By removing/
+         fixing them, the bebop2 will fly much better-->
+    <define name="FILTER_ROLL_RATE" value="FALSE"/>
+    <define name="FILTER_PITCH_RATE" value="FALSE"/>
+    <define name="FILTER_YAW_RATE" value="FALSE"/>
+
+    <!-- reference acceleration for attitude control -->
+    <define name="REF_ERR_P" value="600.0"/>
+    <define name="REF_ERR_Q" value="600.0"/>
+    <define name="REF_ERR_R" value="600.0"/>
+    <define name="REF_RATE_P" value="28.0"/>
+    <define name="REF_RATE_Q" value="28.0"/>
+    <define name="REF_RATE_R" value="28.0"/>
+
+    <!-- second order filter parameters -->
+    <!-- old?
+	<define name="FILT_OMEGA" value="20.0"/>
+    <define name="FILT_ZETA" value="0.7"/>
+    <define name="FILT_OMEGA_R" value="20.0"/>
+    <define name="FILT_ZETA_R" value="0.7"/>
+	-->
+
+	<define name="FILT_CUTOFF" value="3.2"/>
+    <define name="FILT_CUTOFF_R" value="3.2"/>
+
+    <!-- first order actuator dynamics -->
+    <define name="ACT_DYN_P" value="0.06"/>
+    <define name="ACT_DYN_Q" value="0.06"/>
+    <define name="ACT_DYN_R" value="0.06"/>
+
+    <!-- Adaptive Learning Rate -->
+    <define name="USE_ADAPTIVE" value="FALSE"/>
+    <define name="ADAPTIVE_MU" value="0.0001"/>
+  </section>
+
+  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+    <define name="HOVER_KP" value="350"/>
+    <define name="HOVER_KD" value="85"/>
+    <define name="HOVER_KI" value="20"/>
+    <define name="NOMINAL_HOVER_THROTTLE" value="0.52"/>
+    <define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
+	<!-- old?
+    <define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.55"/> 
+    <define name="ADAPT_MIN_HOVER_THROTTLE" value="0.48"/> 		
+    <define name="ADAPT_MAX_HOVER_THROTTLE" value="0.62"/> 		
+    <define name="ADAPT_NOISE_FACTOR" value="0.8"/> 			
+	-->
+  </section>
+
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <define name="REF_MAX_SPEED" value="2" unit="m/s"/>
+    <define name="MAX_BANK" value="35" unit="deg"/>
+    <define name="PGAIN" value="82"/> <!-- GPS: 120 no GPS: 0--> 
+    <define name="DGAIN" value="68"/> <!-- GPS: 80 no GPS: 302-->
+    <define name="IGAIN" value="10"/> <!-- GPS: 30 no GPS: 0-->
+  </section>
+
+  <section name="NAVIGATION" prefix="NAV_">
+    <define name="CLIMB_VSPEED" value="1.0"/>
+    <define name="DESCEND_VSPEED" value="-0.75"/>
+  </section>
+
+  <section name="SIMULATOR" prefix="NPS_">
+    <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
+    <define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
+    <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
+  </section>
+
+  <section name="AUTOPILOT">
+    <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
+    <define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
+    <define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
+    <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
+
+    <!--
+    <define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
+    <define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
+    <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
+    <define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
+    -->
+    <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="11.0" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
+    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
+  </section>
+</airframe>
+
diff --git a/conf/airframes/TUDELFT/tudelft_ardrone2_optitrack.xml b/conf/airframes/TUDELFT/tudelft_ardrone2_optitrack.xml
@@ -29,7 +29,6 @@
 
   <modules main_freq="512">
     <module name="bat_voltage_ardrone2"/>
-    <module name="stereocam_droplet" />
   </modules>
 
   <commands>

diff --git a/conf/airframes/TUDELFT/tudelft_guido_ardrone2_optitrack.xml b/conf/airframes/TUDELFT/tudelft_guido_ardrone2_optitrack.xml
@@ -29,7 +29,6 @@
 
   <modules main_freq="512">
     <module name="bat_voltage_ardrone2"/>
-    <module name="stereocam_droplet" />
     <module name="video_thread"/>
     <module name="pose_history"/>
     <module name="cv_opticflow">

diff --git a/conf/airframes/TUDELFT/tudelft_ladybird_lisamxs_wifi_indi_stereoboard.xml b/conf/airframes/TUDELFT/tudelft_ladybird_lisamxs_wifi_indi_stereoboard.xml
@@ -231,17 +231,7 @@
      <define name= "STEREO_BODY_TO_STEREO_PSI" value="-90" unit="deg"/>
      <configure name="STEREO_UART" value="UART2"/>
      <configure name="STEREO_BAUD" value="B115200"/>
-     <define name="SEND_STEREO" value="FALSE"/>
    </module>
-   <module name="stereocam_state2camera">
-     <configure name="STEREO_UART" value="UART2"/>
-     <configure name="STEREO_BAUD" value="B115200"/>
-     <define name="STATE2CAMERA_SEND_DATA_TYPE" value="1"/>
-   </module>
-   <module name="stereocam_stereocam2state">
-     <define name="STEREOCAM2STATE_RECEIVED_DATA_TYPE" value="0"/>
-     <define name="STEREOCAM2STATE_EDGEFLOW_PIXELWISE" value="TRUE"/>
-	 </module>
  </modules>
 
   <firmware name="rotorcraft">

diff --git a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml
@@ -202,13 +202,8 @@
    <module name="stereocam">
      <configure name="STEREO_UART" value="UART1"/>
      <configure name="STEREO_BAUD" value="B115200"/>
-     <define name="SEND_STEREO" value="TRUE"/>
+     <define name="FORWARD_IMAGE_DATA" value="TRUE"/>
    </module>
-   <module name="stereocam_state2camera">
-     <configure name="STEREO_UART" value="UART1"/>
-     <configure name="STEREO_BAUD" value="B115200"/>
-   </module>
-   <module name="stereocam_stereocam2state"/>
  </modules>
 
   <firmware name="rotorcraft">

diff --git a/conf/airframes/TUDELFT/tudelft_rm_ardrone2.xml b/conf/airframes/TUDELFT/tudelft_rm_ardrone2.xml
@@ -32,9 +32,9 @@
 
   <modules main_freq="512">
     <module name="bat_voltage_ardrone2"/>
-    <module name="stereocam_state2camera">
-	<configure name="STEREO_UART" value="UART1"/>
-        <configure name="STEREO_BAUD" value="B1000000"/>
+    <module name="stereocam">
+      <configure name="STEREO_UART" value="UART1"/>
+      <configure name="STEREO_BAUD" value="B1000000"/>
     </module>
   </modules>
 

diff --git a/conf/airframes/TUDELFT/tudelft_selfie.xml b/conf/airframes/TUDELFT/tudelft_selfie.xml
@@ -28,8 +28,8 @@
   <modules main_freq="512">
     <module name="bat_voltage_ardrone2"/>
     <module name="stereocam">
-	<configure name="STEREO_UART" value="UART1"/>
-	<configure name="STEREO_BAUD" value="B115200"/>
+      <configure name="STEREO_UART" value="UART1"/>
+      <configure name="STEREO_BAUD" value="B115200"/>
     </module>
     <module name="stereocam_follow_me"/>
   </modules>

diff --git a/conf/modules/stereocam.xml b/conf/modules/stereocam.xml
@@ -7,35 +7,33 @@
       Module to read and decode messages from the TU Delft stereo camera over uart.
       The stereo camera is designed and produced by Christophe de Wagter from the TU Delft.
       Baud rates tested include B38400, B230400, B921600.
-      If entire images are being sent, it is recommended to use a STEREO_BAUD of B921600 and a STEREO_BUF_SIZE of 32768.
+      If entire images are being sent, it is recommended to use a STEREO_BAUD of B921600.
     </description>
     <configure name="STEREO_UART" value="UARTX" description="Sets the UART port number of the connected camera (required)"/>
     <configure name="STEREO_BAUD" value="BXXXXX" description="Sets the BAUD rate of the connected camera (required: must be same as camera)"/>
-    <define name="SEND_STEREO" value="TRUE|FALSE" description="If TRUE, a copy of received messages is sent to the ground station over the datalink (default: TRUE)"/>
-    <define name="STEREO_BUF_SIZE" value="NUM" description="Size of the receiving buffer, must be at least as large as the message being sent (default: 1024)"/>
+    <define name="FORWARD_IMAGE_DATA" value="TRUE|FALSE" description="If TRUE, a copy of received iamges is sent to the ground station over the datalink (default: TRUE)"/>
+    <define name="STEREO_BODY_TO_STEREO_PHI" value="90" description="Rotation matrix from body to camera frame"/>
+    <define name="STEREO_BODY_TO_STEREO_THETA" value="0"/>
+    <define name="STEREO_BODY_TO_STEREO_PSI" value="90"/>
   </doc>
-  <settings>
-    <dl_settings>
-      <dl_settings name="stereocam">
-        <dl_setting var="stereocam_derotation_on" min="0" step="1" max="1" shortname="derotation"
-                    module="stereocam/state2camera/state2camera">
-        </dl_setting>
-        <dl_setting var="stereocam_medianfilter_on" min="0" step="1" max="1" shortname="median_filter"
-                    module="stereocam/stereocam2state/stereocam2state">
-        </dl_setting>
-      </dl_settings>
-    </dl_settings>
-  </settings>
   <header>
     <file name="stereocam.h"/>
   </header>
-  <periodic fun="stereocam_periodic()" start="stereocam_start()" stop="stereocam_stop()" autorun="TRUE" freq="512"/>
+  <init fun="stereocam_init()"/>
+  <periodic fun="state2stereocam()" autorun="TRUE" freq="25"/>
+  <event fun="stereocam_event()"/>
   <makefile>
-    <file name="stereoprotocol.c" dir="modules/stereocam"/>
-    <configure name="STEREO_UART" case="upper|lower"/>
+  	<!-- Configure default UART port and baudrate -->
+    <configure name="STEREO_UART" default="UART1" case="upper|lower"/>
+    <configure name="STEREO_BAUD" default="B921600"/>
+
+    <!-- Enable UART and set baudrate -->
     <define name="USE_$(STEREO_UART_UPPER)"/>
     <define name="UART_LINK" value="$(STEREO_UART_LOWER)"/>
     <define name="$(STEREO_UART_UPPER)_BAUD" value="$(STEREO_BAUD)"/>
+
+    <!-- Sources and PPRZLink for transport -->
     <file name="stereocam.c"/>
+    <file name="pprz_transport.c" dir="pprzlink/src"/>
   </makefile>
  </module>
diff --git a/conf/modules/stereocam_follow_me.xml b/conf/modules/stereocam_follow_me.xml
@@ -10,9 +10,8 @@
   <header>
     <file name="stereocam_follow_me/follow_me.h"/>
   </header>
-  <periodic fun="follow_me_periodic()" freq="512"/>
   <makefile>
-    <file name="stereocam_follow_me/follow_me.c"  />
+    <define name="STEREOCAM_FOLLOWME"/>
+    <file name="stereocam_follow_me/follow_me.c"/>
   </makefile>
 </module>
-