diff --git a/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml
new file mode 100644
index 0000000000..3adef118be
--- /dev/null
+++ b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml
@@ -0,0 +1,330 @@
+<!-- This is a 7kg Rotating Wing Drone AG
+     * Airframe:    TUD00???
+     * Autopilot:   Cube orange
+     * Datalink:    Herelink
+     * GPS:         UBlox F9P
+     * RC:          SBUS Crossfire
+-->
+
+<airframe name="RotatingWing_AG">
+    <description>RotatingWingV3AG with optitrack and INS ext pose</description>
+
+    <firmware name="rotorcraft">
+     <autopilot name="rotorcraft_oneloop_switch.xml"/>
+        <target name="ap" board="cube_orangeplus">
+            <configure name="PERIODIC_FREQUENCY"   value="500"/> <!-- Configure the main periodic frequency to 500Hz -->
+            <configure name="NAVIGATION_FREQUENCY" value="500"/>
+            <module name="radio_control" type="sbus">
+                 <configure name="SBUS_PORT" value="UART3"/> <!-- On the TELEM2 port -->
+            </module>
+            <module name="sys_mon"/>
+            <module name="flight_recorder"/>
+
+            <module name="lidar_tfmini">
+                <configure name="TFMINI_PORT"    value="UART8"/>
+                <configure name="USE_TFMINI_AGL" value="TRUE"/>
+            </module>     
+            <!-- RC switches -->
+            <define name="RADIO_TH_HOLD"            value="RADIO_AUX1"/>
+            <define name="RADIO_KILL_SWITCH"        value="RADIO_AUX1"/>
+            <define name="RADIO_FMODE"              value="RADIO_AUX2"/>
+            <define name="AP_MODE_SWITCH"           value="RADIO_AUX3"/>
+            <define name="RADIO_CONTROL_THRUST_X"   value="RADIO_AUX4"/>
+            <!-- EKF EXT POSE  configure inputs -->
+            <define name="INS_EXT_POSE_MAG_ID"      value="MAG_RM3100_SENDER_ID"/>
+            <define name="INS_EXT_POSE_IMU_ID"      value="IMU_CUBE1_ID"/> 
+            <!--Only send gyro and accel that is being used-->
+            <define name="IMU_GYRO_ABI_SEND_ID"     value= "IMU_CUBE1_ID"/>
+            <define name="IMU_ACCEL_ABI_SEND_ID"    value= "IMU_CUBE1_ID"/>
+            <!-- Range sensor connected to supercan -->
+            <module name="range_sensor_uavcan"/>
+            <!-- Log in high speed (Remove for outdoor flights) -->
+            <!-- <define name="IMU_LOG_HIGHSPEED"    value="TRUE"/> -->
+            <define name="I2C2_CLOCK_SPEED" value="100000"/>
+        </target>
+
+        <target name="nps" board="pc">
+            <configure name="PERIODIC_FREQUENCY"   value="500"/>
+            <configure name="NAVIGATION_FREQUENCY" value="500"/>
+            <module name="radio_control" type="datalink"/>
+            <module name="fdm" type="jsbsim"/>
+
+            <!--Not dealing with these in the simulation-->
+            <define name="RADIO_TH_HOLD"          value="0"/> <!-- Throttle hold in command laws -->
+            <define name="RADIO_FMODE"            value="0"/> <!-- Throttle curve select -->
+            <define name="AP_MODE_SWITCH"         value="RADIO_AUX7"/> <!-- Switch between AP controllers -->
+            <define name="RADIO_KILL_SWITCH"      value="RADIO_AUX1"/> <!-- Kill switch -->
+            <define name="RADIO_CONTROL_THRUST_X" value="0"/>
+        </target>
+
+       <!-- Datalink -->
+        <module name="telemetry" type="transparent">
+            <!-- herelink -->
+            <configure name="MODEM_BAUD" value="B460800"/>
+            <!-- usb connection -->
+            <!-- <configure name="MODEM_PORT" value="usb_serial"/> -->
+            <!-- Xbee -->
+            <!-- <configure VALUE="B57600" name="MODEM_BAUD"/> -->
+        </module>
+
+        <!-- Sensors -->     
+         <module name="mag" type="rm3100">
+            <define     name="MODULE_RM3100_UPDATE_AHRS"    value="TRUE"/>
+            <configure  name="MAG_RM3100_I2C_DEV"           value="I2C2"/>
+        </module> 
+        <module name="airspeed" type="ms45xx_i2c">
+            <define name="MS45XX_I2C_DEV"               value="i2c2"/>
+            <define name="MS45XX_PRESSURE_SCALE"        value="1.9077609"/>
+            <define name="USE_AIRSPEED_LOWPASS_FILTER"  value="TRUE"/>
+            <define name="MS45XX_LOWPASS_TAU"           value="0.15"/>
+            <define name="AIRSPEED_MS45XX_SEND_ABI"     value="1"/>
+            </module>       
+        <module name="airspeed" type="uavcan">
+            <define name="AIRSPEED_UAVCAN_LOWPASS_FILTER"   value="TRUE" />
+            <define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD"   value="0.1" />
+            <define name="AIRSPEED_UAVCAN_LOWPASS_TAU"      value="0.15" />
+            <define name="AIRSPEED_UAVCAN_SEND_ABI"         value="0" /> <!-- Read Airspeed for logging but do not use it -->
+        </module>                
+        <module name="air_data"/>          
+
+        <module name="gps" type="datalink"/>
+        
+        <!-- IMU / INS -->  
+        <module name="imu" type="cube"/>  
+        <module name="ins" type="ext_pose"/>
+ 
+        <!-- Actuators on dual CAN bus -->
+        <module name="actuators" type="uavcan">
+            <configure value="TRUE" name="UAVCAN_USE_CAN1"/>
+            <configure value="TRUE" name="UAVCAN_USE_CAN2"/>
+        </module>
+ 
+        <!-- Actuators on PWM -->       
+        <module name="actuators" type="pwm" >
+            <define name="SERVO_HZ" value="400"/>
+        </module>
+
+        <!-- Control -->      
+        <module name="stabilization" type="oneloop" >
+            <configure name="INDI_NUM_ACT" value="9"/>
+        </module>
+        
+        <module name="guidance" type="oneloop"/>
+        <module name="nav" type="hybrid">
+            <define name="NAV_HYBRID_MAX_AIRSPEED"      value="19.0f"/>
+            <define name="NAV_HYBRID_SPEED_MARGIN"      value="0.0f"/>
+            <define name="NAV_HYBRID_MAX_ACCELERATION"  value="4.0f"/>
+            <define name="NAV_HYBRID_MAX_DECELERATION"  value="2.0f"/>
+            <define name="GUIDANCE_H_USE_REF"           value="FALSE"/>
+            <define name="NAV_OPTITRACK"                value="TRUE"/>
+        </module>
+
+        <module name="oneloop" type="andi">
+            <configure name="ANDI_OUTPUTS" value="6"/>
+        </module>
+
+        <module name="eff_scheduling_rotwing_V2"/>
+
+        <module name="wls">
+            <define name="WLS_N_U_MAX" value = "11"/>
+            <define name="WLS_N_V_MAX" value = "6"/>
+        </module>
+
+        <module name="ground_detect"/>
+        <module name="rotwing_state_V2">
+            <define name="RW_USE_MODULES_V2" value="TRUE"/>
+        </module>
+        <module name="agl_dist"/>
+    </firmware>
+
+    <!-- PWM actuators -->   
+    <!-- <servos driver="Pwm">
+        <servo no="0" name="ROTATION_MECH"   min="1240" neutral="1468" max="1696"/>
+    </servos> -->
+
+    <!-- Can bus 1 actuators -->
+    <servos driver="Uavcan1">
+        <servo no="0" name="MOTOR_FRONT"    min="0"     neutral="600"   max="7372"/>
+        <servo no="1" name="MOTOR_RIGHT"    min="0"     neutral="600"   max="7372"/>
+        <servo no="2" name="MOTOR_BACK"     min="0"     neutral="600"   max="7372"/>
+        <servo no="3" name="MOTOR_LEFT"     min="0"     neutral="600"   max="7372"/>
+        <servo no="4" name="MOTOR_PUSH"     min="0"     neutral="200"   max="7372"/>
+        <servo no="5"  name="ROTATION_MECH" min="-2048" neutral="-205"  max="1638"/>
+    </servos>
+
+    <!-- CAN BUS 1 command outputs-->
+    <servos driver="Uavcan1Cmd">
+        <servo no="6" name="SERVO_ELEVATOR" min="3767"  neutral="3767" max="-5668"/>
+        <servo no="7" name="SERVO_RUDDER"   min="-5225" neutral="0"    max="5225"/>
+    </servos>
+    
+    <!-- Can bus 2 actuators -->
+    <servos driver="Uavcan2">
+        <servo no="0" name="BMOTOR_FRONT"   min="0"     neutral="600"   max="7372"/>
+        <servo no="1" name="BMOTOR_RIGHT"   min="0"     neutral="600"   max="7372"/>
+        <servo no="2" name="BMOTOR_BACK"    min="0"     neutral="600"   max="7372"/>
+        <servo no="3" name="BMOTOR_LEFT"    min="0"     neutral="600"   max="7372"/>
+        <servo no="5" name="BROTATION_MECH" min="-2048" neutral="-205"  max="1638"/>
+    </servos>
+
+    <!-- CAN BUS 2 command outputs-->
+    <servos driver="Uavcan2Cmd">
+        <servo no="8"  name="AIL_LEFT"   min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
+        <servo no="9"  name="FLAP_LEFT"  min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
+        <servo no="10" name="FLAP_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
+        <servo no="11" name="AIL_RIGHT"  min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
+    </servos>
+
+    <commands>
+        <!-- Real actuators commands -->
+        <axis name="MOTOR_FRONT"  group ="REAL" failsafe_value="0"/> <!-- IDX 0--> 
+        <axis name="MOTOR_RIGHT"  group ="REAL" failsafe_value="0"/> <!-- IDX 1-->
+        <axis name="MOTOR_BACK"   group ="REAL" failsafe_value="0"/> <!-- IDX 2-->
+        <axis name="MOTOR_LEFT"   group ="REAL" failsafe_value="0"/> <!-- IDX 3-->
+        <axis name="MOTOR_PUSHER" group ="REAL" failsafe_value="0"/> <!-- IDX 4-->
+        <axis name="ELEVATOR"     group ="REAL" failsafe_value="0"/> <!-- IDX 5-->
+        <axis name="RUDDER"       group ="REAL" failsafe_value="0"/> <!-- IDX 6-->
+        <axis name="AILERONS"     group ="REAL" failsafe_value="0"/> <!-- IDX 7-->
+        <axis name="FLAPS"        group ="REAL" failsafe_value="0"/> <!-- IDX 8-->
+        <!-- Virtual actuators commands -->
+        <axis name="ROLL"         group ="VIRTUAL" failsafe_value="0"/> <!-- ID X 9-->
+        <axis name="PITCH"        group ="VIRTUAL" failsafe_value="0"/> <!-- IDX 10-->
+        <!-- Passive actuators commands -->
+        <axis name="ROT_MECH"     group ="PASSIVE" failsafe_value="0"/> <!-- IDX 11-->
+        <!-- Legacy commands-->
+        <axis name="YAW"          group ="OTHER" failsafe_value="0"/>
+        <axis name="THRUST"       group ="OTHER" failsafe_value="0"/>
+    </commands>
+
+    <command_laws>
+        <let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
+        <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])"                                                                   SERVO="MOTOR_FRONT"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])"                                                                   SERVO="MOTOR_RIGHT"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )"                                                                   SERVO="MOTOR_BACK"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )"                                                                   SERVO="MOTOR_LEFT"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_PUSHER])"                                                                  SERVO="MOTOR_PUSH"/> 
+        <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : commands[COMMAND_ELEVATOR]))"  SERVO="SERVO_ELEVATOR"/>  
+        <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : commands[COMMAND_RUDDER])"                                             SERVO="SERVO_RUDDER"/>
+        <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])"                                          SERVO="AIL_LEFT"/>
+        <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])"                                          SERVO="AIL_RIGHT"/>
+        <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])"                                             SERVO="FLAP_LEFT"/>
+        <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])"                                             SERVO="FLAP_RIGHT"/>
+        <set VALUE="commands[COMMAND_ROT_MECH]"                                                                                          SERVO="ROTATION_MECH"/>
+
+        <!-- Backup commands -->
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])"  SERVO="BMOTOR_FRONT"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])"  SERVO="BMOTOR_RIGHT"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )"  SERVO="BMOTOR_BACK"/>
+        <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )"  SERVO="BMOTOR_LEFT"/>
+        <set VALUE="commands[COMMAND_ROT_MECH]"                         SERVO="BROTATION_MECH"/>
+    </command_laws>
+
+    <section NAME="MISC">
+        <!-- Voltage and current measurements -->
+        <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
+        <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
+        <!-- Others -->
+        <define name="NO_RC_THRUST_LIMIT"                   value="TRUE"/>
+        <define name="NAV_CLIMB_VSPEED"                     value="2.0" />
+        <define name="NAV_DESCEND_VSPEED"                   value="-1.0"/>
+        <define name="NAV_CARROT_DIST"                      value="15"/>
+        <define name="NAV_LINE_DIST"                        value="100"/>
+        <define name="CLOSE_TO_WAYPOINT"                    value="15"/>
+        <define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST"       value="300"/>
+        <define name="ARRIVED_AT_WAYPOINT"                  value="50.0"/> 
+        <define name="NO_GPS_LOST_WITH_DATALINK_TIME"       value="20"/>
+        <define name="NO_GPS_LOST_WITH_RC_VALID"            value="TRUE"/>
+        <define name="USE_AIRSPEED"                         value="TRUE"/>
+        <define name="STABILIZATION_ATTITUDE_SP_MAX_PHI"    value="45." unit="deg"  /> 
+        <define name="STABILIZATION_ATTITUDE_SP_MAX_THETA"  value="45." unit="deg"  />
+        <define name="STABILIZATION_ATTITUDE_SP_MAX_R"      value="90." unit="deg/s"/>
+        <define name="STABILIZATION_ATTITUDE_DEADBAND_R"    value="200"             />
+        <define name="FWD_SIDESLIP_GAIN"                    value="0.25"/> <!-- cyfoam 0.32-->
+        <!-- <define name="THRESHOLD_GROUND_DETECT"              value="40"/> -->
+        <define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
+        <define name="INS_EXT_VISION_ROTATION"              value="TRUE"/>
+    </section>
+    <section name="GROUND_DETECT">
+        <define name="THRESHOLD_GROUND_DETECT"              value="40"/>
+        <define name="USE_GROUND_DETECT_INDI_THRUST"        value="FALSE"/>
+        <define name="USE_GROUND_DETECT_AGL_DIST"           value="TRUE"/>
+        <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE"   value="0.24"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
+        <define name="AGL_DIST_FILTER"                      value="0.07"/>
+    </section>
+
+    <section name="IMU" prefix="IMU_">
+        <define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={2,11,22}, .scale={{9223,33875,50285},{941,3469,5167}}, .filter_sample_freq=1131.0, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-2,-2,20}, .scale={{9843,7289,11251},{2014,1485,2294}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-31,-13,10}, .scale={{49268,7401,20459},{10091,1513,4191}}} }"/>
+        <define name="MAG_CALIB"   value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-14,3,42}, .scale={{17279,2209,36874},{30247,3800,64095}}}}"/>
+        <define name="GYRO_CALIB"  value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/>                    
+        <define name="BODY_TO_IMU_PHI"   value="-4.54"  unit="deg"/>
+        <define name="BODY_TO_IMU_THETA" value="1.84"   unit="deg"/>
+        <define name="BODY_TO_IMU_PSI"   value="0."     unit="deg"/>
+    </section>
+
+    <section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
+        <define name  = "HEADING_MANUAL"            value = "TRUE"/>
+        <define name  = "YAW_STICK_IN_AUTO"         value = "TRUE"/>
+        <define name  = "MAX_R"                     value = "120.0" unit="deg/s"/>
+        <define name  = "FILT_CUTOFF"               value = "3.0" />
+        <define name  = "FILT_CUTOFF_ACC"           value = "2.0"/>
+        <define name  = "FILT_CUTOFF_VEL"           value = "5.0"/>
+        <define name  = "FILT_CUTOFF_POS"           value = "10.0"/>
+        <define name  = "ESTIMATION_FILT_CUTOFF"    value = "3.2" />
+        <define name  = "FILT_CUTOFF_P"             value = "2.0"/>
+        <define name  = "FILT_CUTOFF_Q"             value = "2.0"/>
+        <define name  = "FILT_CUTOFF_R"             value = "3.0" />
+        <define name  = "FILT_CUTOFF_RDOT"          value = "0.5" />
+        <define name  = "FILTER_YAW_RATE"           value = "TRUE"/>
+        <!--                                                 |   MF   |   MR   |    MB   |    ML   |   MP   |   ELE  |    RUD  |    AIL  |    FLA  |   PHI  |  THETA | -->
+        <define name  = "ACT_DYN"                   value = "{   15.1f,   15.1f,    15.1f,    15.1f,  24.07f,  50.00f,   50.00f,   50.00f,   50.00f,   0.00f,  0.00f }" />
+        <define name  = "ACT_IS_SERVO"              value = "{       0,       0,        0,        0,       0,       1,        1,        1,        1,       0,      0 }" />
+        <define name  = "ACT_MAX"                   value = "{ 9600.0f, 9600.0f,  9600.0f,  9600.0f, 9600.0f, 9600.0f,  9600.0f,  9600.0f,  9600.0f,  M_PI_4,  M_PI_4}"/>
+        <define name  = "ACT_MIN"                   value = "{    0.0f,    0.0f,     0.0f,     0.0f,    0.0f,    0.0f, -9600.0f, -9600.0f, -9600.0f, -M_PI_4, -M_PI_4}"/>
+        <define name  = "ACT_MAX_NORM"              value = "{    1.0f,    1.0f,     1.0f,     1.0f,    1.0f,    1.0f,     1.0f,     1.0f,     1.0f,    1.0f,    1.0f}"/>
+        <define name  = "ACT_MIN_NORM"              value = "{    0.0f,    0.0f,     0.0f,     0.0f,    0.0f,    0.0f,    -1.0f,    -1.0f,    -1.0f,   -1.0f,   -1.0f}"/>
+        <define name  = "WU"                        value = "{   0.75f,   0.75f,    0.75f,    0.75f,   1.00f,    6.0f,     6.0f,     6.0f,     6.0f,   1.20f,   1.20f}"/>
+        <define name  = "U_PREF"                    value = "{   0.00f,   0.00f,    0.00f,    0.00f,   0.00f, 8000.0f,    0.00f,    0.00f,    0.00f,   0.00f,   0.00f}"/>
+        <define name  = "WV"                        value = "{  4.0f,  4.0f,   4.0f,  8.0f,  8.0f,  1.00f}"/>   
+    </section>
+
+    <section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
+        <!--                                          | MF   |   MR   |   MB   |   ML   |   MP   |   ELE  |   RUD  |   AIL  |   FLA  |   PHI  |   THETA | -->
+        <!-- <define name  = "dFdu"               value = "{-0.50f,  -0.50f,  -0.50f,  -0.50f,   0.55f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f }"/>  
+        <define name  = "dMzdu"              value = "{-0.40f,   0.40f,  -0.40f,   0.40f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f }"/>
+        <define name  = "dMzdu_G2"           value = "{-0.02f,   0.02f,  -0.02f,   0.02f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f,   0.00f }"/>
+        <define name  = "l"                  value = "{ 0.423f,  0.408f,   0.423f, 0.408f,  0.00f,   1.00f,   1.00f,   1.00f,   1.00f,   0.00f,   0.00f }"/> -->
+        <define name="IXX_BODY"              value="0.04780"/>
+        <define name="IYY_BODY"              value="0.7546"/>
+        <define name="IZZ"                   value="0.9752"/>
+        <define name="IXX_WING"              value="0.08099"/>
+        <define name="IYY_WING"              value="0.1949"/>
+        <define name="M"                     value="6.67"/>
+    </section>   
+
+    <section name="AUTOPILOT">
+        <define name="MODE_MANUAL"      value="AP_MODE_ATTITUDE_DIRECT"/>
+        <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="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/> -->
+    </section>
+
+    <section name="BAT">
+        <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
+        <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
+        <define name="BAT_NB_CELLS"             value="6"/>
+    </section>
+
+    <section name="SIMULATOR" prefix="NPS_">
+        <define name="JSBSIM_MODEL"    value="rotwingv3c_SI" type="string"/>
+        <define name="SENSORS_PARAMS"  value="nps_sensors_params_default.h" type="string"/>
+        <define name="USE_COMMANDS"    value="TRUE"/>
+        <define name="JS_AXIS_MODE"    value="4"/>
+    </section>
+</airframe>
diff --git a/conf/airframes/examples/bebop2_gvf.xml b/conf/airframes/examples/bebop2_gvf.xml
new file mode 100644
index 0000000000..c10262e126
--- /dev/null
+++ b/conf/airframes/examples/bebop2_gvf.xml
@@ -0,0 +1,187 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<airframe name="bebop2_gvf">
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="bebop2">
+      <define name="STABILIZATION_INDI_G2_R" value="0.2784"/>
+    </target>
+
+    <target name="nps" board="pc">
+      <module name="fdm" type="jsbsim"/>
+      <module name="udp"/>
+      <define name="STABILIZATION_INDI_G2_R" value="0.0"/><!-- for jsbsim (rotor inertia is not modelled) -->
+      <!-- <define name="STABILIZATION_INDI_G2_R" value="0.20"/> --><!-- for gazebo -->
+    </target>
+
+	  <!-- 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="ublox"/>
+    <module name="stabilization" type="indi_simple"/>
+    <module name="ins" type="ekf2"/>
+    <module name="gvf_module"/>
+    <!--module name="gvf_parametric"/-->
+    <module name="air_data"/>
+    <module name="gps" type="ubx_ucenter"/>
+    <module name="logger_file">
+      <define name="LOGGER_FILE_PATH" value="/data/ftp/internal_000"/>
+    </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="TRUE"/>
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+    <!-- Magneto calibration -->
+    <define name="MAG_X_NEUTRAL" value="43"/>
+    <define name="MAG_Y_NEUTRAL" value="-151"/>
+    <define name="MAG_Z_NEUTRAL" value="-64"/>
+    <define name="MAG_X_SENS" value="7.21201776785" integer="16"/>
+    <define name="MAG_Y_SENS" value="7.20541442846" integer="16"/>
+    <define name="MAG_Z_SENS" value="7.55306977197" integer="16"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="SONAR_MAX_RANGE" value="2.2"/>
+  </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 -->
+    <define name="G1_P" value="0.0390"/>
+    <define name="G1_Q" value="0.0473"/>
+    <define name="G1_R" value="0.00122"/>
+
+    <!-- reference acceleration for attitude control -->
+    <define name="REF_ERR_P" value="170.0"/>
+    <define name="REF_ERR_Q" value="600.0"/>
+    <define name="REF_ERR_R" value="600.0"/>
+    <define name="REF_RATE_P" value="14.3"/>
+    <define name="REF_RATE_Q" value="28.0"/>
+    <define name="REF_RATE_R" value="28.0"/>
+
+    <!-- second order filter parameters -->
+    <define name="FILT_CUTOFF" value="3.2"/>
+
+    <!-- first order actuator dynamics -->
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
+
+    <!-- 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.655"/>
+    <define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
+  </section>
+
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <!-- Good weather -->
+    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="REF_MAX_SPEED" value="2" unit="m/s"/>
+    <!-- Bad weather -->
+    <!-- define name="MAX_BANK" value="32" unit="deg"/ -->
+    <define name="PGAIN" value="120"/>
+    <define name="DGAIN" value="100"/>
+    <define name="IGAIN" value="30"/>
+  </section>
+
+  <section name="NAVIGATION" prefix="NAV_">
+    <define name="CLIMB_VSPEED" value="4.5"/>
+    <define name="DESCEND_VSPEED" value="-1.0"/>
+  </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" type="string"/>
+  </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="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/rotwing_v3c_oneloop.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
index db96fad503..c2dddf747c 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
@@ -138,11 +138,10 @@
         </module>
 
         <module name="ground_detect"/>
