From 2c55ce368680350bb6c0e274850fd04113373cea Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 9 Apr 2021 09:12:55 +0200
Subject: [PATCH] Files for PDF exercise (#67) (#2687)

* files for PDF exercise

* Explicit call to python2 for systems that default to python3

* Description and comments fixes

[flight_plan] remove warning about dummy WP

Reformatted indents to 2 spaces

Added configs for ps4 gamepads, fixed typos (#68)

Update: for flight testing in the CyberZoo

Fix color gain in mav_course_exercise.xml airframe (#72)

[flight_plan] Re-measured the autoland border. It is not safe to exit the red sqaure so it auto-lands to prevent damage. (#73)

[Camera] Vision runs faster is unnecessary frames are not grabbed. (#74)

Co-authored-by: Matteo Barbera <matteo.barbera97@gmail.com>
---
 .../tudelft/bebop_course_orangeavoid.xml      |   1 +
 .../bebop_course_orangeavoid_guided.xml       |   1 +
 .../tudelft/bebop_mav_course_exercise.xml     | 272 ++++++++++++++++++
 .../tudelft/course_orangeavoid_cyberzoo.xml   |   8 +-
 .../course_orangeavoid_cyberzoo_guided.xml    |  10 +-
 conf/joystick/ps4_gamepad.xml                 | 104 +++++++
 conf/joystick/ps4_gamepad_yawontriggers.xml   | 104 +++++++
 conf/joystick/sm600.xml                       |   2 +-
 conf/modules/mav_course_exercise.xml          |  11 +
 .../intermediate/create_joystick.rst          |  14 +-
 .../mav_course_exercise/mav_exercise.c        | 176 ++++++++++++
 .../mav_course_exercise/mav_exercise.h        |  27 ++
 12 files changed, 713 insertions(+), 17 deletions(-)
 create mode 100644 conf/airframes/tudelft/bebop_mav_course_exercise.xml
 create mode 100644 conf/joystick/ps4_gamepad.xml
 create mode 100644 conf/joystick/ps4_gamepad_yawontriggers.xml
 create mode 100644 conf/modules/mav_course_exercise.xml
 create mode 100644 sw/airborne/modules/mav_course_exercise/mav_exercise.c
 create mode 100644 sw/airborne/modules/mav_course_exercise/mav_exercise.h

diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid.xml b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
index a50fe3e084..18b3658bad 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
@@ -9,6 +9,7 @@
     <target name="ap" board="bebop">
       <define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
       <define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
+      <define name="MT9V117_TARGET_FPS" value="20"/>
       
       <!-- Detect orange -->
       <define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="30"/>
diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
index b7ccb7b032..9c29079b2c 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
@@ -8,6 +8,7 @@
     <target name="ap" board="bebop">
       <define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
       <define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
+      <define name="MT9V117_TARGET_FPS" value="20"/>
       
       <!-- Detect orange -->
       <define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="30"/>
diff --git a/conf/airframes/tudelft/bebop_mav_course_exercise.xml b/conf/airframes/tudelft/bebop_mav_course_exercise.xml
new file mode 100644
index 0000000000..253b5f5198
--- /dev/null
+++ b/conf/airframes/tudelft/bebop_mav_course_exercise.xml
@@ -0,0 +1,272 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<airframe name="bebop_avoider">
+  <description>MAVLAB Course TUDelft
+  </description>
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="bebop">
+      <define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
+      <define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
+      <define name="MT9V117_TARGET_FPS" value="20"/>
+      
+      <!-- Detect orange -->
+      <define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="40"/>
+      <define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="145"/>
+      <define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="65"/>
+      <define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="140"/>
+      <define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="160"/>
+      <define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="225"/>
+    </target>
+    <target name="nps" board="pc">
+      <module name="fdm" type="gazebo"/>
+      <define name="VIDEO_CAPTURE_PATH" value="/tmp/paparazzi/images"/>
+      <define name="FILE_LOGGER_PATH" value="/tmp/paparazzi/log"/>
+      
+      <!-- Detect orange -->
+      <define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="41"/>
+      <define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="183"/>
+      <define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="53"/>
+      <define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="121"/>
+      <define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="134"/>
+      <define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="249"/>
+    </target>
+    
+    <define name="ARRIVED_AT_WAYPOINT" value="0.5"/><!-- Detect arrival at waypoint when within 0.5m -->
+
+  <!-- Subsystem section -->
+    <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="stabilization" type="rate_indi"/>
+    <module name="ahrs" type="int_cmpl_quat">
+      <configure name="USE_MAGNETOMETER" value="FALSE"/>
+      <define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
+    </module>
+    <module name="ins" type="extended"/>
+
+    <module name="logger_file"/>
+
+    <!-- Video/Camera modules -->
+    <module name="bebop_cam"/>
+    <!--module name="bebop_ae_awb">
+      <define name="CV_AE_AWB_AV" value="1" />
+      <define name="CV_AE_AWB_VERBOSE" value="0" />
+    </module-->
+    <module name="video_capture">
+      <define name="VIDEO_CAPTURE_CAMERA" value="front_camera"/>
+      <define name="VIDEO_CAPTURE_FPS" value="10"/>
+    </module>
+    <module name="cv_detect_color_object">
+      <define name="COLOR_OBJECT_DETECTOR_CAMERA1" value="front_camera"/>
+      <define name="COLOR_OBJECT_DETECTOR_FPS1" value="0"/>
+      <define name="COLOR_OBJECT_DETECTOR_DRAW1" value="1"/>
+    </module>
+    <module name="video_rtp_stream">
+      <define name="VIEWVIDEO_CAMERA" value="front_camera"/>
+      <define name="VIEWVIDEO_CAMERA2" value="bottom_camera"/>
+      <define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
+      <define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
+    </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="3000" neutral="3000" max="9800" />
+    <servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="9800" />
+    <servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="9800" />
+    <servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="9800" />
+  </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="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="BEBOP_FRONT_CAMERA" prefix="MT9F002_">
+    <define name="OUTPUT_HEIGHT" value="520" />
+    <define name="OUTPUT_WIDTH" value="240" />
+    <define name="OFFSET_X" value="0.09" />
+    <define name="OFFSET_Y" value="0.00" />
+    <define name="ZOOM" value="1.25"/>
+    <define name="GAIN_GREEN1" value="10.0"/>
+    <define name="GAIN_GREEN2" value="10.0"/>
+    <define name="GAIN_BLUE" value="12.5"/>
+    <define name="GAIN_RED" value="9.0"/>
+    <define name="TARGET_EXPOSURE" value="20" />
+    <define name="OUTPUT_SCALER" value="0.25"/>
+    <define name="TARGET_FPS" value="30" />
+  </section>
+
+  <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="TRUE"/>
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+    <define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
+    <!-- Magneto calibration -->
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
+  </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 -->
+    <!-- Delft -->
+    <define name="H_X" value="0.3892503"/>
+    <define name="H_Y" value="0.0017972"/>
+    <define name="H_Z" value="0.9211303"/>
+
+    <define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
+    <define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="SONAR_MAX_RANGE" value="2.2"/>
+    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
+
+    <!-- Use GPS altitude measurments and set the R gain -->
+    <define name="USE_GPS_ALT" value="1"/>
+    <define name="VFF_R_GPS" value="0.01"/>
+  </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="120" 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="200" unit="deg/s"/>
+    <define name="REF_ZETA_R" value="0.9"/>
+    <define name="REF_MAX_R" value="300." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
+  </section>
+
+  <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
+    <!-- control effectiveness -->
+    <define name="G1_P" value="0.0397"/>
+    <define name="G1_Q" value="0.0299"/>
+    <define name="G1_R" value="0.0014"/>
+    <define name="G2_R" value="0.1219"/>
+
+    <!-- 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"/>
+
+    <!--Maxium yaw rate, to avoid instability-->
+    <define name="MAX_R" value="120.0" unit="deg/s"/>
+
+    <!-- second order filter parameters -->
+    <define name="FILT_CUTOFF" value="8.0"/>
+    <define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>
+
+    <!-- first order actuator dynamics -->
+    <define name="ACT_DYN_P" value="0.1"/>
+    <define name="ACT_DYN_Q" value="0.1"/>
+    <define name="ACT_DYN_R" value="0.1"/>
+
+    <!-- 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="283"/>
+    <define name="HOVER_KD" value="82"/>
+    <define name="HOVER_KI" value="20"/>
+    <define name="NOMINAL_HOVER_THROTTLE" value="0.68"/>
+    <define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
+
+    <define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.68" />
+    <define name="ADAPT_MIN_HOVER_THROTTLE" value="0.55" />
+    <define name="ADAPT_MAX_HOVER_THROTTLE" value="0.72" />
+    <!-- <define name="ADAPT_NOISE_FACTOR" value="0.8" /> -->
+  </section>
+
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <!-- Good weather -->
+    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="REF_MAX_SPEED" value="0.5" unit="m/s"/>
+    <!-- Bad weather -->
+    <!-- define name="MAX_BANK" value="32" unit="deg"/ -->
+    <define name="PGAIN" value="120"/>
+    <define name="DGAIN" value="230"/>
+    <define name="IGAIN" value="40"/>
+  </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="bebop" type="string"/>
+    <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
+    <define name="MT9F002_SENSOR_RES_DIVIDER" value="4"/>
+  </section>
+  <include href="conf/simulator/gazebo/airframes/bebop.xml"/>
+
+  <section name="AUTOPILOT">
+    <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
+    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+    <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
+    <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
+
+    <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="10.8" 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/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml
index 78e98087da..2ea24e6b2e 100644
--- a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml
+++ b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
 
-<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="8" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
+<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
   <header>
     #include "subsystems/datalink/datalink.h"
     #include "subsystems/electrical.h"
@@ -16,12 +16,12 @@
     <waypoint name="GOAL" x="1.9" y="1.0"/>
     <waypoint name="TRAJECTORY" x="1.9" y="1.0"/>
     <waypoint lat="51.9905834" lon="4.3767710" name="_CZ1"/>
-    <waypoint lat="51.9906465" lon="4.3767025" name="_CZ2"/>
-    <waypoint lat="51.9906882" lon="4.376805" name="_CZ3"/>
+    <waypoint lat="51.9906440" lon="4.3767060" name="_CZ2"/>
+    <waypoint lat="51.9906860" lon="4.3768080" name="_CZ3"/>
     <waypoint lat="51.9906238" lon="4.3768729" name="_CZ4"/>
 
     <waypoint lat="51.9905860" lon="4.3767712" name="_OZ1"/>
-    <waypoint lat="51.9906456" lon="4.3767088" name="_OZ2"/>
+    <waypoint lat="51.9906430" lon="4.3767110" name="_OZ2"/>
     <waypoint lat="51.9906830" lon="4.3768048" name="_OZ3"/>
     <waypoint lat="51.9906239" lon="4.3768684" name="_OZ4"/>
   </waypoints>
diff --git a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml
index bdf71c8144..0909b8212a 100644
--- a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml
+++ b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
 
-<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="8" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
+<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
   <header>
     #include "subsystems/datalink/datalink.h"
     #include "subsystems/electrical.h"
@@ -24,10 +24,10 @@ inline void setGuided(void){
     <waypoint name="TD" x="0.8" y="-1.7"/>
     <waypoint name="GOAL" x="1.9" y="1.0"/>
     <waypoint name="TRAJECTORY" x="1.9" y="1.0"/>
-    <waypoint lat="51.9905874" lon="4.3767766" name="_CZ1"/>
-    <waypoint lat="51.990644" lon="4.376721" name="_CZ2"/>
-    <waypoint lat="51.990676" lon="4.376805" name="_CZ3"/>
-    <waypoint lat="51.9906213" lon="4.3768628" name="_CZ4"/>
+    <waypoint lat="51.9905834" lon="4.3767710" name="_CZ1"/>
+    <waypoint lat="51.9906440" lon="4.3767060" name="_CZ2"/>
+    <waypoint lat="51.9906860" lon="4.3768080" name="_CZ3"/>
+    <waypoint lat="51.9906238" lon="4.3768729" name="_CZ4"/>
     <waypoint lat="51.990595" lon="4.376779" name="_OZ1"/>
     <waypoint lat="51.990640" lon="4.376734" name="_OZ2"/>
     <waypoint lat="51.990667" lon="4.376804" name="_OZ3"/>
diff --git a/conf/joystick/ps4_gamepad.xml b/conf/joystick/ps4_gamepad.xml
new file mode 100644
index 0000000000..2600a47f0e
--- /dev/null
+++ b/conf/joystick/ps4_gamepad.xml
@@ -0,0 +1,104 @@
+<!-- PlayStation 4 gamepad
+
+ It has 6 axes:
+  axis 0: LeftStickHorizontal
+  axis 1: LeftStickVertical
+  axis 2: L2 (left trigger). Also triggers button 6
+  axis 3: RightStickHorizontal
+  axis 4: RightStickVertical
+  axis 5: R2 (right trigger). Also triggers button 7
+
+ It has 13 buttons.
+  b0  - X (blue)
+  b1  - Circle (red)
+  b2  - Triangle (green)
+  b3  - Square (pink)
+  b4  - L1 (left shoulder button)
+  b5  - R1 (right shoulder button)
+  b6  - L2 (left trigger). Also triggers axis 2
+  b7  - R2 (right trigger). Also triggers axis 5
+  b8  - Share (left of touchscreen)
+  b9  - Options (right of touchscreen)
+  b10 - PS (below touchscreen)
+  b11 - L3 (press left stick)
+  b12 - R3 (press right stick)
+
+ and the DPad as a hat
+ You can use the Hat<Position>(<hat_name>) function to trigger events,
+ where <Position> is one of
+ Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
+ so e.g. HatDown(dpad)
+
+-->
+
+<joystick>
+  <input>
+    <axis index="0" name="LeftStickHorizontal"/>
+    <axis index="1" name="LeftStickVertical"/>
+    <axis index="2" name="L2" trim="127"/>
+    <axis index="3" name="RightStickHorizontal"/>
+    <axis index="4" name="RightStickVertical"/>
+    <axis index="5" name="R2" trim="127"/>
+    <button index="0" name="X"/>
+    <button index="1" name="Circle"/>
+    <button index="2" name="Triangle"/>
+    <button index="3" name="Square"/>
+    <button index="4" name="L1"/>
+    <button index="5" name="R1"/>
+    <button index="6" name="L2"/>
+    <button index="7" name="R2"/>
+    <button index="8" name="Share"/>
+    <button index="9" name="Options"/>
+    <button index="10" name="PS"/>
+    <button index="11" name="L3"/>
+    <button index="12" name="R3"/>
+    <hat index="0" name="dpad"/>
+  </input>
+
+  <variables>
+    <!-- manual by default and when pressing X, AUTO1 on Square, AUTO2 on Triangle -->
+    <var name="mode" default="0"/>
+    <set var="mode" value="0" on_event="X"/>
+    <set var="mode" value="1" on_event="Square"/>
+    <set var="mode" value="2" on_event="Triangle"/>
+  </variables>
+
+  <messages period="0.05">
+
+    <message class="datalink" name="RC_4CH" send_always="true">
+      <field name="mode" value="mode"/>
+      <field name="throttle" value="Fit(-LeftStickVertical,-127,127,0,127)"/>
+      <field name="roll" value="RightStickHorizontal"/>
+      <field name="pitch" value="RightStickVertical"/>
+      <field name="yaw" value="LeftStickHorizontal"/>
+    </message>
+
+    <!-- go to "Start Engine" block if in AUTO2 and pressing Options button -->
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && Options">
+      <field name="block_id" value="IndexOfBlock('Start Engine')"/>
+    </message>
+
+    <!-- go to "Takeoff" block if in AUTO2 and pressing up on dpad -->
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatUp(dpad)">
+      <field name="block_id" value="IndexOfBlock('Takeoff')"/>
+    </message>
+
+    <!-- go to "land here" block if in AUTO2 and pressing down on dpad -->
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatDown(dpad)">
+      <field name="block_id" value="IndexOfBlock('land here')"/>
+    </message>
+
+    <!-- resurrect throttle if throttle is 0 and R1 button is pressed -->
+    <message class="ground" name="DL_SETTING" on_event="(LeftStickVertical > 120) && R1">
+      <field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
+      <field name="value" value="0"/>
+    </message>
+
+    <!-- kill throttle on L1 button -->
+    <message class="ground" name="DL_SETTING" on_event="L1">
+      <field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
+      <field name="value" value="1"/>
+    </message>
+
+  </messages>
+</joystick>
diff --git a/conf/joystick/ps4_gamepad_yawontriggers.xml b/conf/joystick/ps4_gamepad_yawontriggers.xml
new file mode 100644
index 0000000000..9c1a231144
--- /dev/null
+++ b/conf/joystick/ps4_gamepad_yawontriggers.xml
@@ -0,0 +1,104 @@
+<!-- PlayStation 4 gamepad
+
+ It has 6 axes:
+  axis 0: LeftStickHorizontal
+  axis 1: LeftStickVertical
+  axis 2: L2 (left trigger). Also triggers button 6
+  axis 3: RightStickHorizontal
+  axis 4: RightStickVertical
+  axis 5: R2 (right trigger). Also triggers button 7
+
+ It has 13 buttons.
+  b0  - X (blue)
+  b1  - Circle (red)
+  b2  - Triangle (green)
+  b3  - Square (pink)
+  b4  - L1 (left shoulder button)
+  b5  - R1 (right shoulder button)
+  b6  - L2 (left trigger). Also triggers axis 2
+  b7  - R2 (right trigger). Also triggers axis 5
+  b8  - Share (left of touchscreen)
+  b9  - Options (right of touchscreen)
+  b10 - PS (below touchscreen)
+  b11 - L3 (press left stick)
+  b12 - R3 (press right stick)
+
+ and the DPad as a hat
+ You can use the Hat<Position>(<hat_name>) function to trigger events,
+ where <Position> is one of
+ Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
+ so e.g. HatDown(dpad)
+
+-->
+
+<joystick>
+  <input>
+    <axis index="0" name="LeftStickHorizontal"/>
+    <axis index="1" name="LeftStickVertical"/>
+    <axis index="2" name="L2" trim="127"/>
+    <axis index="3" name="RightStickHorizontal"/>
+    <axis index="4" name="RightStickVertical"/>
+    <axis index="5" name="R2" trim="127"/>
+    <button index="0" name="X"/>
+    <button index="1" name="Circle"/>
+    <button index="2" name="Triangle"/>
+    <button index="3" name="Square"/>
+    <button index="4" name="L1"/>
+    <button index="5" name="R1"/>
+    <button index="6" name="L2"/>
+    <button index="7" name="R2"/>
+    <button index="8" name="Share"/>
+    <button index="9" name="Options"/>
+    <button index="10" name="PS"/>
+    <button index="11" name="L3"/>
+    <button index="12" name="R3"/>
+    <hat index="0" name="dpad"/>
+  </input>
+
+  <variables>
+    <!-- manual by default and when pressing X, AUTO1 on Square, AUTO2 on Triangle -->
+    <var name="mode" default="0"/>
+    <set var="mode" value="0" on_event="X"/>
+    <set var="mode" value="1" on_event="Square"/>
+    <set var="mode" value="2" on_event="Triangle"/>
+  </variables>
+
+  <messages period="0.05">
+
+    <message class="datalink" name="RC_4CH" send_always="true">
+      <field name="mode" value="mode"/>
+      <field name="throttle" value="Fit(-LeftStickVertical,-127,127,0,127)"/>
+      <field name="roll" value="RightStickHorizontal"/>
+      <field name="pitch" value="RightStickVertical"/>
+      <field name="yaw" value="Fit(R2-L2,-127,127,-127,127)"/>
+    </message>
+
+    <!-- go to "Start Engine" block if in AUTO2 and pressing Options button -->
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && Options">
+      <field name="block_id" value="IndexOfBlock('Start Engine')"/>
+    </message>
+
+    <!-- go to "Takeoff" block if in AUTO2 and pressing up on dpad -->
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatUp(dpad)">
+      <field name="block_id" value="IndexOfBlock('Takeoff')"/>
+    </message>
+
+    <!-- go to "land here" block if in AUTO2 and pressing down on dpad -->
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatDown(dpad)">
+      <field name="block_id" value="IndexOfBlock('land here')"/>
+    </message>
+
+    <!-- resurrect throttle if throttle is 0 and R1 button is pressed -->
+    <message class="ground" name="DL_SETTING" on_event="(LeftStickVertical > 120) && R1">
+      <field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
+      <field name="value" value="0"/>
+    </message>
+
+    <!-- kill throttle on L1 button -->
+    <message class="ground" name="DL_SETTING" on_event="L1">
+      <field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
+      <field name="value" value="1"/>
+    </message>
+
+  </messages>
+</joystick>
diff --git a/conf/joystick/sm600.xml b/conf/joystick/sm600.xml
index 2861171f5b..d60d148bee 100644
--- a/conf/joystick/sm600.xml
+++ b/conf/joystick/sm600.xml
@@ -36,7 +36,7 @@ The basis of steering is the standard signs of aerospace convention
 
   <messages period="0.05">
     <message class="datalink" name="RC_4CH" send_always="true">
-      <field name="throttle" value="Fit(LeftStickVertical,-90,127,0,127)"/>
+      <field name="throttle" value="Fit(LeftStickVertical,0,127,0,127)"/>
       <field name="roll" value="RightStickHorizontal"/>
       <field name="pitch" value="-RightStickVertical"/>
       <field name="yaw" value="LeftStickHorizontal"/>
diff --git a/conf/modules/mav_course_exercise.xml b/conf/modules/mav_course_exercise.xml
new file mode 100644
index 0000000000..ff90cc19fd
--- /dev/null
+++ b/conf/modules/mav_course_exercise.xml
@@ -0,0 +1,11 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="mav_course_exercise">
+  <doc>
+    <description>
+    Exercise module for Autonomous Flight of MAV Course
+    </description>
+  </doc>
+  <depends>cv_detect_color_object</depends>
+  <!-- TODO Specify header, init, periodic, makefile sections  -->
+</module>
diff --git a/doc/sphinx/source/tutorials/intermediate/create_joystick.rst b/doc/sphinx/source/tutorials/intermediate/create_joystick.rst
index f521131cc2..1f7d588445 100644
--- a/doc/sphinx/source/tutorials/intermediate/create_joystick.rst
+++ b/doc/sphinx/source/tutorials/intermediate/create_joystick.rst
@@ -9,7 +9,7 @@ New joystick configurations can be added in the ``paparazzi/conf/joystick`` dire
 Profile a joystick
 ==================
 
-Test if your joystick is recognized: plug your joystick then run ``dmesg``. The message is different for every device, but the last lines should look like these::
+Test if your joystick is recognized: plug your joystick then run ``dmesg``. The message is different for every device, but the last lines should look like these:
 
     [49174.642275] usb 1-1: new low-speed USB device number 8 using xhci_hcd
     [49174.812307] usb 1-1: New USB device found, idVendor=046d, idProduct=c214, bcdDevice= 2.05
@@ -20,7 +20,7 @@ Test if your joystick is recognized: plug your joystick then run ``dmesg``. The
     [49174.823608] hid-generic 0003:046D:C214.000B: input,hidraw4: USB HID v1.10 Joystick [Logitech Logitech Attack 3] on usb-0000:00:14.0-1/input0
 
 
-Launch ``./sw/ground_segment/joystick/test_stick``. It will display joystick informations, then print current status::
+Launch ``./sw/ground_segment/joystick/test_stick``. It will display joystick information, then print current status:
 
     Available button: 5 (0x5)
     Available hats: 0 (0x0)
@@ -94,7 +94,7 @@ Create a new file for your joystick in the ``paparazzi/conf/joystick`` directory
 Inputs
 ------
 
-Edit the *input* section according to your info given by *test_stick*. There are 3 kind of inputs :
+Edit the *input* section according to your info given by *test_stick*. There are 3 kind of inputs:
 
 - *axis*: "analog" stick that range from a min to a max value,
 - *hat*: tiny stick or arrows that can have 8 directions (up, down, left, right, up-left, ...),
@@ -102,7 +102,7 @@ Edit the *input* section according to your info given by *test_stick*. There are
 
 *name* and *index* attributes are mandatory for all.
 
-Axis has 4 optionnal attributes:
+Axis has 4 optional attributes:
 
 - *deadband*: input values within the deadband output 0. Range in [0, 127].
 - *exponent*: gives precise control around center values, and greater speed at high values. Range in [0, 1.0]. 0 has no effect, 1.0 has maximum effect.
@@ -124,7 +124,7 @@ The *period* attribute on the *messages* section is the period in seconds at whi
 
 In this section, you define which messages will be sent, the value of each field, and the conditions required to send the message.
 
-The *message* tag has two required attributes: the *name* and *class* of the message, and two optionnal attributes : *send_always* and *on_event*.
+The *message* tag has two required attributes: the *name* and *class* of the message, and two optional attributes : *send_always* and *on_event*.
 
 *send_always* is a boolean that default to *false*. If set to *true*, messages will keep be sent at the *period* rate. If set to *false*, message will be sent only when one of its field change value.
 
@@ -137,12 +137,12 @@ In the message node, all fields must be specified except the *ac_id* field, that
 
 *value* is a "C like" expression made of axis and variables names, operators, and a set of utily functions.
 
-Thoses functions are:
+Those functions are:
 
 - ``Scale(toto, min, max)`` : scale toto from default min/max values [-128, 127] to [*min*, *max*] 
 - ``Fit(x, min_in, max_in, min_out, max_out)`` : scale *x* from *min_in* *max_in* to *min_out*, *max_out*
 - ``Bound(x, min, max)`` : bound x between *min* and *max*
-- ``PprzMode(x)`` : scale input value to [0;1;2]. usefull for RC mode.
+- ``PprzMode(x)`` : scale input value to [0;1;2]. useful for RC mode.
 - ``JoystickID()`` : return the joystick ID.
 - ``IndexOfEnum(NAME)`` : return the index of the enum member *NAME*
 - ``IndexOfSetting('setting_name')`` : return the index of the setting *setting_name*.
diff --git a/sw/airborne/modules/mav_course_exercise/mav_exercise.c b/sw/airborne/modules/mav_course_exercise/mav_exercise.c
new file mode 100644
index 0000000000..bd5d6c223c
--- /dev/null
+++ b/sw/airborne/modules/mav_course_exercise/mav_exercise.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2021 Matteo Barbera <matteo.barbera97@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+#include "mav_exercise.h"
+#include "subsystems/abi.h"
+#include "firmwares/rotorcraft/navigation.h"
+#include "state.h"
+#include "autopilot_static.h"
+#include <stdio.h>
+
+#define NAV_C // needed to get the nav functions like Inside...
+#include "generated/flight_plan.h"
+
+#define PRINT(string, ...) fprintf(stderr, "[mav_exercise->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
+
+uint8_t increase_nav_heading(float incrementDegrees);
+uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
+uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
+
+enum navigation_state_t {
+  SAFE,
+  OBSTACLE_FOUND,
+  OUT_OF_BOUNDS,
+  HOLD
+};
+
+// define and initialise global variables
+float oa_color_count_frac = 0.18f;
+enum navigation_state_t navigation_state = SAFE;
+int32_t color_count = 0;               // orange color count from color filter for obstacle detection
+int16_t obstacle_free_confidence = 0;   // a measure of how certain we are that the way ahead is safe.
+float moveDistance = 2;                 // waypoint displacement [m]
+float oob_haeding_increment = 5.f;      // heading angle increment if out of bounds [deg]
+const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
+
+
+// needed to receive output from a separate module running on a parallel process
+#ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
+#define ORANGE_AVOIDER_VISUAL_DETECTION_ID ABI_BROADCAST
+#endif
+static abi_event color_detection_ev;
+static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
+                               int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
+                               int16_t __attribute__((unused)) pixel_width,
+                               int16_t __attribute__((unused)) pixel_height,
+                               int32_t quality, int16_t __attribute__((unused)) extra) {
+  color_count = quality;
+}
+
+void mav_exercise_init(void) {
+  // bind our colorfilter callbacks to receive the color filter outputs
+  AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
+}
+
+void mav_exercise_periodic(void) {
+  // only evaluate our state machine if we are flying
+  if (!autopilot_in_flight()) {
+    return;
+  }
+
+  // compute current color thresholds
+  // front_camera defined in airframe xml, with the video_capture module
+  int32_t color_count_threshold = oa_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
+
+  PRINT("Color_count: %d  threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
+
+  // update our safe confidence using color threshold
+  if (color_count < color_count_threshold) {
+    obstacle_free_confidence++;
+  } else {
+    obstacle_free_confidence -= 2;  // be more cautious with positive obstacle detections
+  }
+
+  // bound obstacle_free_confidence
+  Bound(obstacle_free_confidence, 0, max_trajectory_confidence);
+
+  switch (navigation_state) {
+    case SAFE:
+      moveWaypointForward(WP_TRAJECTORY, 1.5f * moveDistance);
+      if (!InsideObstacleZone(WaypointX(WP_TRAJECTORY), WaypointY(WP_TRAJECTORY))) {
+        navigation_state = OUT_OF_BOUNDS;
+      } else if (obstacle_free_confidence == 0) {
+        navigation_state = OBSTACLE_FOUND;
+      } else {
+        moveWaypointForward(WP_GOAL, moveDistance);
+      }
+      break;
+    case OBSTACLE_FOUND:
+      // TODO Change behavior
+      // stop as soon as obstacle is found
+      waypoint_move_here_2d(WP_GOAL);
+      waypoint_move_here_2d(WP_TRAJECTORY);
+
+      navigation_state = HOLD;
+      break;
+    case OUT_OF_BOUNDS:
+      // stop
+      waypoint_move_here_2d(WP_GOAL);
+      waypoint_move_here_2d(WP_TRAJECTORY);
+
+      increase_nav_heading(oob_haeding_increment);
+      moveWaypointForward(WP_TRAJECTORY, 1.5f);
+
+      if (InsideObstacleZone(WaypointX(WP_TRAJECTORY), WaypointY(WP_TRAJECTORY))) {
+        // add offset to head back into arena
+        increase_nav_heading(oob_haeding_increment);
+        navigation_state = SAFE;
+      }
+      break;
+    case HOLD:
+    default:
+      break;
+  }
+}
+
+/*
+ * Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
+ */
+uint8_t increase_nav_heading(float incrementDegrees) {
+  float new_heading = stateGetNedToBodyEulers_f()->psi + RadOfDeg(incrementDegrees);
+
+  // normalize heading to [-pi, pi]
+  FLOAT_ANGLE_NORMALIZE(new_heading);
+
+  // set heading
+  nav_heading = ANGLE_BFP_OF_REAL(new_heading);
+
+  return false;
+}
+
+/*
+ * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
+ */
+static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters) {
+  float heading = stateGetNedToBodyEulers_f()->psi;
+
+  // Now determine where to place the waypoint you want to go to
+  new_coor->x = stateGetPositionEnu_i()->x + POS_BFP_OF_REAL(sinf(heading) * (distanceMeters));
+  new_coor->y = stateGetPositionEnu_i()->y + POS_BFP_OF_REAL(cosf(heading) * (distanceMeters));
+  return false;
+}
+
+/*
+ * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
+ */
+uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor) {
+  waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
+  return false;
+}
+
+/*
+ * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
+ */
+uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters) {
+  struct EnuCoor_i new_coor;
+  calculateForwards(&new_coor, distanceMeters);
+  moveWaypoint(waypoint, &new_coor);
+  return false;
+}
diff --git a/sw/airborne/modules/mav_course_exercise/mav_exercise.h b/sw/airborne/modules/mav_course_exercise/mav_exercise.h
new file mode 100644
index 0000000000..67de1e9540
--- /dev/null
+++ b/sw/airborne/modules/mav_course_exercise/mav_exercise.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2021 Matteo Barbera <matteo.barbera97@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef PAPARAZZI_MAV_EXERCISE_H
+#define PAPARAZZI_MAV_EXERCISE_H
+
+extern void mav_exercise_init(void);
+extern void mav_exercise_periodic(void);
+
+#endif //PAPARAZZI_MAV_EXERCISE_H