-        <module name="rotwing_state_V2"/>
-        <module name="agl_dist"/>
-        <module name="rotwing_automation">
+        <module name="rotwing_state_V2">
             <define name="RW_USE_MODULES_V2" value="TRUE"/>
         </module>
+        <module name="agl_dist"/>
     </firmware>
 
     <!-- PWM actuators -->   
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
index 0caab89884..6ffb6f9957 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
@@ -139,11 +139,10 @@
         </module>
 
         <module name="ground_detect"/>
-        <module name="rotwing_state_V2"/>
-        <module name="agl_dist"/>
-        <module name="rotwing_automation">
+        <module name="rotwing_state_V2">
             <define name="RW_USE_MODULES_V2" value="TRUE"/>
         </module>
+        <module name="agl_dist"/>
     </firmware>
 
     <!-- PWM actuators -->   
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
index 1d33f7a50c..71d4dc3fb0 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
@@ -131,11 +131,10 @@
         </module>
 
         <module name="ground_detect"/>
-        <module name="rotwing_state_V2"/>
-        <module name="agl_dist"/>
-        <module name="rotwing_automation">
+        <module name="rotwing_state_V2">
             <define name="RW_USE_MODULES_V2" value="TRUE"/>
         </module>
+        <module name="agl_dist"/>
     </firmware>
 
     <!-- PWM actuators -->   
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
index 2f5e19619a..5293c5b3c9 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
@@ -102,11 +102,9 @@
         <module name="rotwing_state_V2">
              <define name = "USE_ROTMECH_VIRTUAL" value = "TRUE"/>
              <define name = "ROTMECH_DYN"         value = "3.0"/>
+             <define name="RW_USE_MODULES_V2"     value="TRUE"/>
         </module>
         <module name="agl_dist"/>
-        <module name="rotwing_automation">
-            <define name="RW_USE_MODULES_V2" value="TRUE"/>
-        </module>
     </firmware>
 
     <!-- PWM actuators -->   
diff --git a/conf/flight_plans/tudelft/oneloop_valkenburg.xml b/conf/flight_plans/tudelft/oneloop_valkenburg.xml
index 9bd8f69481..ab8b4d57b5 100644
--- a/conf/flight_plans/tudelft/oneloop_valkenburg.xml
+++ b/conf/flight_plans/tudelft/oneloop_valkenburg.xml
@@ -135,59 +135,41 @@
     <block name="Standby" strip_button="Standby" strip_icon="wp_quad.png">
         <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
         <call_once fun="rotwing_state_force_skew_off()"/>
-        <stay wp="STDBY" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+        <stay wp="STDBY"/>
     </block>
     <block name="Standby_fwd" strip_button="Standby_fwd" strip_icon="wp_fwd.png">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
       <call_once fun="rotwing_state_force_skew_off()"/>
-      <stay wp="STDBY" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+      <stay wp="STDBY"/>
     </block>
     <block name="Descend" strip_button="Descend" strip_icon="descend.png">
         <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
         <call_once fun="rotwing_state_force_skew_off()"/>
-        <stay wp="Descend" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+        <stay wp="Descend"/>
     </block>
-    <!-- <block name="stay_begin-trans">
-      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-      <stay wp="begin_trans" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
-    </block>  
-    <block name="end_transition">
-      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-      <stay wp="end_trans"/>
-    </block>-->
-    <block name="transition_CW" strip_button="transition_CW" strip_icon="transition_right.png">
-      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
-      <stay wp="end_trans"/>
-      <exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CW_fwd"/>
-    </block>
-    <block name="transition_CCW" strip_button="transition_CCW" strip_icon="transition_left.png">
-      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
-      <stay wp="end_trans"/>
-      <exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CCW_fwd"/>
-    </block>       
     <block name="safe" strip_button="Unsafe" strip_icon="alert.png">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-      <stay wp="HOME" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+      <stay wp="HOME"/>
     </block>
     <block name="Circle_CW_fwd" strip_button="CircleCW_Forward" strip_icon="circle_cw_fwd.png">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
       <call_once fun="rotwing_state_force_skew_off()"/>
-      <circle radius="100" wp="circ" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+      <circle radius="100" wp="circ"/>
     </block>
     <block name="Circle_CCW_fwd" strip_button="CircleCCW_Forward" strip_icon="circle_ccw_fwd.png">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
       <call_once fun="rotwing_state_force_skew_off()"/>
-      <circle radius="-100" wp="circ" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+      <circle radius="-100" wp="circ"/>
     </block>
     <block name="Circle_CW_quad" strip_button="CircleCW_Quad" strip_icon="circle_cw_quad.png">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <call_once fun="rotwing_state_force_skew_off()"/>
-      <circle radius="100" wp="circ" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+      <circle radius="100" wp="circ"/>
     </block>
     <block name="Circle_CCW_quad" strip_button="CircleCCW_Quad" strip_icon="circle_ccw_quad.png">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <call_once fun="rotwing_state_force_skew_off()"/>
-      <circle radius="-100" wp="circ" pre_call="rotwing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+      <circle radius="-100" wp="circ"/>
     </block>
     <block name="route_fw" strip_button="Route_fwd" strip_icon="path.png">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
diff --git a/conf/telemetry/GVF/gvf_default_rotorcraft.xml b/conf/telemetry/GVF/gvf_default_rotorcraft.xml
index d851087e0c..4c6e3d89bd 100644
--- a/conf/telemetry/GVF/gvf_default_rotorcraft.xml
+++ b/conf/telemetry/GVF/gvf_default_rotorcraft.xml
@@ -40,7 +40,7 @@
       <message name="WIND_INFO_RET"            period="1."/>
       <message name="AHRS_REF_QUAT"            period="0.5"/>
       <message name="STAB_ATTITUDE"            period=".25"/>
-      <message name="EFF_MAT_G"                period="2.0"/>
+      <message name="EFF_MAT_STAB"             period="2.0"/>
     </mode>
 
     <mode name="ppm">
@@ -114,7 +114,7 @@
       <message name="STAB_ATTITUDE_FLOAT"     period=".03"/>
       <message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
       <message name="STAB_ATTITUDE"           period=".25"/>
-      <message name="EFF_MAT_G"               period="2.0"/>
+      <message name="EFF_MAT_STAB"            period="2.0"/>
     </mode>
 
     <mode name="vert_loop" key_press="v">
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 7e1fb07889..45c7d655f3 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -249,7 +249,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/nederdrone_cyberzoo.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
    gui_color="blue"
   />
   <aircraft
@@ -582,26 +582,15 @@
    settings_modules="modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/ins_ekf2.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/stabilization_indi.xml"
    gui_color="#ffffcccaccca"
   />
-  <aircraft
-   name="Bebop2_DePonti"
-   ac_id="37"
-   airframe="airframes/tudelft/bebop2_oneloop.xml"
-   radio="radios/dummy.xml"
-   telemetry="telemetry/highspeed_rotorcraft.xml"
-   flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml"
-   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml [modules/guidance_indi.xml] [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/ins_extended.xml [modules/nav_basic_rotorcraft.xml] [modules/stabilization_indi_simple.xml] modules/stabilization_indi.xml modules/oneloop_andi.xml"
-   gui_color="#ffffcccaccca"
-  />
   <aircraft
    name="RW3C_DePonti"
    ac_id="95"
-   airframe="airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml"
+   airframe="airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml"
    radio="radios/crossfire_sbus.xml"
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/oneloop_cyberzoo.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rot_wing_V2.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rot_wing_automation.xml modules/rotwing_state_V2.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rotwing_V2.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rotwing_state_V2.xml"
    gui_color="#ffffcccaccca"
   />
   <aircraft
@@ -612,7 +601,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rotwing_V2.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rotwing_automation.xml modules/rotwing_state_V2.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rotwing_V2.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rotwing_state_V2.xml"
    gui_color="#ffffcccaccca"
   />
   <aircraft
@@ -623,7 +612,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
    gui_color="red"
   />
   <aircraft
@@ -656,10 +645,10 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/oneloop_cyberzoo.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rot_wing_V2.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rot_wing_automation.xml modules/rotwing_state_V2.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rotwing_V2.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rotwing_state_V2.xml"
    gui_color="#ffffcccaccca"
-   />
-   <aircraft 
+  />
+  <aircraft
    name="RotatingWingV3C"
    ac_id="34"
    airframe="airframes/tudelft/rotwing_v3c_oneloop.xml"
@@ -667,7 +656,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/oneloop_andi.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rotwing_V2.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rotwing_state_V2.xml"
    gui_color="red"
   />
   <aircraft
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c b/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c
index e65bfabfa7..b5ca4d6d18 100644
--- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c
+++ b/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c
@@ -29,6 +29,7 @@
 #include "modules/actuators/actuators.h"
 #include "modules/core/abi.h"
 #include "filters/low_pass_filter.h"
+#include "modules/ins/ins_ext_pose.h"
 
 #define FORCE_ONELOOP
 #ifdef FORCE_ONELOOP
diff --git a/sw/airborne/modules/guidance/gvf/gvf.c b/sw/airborne/modules/guidance/gvf/gvf.c
index 284b7b11ed..81e25239ea 100644
--- a/sw/airborne/modules/guidance/gvf/gvf.c
+++ b/sw/airborne/modules/guidance/gvf/gvf.c
@@ -128,13 +128,13 @@ void gvf_init(void)
 }
 
 // GENERIC TRAJECTORY CONTROLLER
-void gvf_control_2D(float ke, float kn, float e,
+void gvf_control_2D(float ke, float kn __attribute__((unused)), float e,
                     struct gvf_grad *grad, struct gvf_Hess *hess)
 {
   gvf_t0 = get_sys_time_msec();
 
   gvf_low_level_getState();
-  float course = gvf_state.course;
+  float course __attribute__((unused)) = gvf_state.course;
   float px_dot = gvf_state.px_dot;
   float py_dot = gvf_state.py_dot;
 
@@ -176,8 +176,8 @@ void gvf_control_2D(float ke, float kn, float e,
   float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
                        / norm_pd_dot;
 
-  float md_dot_x =  md_y * md_dot_const;
-  float md_dot_y = -md_x * md_dot_const;
+  float md_dot_x __attribute__((unused)) =  md_y * md_dot_const;
+  float md_dot_y __attribute__((unused))= -md_x * md_dot_const;
 
   #if defined(ROTORCRAFT_FIRMWARE)
 
diff --git a/sw/airborne/modules/ins/ins_ext_pose.h b/sw/airborne/modules/ins/ins_ext_pose.h
index 31e493f61f..ea703e0634 100644
--- a/sw/airborne/modules/ins/ins_ext_pose.h
+++ b/sw/airborne/modules/ins/ins_ext_pose.h
@@ -38,6 +38,7 @@ Section 5.3: Non-additive noise formulation and equations
 
 #include "std.h"
 #include "math/pprz_algebra_float.h"
+#include "generated/airframe.h"
 #include "state.h"
 #include <stdio.h>
 
diff --git a/sw/airborne/modules/nav/ground_detect.c b/sw/airborne/modules/nav/ground_detect.c
index 337cc1b142..1b4c9aa714 100644
--- a/sw/airborne/modules/nav/ground_detect.c
+++ b/sw/airborne/modules/nav/ground_detect.c
@@ -24,7 +24,6 @@
  */
 
 #include "ground_detect.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
 #include "filters/low_pass_filter.h"
 #include "firmwares/rotorcraft/autopilot_firmware.h"
 
@@ -82,10 +81,13 @@ void ground_detect_periodic()
   // Evaluate thrust given (less than hover thrust)
   // Use the control effectiveness in thrust in order to estimate the thrust delivered (only works for multicopters)
   float specific_thrust = 0.0; // m/s2
+ 
+#if USE_GROUND_DETECT_INDI_THRUST
   uint8_t i;
   for (i = 0; i < INDI_NUM_ACT; i++) {
     specific_thrust += actuator_state_filt_vect[i] * g1g2[3][i] * -((int32_t) act_is_servo[i] - 1);
   }
+#endif
 
   // vertical component
   float spec_thrust_down;