diff --git a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
index 643ffc5feb..d1c7755691 100644
--- a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
@@ -27,6 +27,7 @@
         <configure name="MODEM_PORT" value="UART2"/>
         <configure name="MODEM_BAUD" value="B57600"/>
       </module>
+      <module name="telemetry" type="intermcu"/>
 
       <module name="imu" type="px4fmu_v2.4"/>
       <module name="gps" type="ublox"/>
@@ -159,6 +160,11 @@
         <configure name="INTERMCU_PORT" value="UART2" />
         <configure name="INTERMCU_BAUD" value="B1500000" />
       </module>
+      <module name="telemetry" type="transparent">
+        <configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
+        <configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
+        <define name="TELEMETRY_DISABLE_RX"/>
+      </module>
     </target>
 
   </firmware>
diff --git a/conf/airframes/OPENUAS/openuas_df_mini_spirit.xml b/conf/airframes/OPENUAS/openuas_df_mini_spirit.xml
index 27e89212d4..c66926f3a7 100644
--- a/conf/airframes/OPENUAS/openuas_df_mini_spirit.xml
+++ b/conf/airframes/OPENUAS/openuas_df_mini_spirit.xml
@@ -436,11 +436,11 @@
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="SERVO_HATCH_OPEN" value="0"/>
     <define name="SERVO_HATCH_CLOSED" value="-9600"/>
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
     <define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
   </section>
diff --git a/conf/airframes/OPENUAS/openuas_durafly_g2.xml b/conf/airframes/OPENUAS/openuas_durafly_g2.xml
index 4a1a43de58..d6bfcbf886 100644
--- a/conf/airframes/OPENUAS/openuas_durafly_g2.xml
+++ b/conf/airframes/OPENUAS/openuas_durafly_g2.xml
@@ -290,11 +290,11 @@
     <define name="ROTORSTARTER_OFF" value="MIN_PPRZ" />
    <!-- <define name="AGR_CLIMB_PITCH" value="35" unit="deg" />-->
 
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
   </section>
 
   <servos>
diff --git a/conf/airframes/OPENUAS/openuas_durafly_goblin.xml b/conf/airframes/OPENUAS/openuas_durafly_goblin.xml
index a343cfbf55..9351a07307 100644
--- a/conf/airframes/OPENUAS/openuas_durafly_goblin.xml
+++ b/conf/airframes/OPENUAS/openuas_durafly_goblin.xml
@@ -198,11 +198,11 @@ NOTES:
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="SERVO_HATCH_OPEN" value="0"/>
     <define name="SERVO_HATCH_CLOSED" value="-9600"/>
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
     <define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
   </section>
diff --git a/conf/airframes/OPENUAS/openuas_eflite_t28.xml b/conf/airframes/OPENUAS/openuas_eflite_t28.xml
index 1ebf7369cb..a5dab406a7 100644
--- a/conf/airframes/OPENUAS/openuas_eflite_t28.xml
+++ b/conf/airframes/OPENUAS/openuas_eflite_t28.xml
@@ -518,11 +518,11 @@ NOTES:
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="SERVO_HATCH_OPEN" value="0"/>
     <define name="SERVO_HATCH_CLOSED" value="-9600"/>
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
     <define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
   </section>
diff --git a/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml b/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
index e1c0f48782..048404ba78 100644
--- a/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
+++ b/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
@@ -526,11 +526,11 @@ NOTES:
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="SERVO_HATCH_OPEN" value="0"/>
     <define name="SERVO_HATCH_CLOSED" value="-9600"/>
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
     <define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
   </section>
diff --git a/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml b/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml
index 4035c95089..e3a3b83ba1 100644
--- a/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml
+++ b/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml
@@ -389,11 +389,11 @@ NOTES:
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ" />
     <define name="SERVO_HATCH_OPEN" value="0" />
     <define name="SERVO_HATCH_CLOSED" value="-9600" />
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
   </section>
 <!-- ********************** SERVO MIXER **************************** -->
    <!--For mixed controls -->
diff --git a/conf/airframes/OPENUAS/openuas_own_flexo.xml b/conf/airframes/OPENUAS/openuas_own_flexo.xml
index 758377eb46..10e44d18cc 100644
--- a/conf/airframes/OPENUAS/openuas_own_flexo.xml
+++ b/conf/airframes/OPENUAS/openuas_own_flexo.xml
@@ -483,11 +483,11 @@ NOTES:
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="SERVO_HATCH_OPEN" value="0"/>
     <define name="SERVO_HATCH_CLOSED" value="-9600"/>
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
     <define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
   </section>
diff --git a/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml b/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml
index fd01018d4d..e2fae9a3cd 100644
--- a/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml
+++ b/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml
@@ -485,11 +485,11 @@ NOTES:
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="SERVO_HATCH_OPEN" value="0"/>
     <define name="SERVO_HATCH_CLOSED" value="-9600"/>
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
     <define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
   </section>
diff --git a/conf/airframes/OPENUAS/openuas_own_vivify.xml b/conf/airframes/OPENUAS/openuas_own_vivify.xml
index c08eeb1f4d..084cf6b47a 100644
--- a/conf/airframes/OPENUAS/openuas_own_vivify.xml
+++ b/conf/airframes/OPENUAS/openuas_own_vivify.xml
@@ -287,11 +287,11 @@
     <define name="SERVO_HATCH_CLOSED" value="-9600" />
     <define name="AGR_CLIMB_PITCH" value="35" unit="deg" />
 
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
-    <define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />
-    <define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
+    <define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />
+    <define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
   </section>
 
   <commands>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_disco.xml b/conf/airframes/OPENUAS/openuas_parrot_disco.xml
index ce1c2707a3..e1a5739097 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_disco.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_disco.xml
@@ -437,11 +437,11 @@
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ" />
     <define name="SERVO_HATCH_OPEN" value="0" />
     <define name="SERVO_HATCH_CLOSED" value="-9600" />
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
   </section>
 <!-- ********************** SERVO MIXER **************************** -->
   <section name="MIXER">
diff --git a/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml b/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml
index 971129f472..f073aebc4d 100644
--- a/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml
+++ b/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml
@@ -300,11 +300,11 @@ NOTES:
     <define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="SERVO_HATCH_OPEN" value="0"/>
     <define name="SERVO_HATCH_CLOSED" value="-9600"/>
-    <define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
-    <define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
-    <!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
-    <!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
-    <define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
+    <define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
+    <define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
+    <!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
+    <!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
+    <define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
     <define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
     <define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
   </section>
diff --git a/conf/airframes/tudelft/disco_modified.xml b/conf/airframes/tudelft/disco_modified.xml
index 4178f6d41f..4047a8f05a 100644
--- a/conf/airframes/tudelft/disco_modified.xml
+++ b/conf/airframes/tudelft/disco_modified.xml
@@ -214,7 +214,7 @@
   </servos>
 
   <section name="ServoPositions">
-    <define name="ThrottleHigh()" value="(imcu_get_command(COMMAND_THROTTLE)>9600/2)" />
+    <define name="ThrottleHigh()" value="(command_get(COMMAND_THROTTLE)>9600/2)" />
   </section>
 
 <!-- ********************* SERVO MIXER *************************** -->
diff --git a/conf/airframes/tudelft/iris_indi.xml b/conf/airframes/tudelft/iris_indi.xml
index 1630335245..ddfb43a529 100644
--- a/conf/airframes/tudelft/iris_indi.xml
+++ b/conf/airframes/tudelft/iris_indi.xml
@@ -19,6 +19,7 @@
     <define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" />
     <!-- in seconds-->
     <module name="telemetry" type="transparent" />
+    <module name="telemetry" type="intermcu"/>
     <module name="imu" type="px4fmu_v2.4"/>
     <module name="gps" type="ublox" >
       <configure name="GPS_BAUD" value="B57600"/>
@@ -91,6 +92,11 @@
       <configure name="INTERMCU_PORT" value="UART2" />
       <configure name="INTERMCU_BAUD" value="B1500000" />
     </module>
+    <module name="telemetry" type="transparent">
+      <configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
+      <configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
+      <define name="TELEMETRY_DISABLE_RX"/>
+    </module>
   </firmware>
 
   <section name="MISC">
diff --git a/conf/airframes/tudelft/logo600.xml b/conf/airframes/tudelft/logo600.xml
index ea7608cf4d..09f41849b1 100644
--- a/conf/airframes/tudelft/logo600.xml
+++ b/conf/airframes/tudelft/logo600.xml
@@ -26,6 +26,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
     <target name="ap" board="lisa_mx_2.1">
 
       <module name="telemetry"     type="xbee_api"/>
+      <module name="telemetry" type="intermcu"/>
       <module name="imu"           type="lisa_mx_v2.1"/>
       <module name="gps"           type="ublox">
         <configure name="GPS_PORT" value="UART4"/>
@@ -68,6 +69,11 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
       <module name="intermcu"     type="uart">
         <define name="REMAP_UART3" value="TRUE"/>
       </module>
+      <module name="telemetry" type="transparent">
+        <configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
+        <configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
+        <define name="TELEMETRY_DISABLE_RX"/>
+      </module>
     </target>
   </firmware>
 
diff --git a/conf/airframes/tudelft/outback.xml b/conf/airframes/tudelft/outback.xml
index 1e772ebd7e..87755cfa40 100644
--- a/conf/airframes/tudelft/outback.xml
+++ b/conf/airframes/tudelft/outback.xml
@@ -19,6 +19,7 @@
       </target-->
 
       <module name="telemetry"     type="xbee_api"/>
+      <module name="telemetry" type="intermcu"/>
       <module name="imu"           type="mpu6000"/>
       <module name="gps"           type="ublox">
         <define name="GPS_TIMEOUT" value="2"/>
@@ -72,6 +73,11 @@
       </module>
       <module name="intermcu"      type="uart"/>
       <module name="telemetry"     type="intermcu"/>
+      <module name="telemetry" type="transparent">
+        <configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
+        <configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
+        <define name="TELEMETRY_DISABLE_RX"/>
+      </module>
       <module name="opa_controller"/>
       <module name="heli_swashplate_mixing"/>
       <module name="gps" type="ublox"/>
diff --git a/conf/airframes/tudelft/robird.xml b/conf/airframes/tudelft/robird.xml
index 12f20b88ee..6776f8f793 100644
--- a/conf/airframes/tudelft/robird.xml
+++ b/conf/airframes/tudelft/robird.xml
@@ -18,6 +18,7 @@ Flapping wing frame equiped with
     <define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" />
     <!-- in seconds-->
     <module name="telemetry" type="xbee_api"/>
+    <module name="telemetry" type="intermcu"/>
     <module name="imu" type="px4fmu_v2.4"/>
     <module name="gps" type="ublox" >
       <configure name="GPS_BAUD" value="B57600"/>
@@ -82,6 +83,11 @@ Flapping wing frame equiped with
       <configure name="INTERMCU_PORT" value="UART2" />
       <configure name="INTERMCU_BAUD" value="B1500000" />
     </module>
+    <module name="telemetry" type="transparent">
+      <configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
+      <configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
+      <define name="TELEMETRY_DISABLE_RX"/>
+    </module>
   </firmware>
 
   <section name="MISC">
diff --git a/conf/airframes/tudelft/twoseastwenty.xml b/conf/airframes/tudelft/twoseastwenty.xml
index b606b883cc..f5f9edcd56 100644
--- a/conf/airframes/tudelft/twoseastwenty.xml
+++ b/conf/airframes/tudelft/twoseastwenty.xml
@@ -9,7 +9,6 @@
   <!-- ************************* FIRMWARE ************************* -->
 
   <firmware name="fixedwing">
-    <define name="RADIO_CONTROL_NB_CHANNEL" value="8" />
     <target name="ap" board="lisa_l_1.0">
       <configure name="SEPARATE_FBW" value="1"/>
       <configure name="AHRS_ALIGNER_LED" value="1"/>
@@ -20,10 +19,10 @@
       <module name="intermcu" type="uart">
         <configure name="INTERMCU_PORT" value="UART2"/>
       </module>
-      <module name="radio_control" type="sbus_dual">
-        <configure name="SBUS1_PORT" value="UART3"/>
-        <configure name="SBUS2_PORT" value="UART5"/>
-      </module>
+      <!-- mapping with intermcu channel -->
+      <define name="RADIO_FLAPS" value="RADIO_AUX1"/>
+      <define name="RADIO_GAIN1" value="RADIO_AUX2"/>
+      <define name="RADIO_GAIN2" value="RADIO_AUX3"/>
       <!-- AP -->
       <module name="control"/>
       <module name="navigation"/>
@@ -32,6 +31,7 @@
         <configure name="MODEM_BAUD" value="B9600"/>
         <configure name="MODEM_PORT" value="UART3"/>
       </module>
+      <module name="telemetry" type="intermcu"/>
       <module name="ins" type="xsens700">
         <configure name="XSENS_PORT" value="uart1"/>
         <configure name="XSENS_BAUD" value="B230400"/>
@@ -60,6 +60,11 @@
       <module name="intermcu" type="uart">
         <configure name="INTERMCU_PORT" value="UART1"/>
       </module>
+      <module name="telemetry" type="transparent">
+        <configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
+        <configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
+        <define name="TELEMETRY_DISABLE_RX"/>
+      </module>
       <!-- SERVO'S -->
       <module name="actuators" type="pwm"/>
       <define name="USE_SERVOS_7AND8"/>
diff --git a/conf/autopilot/fixedwing_autopilot.xml b/conf/autopilot/fixedwing_autopilot.xml
index 4e8af14afa..75d5a5951a 100644
--- a/conf/autopilot/fixedwing_autopilot.xml
+++ b/conf/autopilot/fixedwing_autopilot.xml
@@ -21,22 +21,18 @@
     </includes>
 
     <control_block name="actuators_ap">
-      <call fun="PPRZ_MUTEX_LOCK(ap_state_mtx)"/>
       <call fun="AP_COMMAND_SET_THROTTLE(v_ctl_throttle_slewed)"/>
       <call fun="AP_COMMAND_SET_ROLL(-h_ctl_aileron_setpoint)"/>
       <call fun="AP_COMMAND_SET_PITCH(h_ctl_elevator_setpoint)"/>
       <call fun="AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint)"/>
       <call fun="AP_COMMAND_SET_CL(h_ctl_flaps_setpoint)"/>
-      <call fun="PPRZ_MUTEX_UNLOCK(ap_state_mtx)"/>
     </control_block>
 
     <control_block name="set_attitude_from_rc">
-      <call fun="PPRZ_MUTEX_LOCK(fbw_state_mtx)"/>
       <call fun="AP_SETPOINT_ROLL(h_ctl_roll_setpoint, AUTO1_MAX_ROLL)"/>
       <call fun="AP_SETPOINT_PITCH(h_ctl_pitch_setpoint, AUTO1_MAX_PITCH)"/>
       <call fun="AP_SETPOINT_YAW_RATE(h_ctl_yaw_rate_setpoint, AUTO1_MAX_YAW_RATE)"/>
       <call fun="AP_SETPOINT_THROTTLE(v_ctl_throttle_setpoint)"/>
-      <call fun="PPRZ_MUTEX_UNLOCK(fbw_state_mtx)"/>
     </control_block>
 
     <control_block name="attitude">
@@ -60,7 +56,7 @@
         <call fun="nav_periodic_task()"/>
       </control>
       <control> <!-- only for display -->
-        <call fun="v_ctl_throttle_slewed = imcu_get_radio(RADIO_THROTTLE)"/>
+        <call fun="v_ctl_throttle_slewed = radio_control_get(RADIO_THROTTLE)"/>
       </control>
       <exception cond="RCLost() && autopilot_in_flight()" deroute="HOME"/>
     </mode>
diff --git a/conf/autopilot/fixedwing_rusty.xml b/conf/autopilot/fixedwing_rusty.xml
index a9913de67c..28a91b9fcb 100644
--- a/conf/autopilot/fixedwing_rusty.xml
+++ b/conf/autopilot/fixedwing_rusty.xml
@@ -26,22 +26,18 @@
     </includes>
 
     <control_block name="actuators_ap">
-      <call fun="PPRZ_MUTEX_LOCK(ap_state_mtx)"/>
       <call fun="AP_COMMAND_SET_THROTTLE(v_ctl_throttle_slewed)"/>
       <call fun="AP_COMMAND_SET_ROLL(-h_ctl_aileron_setpoint)"/>
       <call fun="AP_COMMAND_SET_PITCH(h_ctl_elevator_setpoint)"/>
       <call fun="AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint)"/>
       <call fun="AP_COMMAND_SET_CL(h_ctl_flaps_setpoint)"/>
-      <call fun="PPRZ_MUTEX_UNLOCK(ap_state_mtx)"/>
     </control_block>
 
     <control_block name="set_attitude_from_rc">
-      <call fun="PPRZ_MUTEX_LOCK(fbw_state_mtx)"/>
       <call fun="AP_SETPOINT_ROLL(h_ctl_roll_setpoint, AUTO1_MAX_ROLL)"/>
       <call fun="AP_SETPOINT_PITCH(h_ctl_pitch_setpoint, AUTO1_MAX_PITCH)"/>
       <call fun="AP_SETPOINT_YAW_RATE(h_ctl_yaw_rate_setpoint, AUTO1_MAX_YAW_RATE)"/>
       <call fun="AP_SETPOINT_THROTTLE(v_ctl_throttle_setpoint)"/>
-      <call fun="PPRZ_MUTEX_UNLOCK(fbw_state_mtx)"/>
     </control_block>
 
     <control_block name="attitude">
@@ -65,7 +61,7 @@
         <call fun="nav_periodic_task()"/>
       </control>
       <control> <!-- only for display -->
-        <call fun="v_ctl_throttle_slewed = imcu_get_radio(RADIO_THROTTLE)"/>
+        <call fun="v_ctl_throttle_slewed = radio_control_get(RADIO_THROTTLE)"/>
       </control>
       <exception cond="RCLost() && autopilot_in_flight()" deroute="HOME"/>
     </mode>
diff --git a/conf/flight_plans/OPENUAS/include_obc2014_mission.xml b/conf/flight_plans/OPENUAS/include_obc2014_mission.xml
index d226a43002..b401f4fbb2 100644
--- a/conf/flight_plans/OPENUAS/include_obc2014_mission.xml
+++ b/conf/flight_plans/OPENUAS/include_obc2014_mission.xml
@@ -235,8 +235,8 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
       <!--  TODO: make some movements on all defelctros as a sign we are in holding and to see if all works Maybe a separate testblock-->
       <!-- <set var="waypoints[WP_START].x" value="WaypointX(WP_EL2) + 50"/> -->
       <!-- <set var="waypoints[WP_START].y" value="WaypointY(WP_EL2) + 50"/> -->
-      <!-- <set value="0" var="ap_state->commands[COMMAND_CAMBER]"/> -->
-      <!-- <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
+      <!-- <set value="0" var="commands[COMMAND_CAMBER]"/> -->
+      <!-- <set value="0" var="commands[COMMAND_BRAKE]"/> -->
       <call_once fun="AirbrakesOff()"/>
       <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/><!-- maybe not moving the plane would still make a test of defelction possible -->
     </block>
@@ -265,7 +265,7 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
     </block>
 
     <block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
-      <!-- <set value="3000" var="ap_state->commands[COMMAND_CAMBER]"/> -->
+      <!-- <set value="3000" var="commands[COMMAND_CAMBER]"/> -->
       <set value="0" var="autopilot.kill_throttle"/>
       <set value="1" var="autopilot.launch"/>
       <set value="0" var="autopilot.flight_time"/>
@@ -317,7 +317,7 @@ to “Airfield Home” and orbit for either a landing or regain of Data Link.
      ***************************************************** -->
 
 <!-- maybe a HelperWaypoint to gettart n return from a mission Tracking to EL1
-      <set value="0" var="ap_state->commands[COMMAND_CAMBER]"/>-->
+      <set value="0" var="commands[COMMAND_CAMBER]"/>-->
 
 <!-- *********** Track to EL-1 *********** -->
     <block name="EntryLane1" strip_button="Joe" strip_icon="lookdown.png" group="Mission">
@@ -441,7 +441,7 @@ extern bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t e
 #define NavDropComputeApproach(_target, _start, _radius) nav_drop_compute_approach(_target, _start, _radius)
 #define NavDropUpdateRelease(_wp) nav_drop_update_release(_wp)
 #define NavDropShoot() nav_drop_shoot()
-#define NavDropCloseHatch() ({ ap_state->commands[COMMAND_HATCH] = MIN_PPRZ; })
+#define NavDropCloseHatch() ({ commands[COMMAND_HATCH] = MIN_PPRZ; })
 #define NavDropAligned() Qdr(DegOfRad(nav_drop_qdr_aligned)) -->
 
 
@@ -468,7 +468,7 @@ extern bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t e
 <!-- *********** Release Payload *********** -->
     <block name="shoot">
       <!-- TODO maybe extend crowbreak 80%? for slowes possible speed? -->
-      <!-- <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
+      <!-- <set value="0" var="commands[COMMAND_BRAKE]"/> -->
       <!-- TODO is electrcal plane and almost there maybe stop proppelor from spinning for a few seconds -->
       <call_once fun="NavDropShoot()"/>
       <!-- TODO add maxspeed again -->
diff --git a/conf/flight_plans/OPENUAS/include_obc2014_testing.xml b/conf/flight_plans/OPENUAS/include_obc2014_testing.xml
index c654c72320..bbc66fbe36 100644
--- a/conf/flight_plans/OPENUAS/include_obc2014_testing.xml
+++ b/conf/flight_plans/OPENUAS/include_obc2014_testing.xml
@@ -10,7 +10,7 @@
   <blocks>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
       <call fun="Fly()"/>
-        <!--   <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
+        <!--   <set value="0" var="commands[COMMAND_BRAKE]"/> -->
       <circle alt="WaypointAlt(WP_STDBY)" radius="nav_radius" wp="STDBY"/>
     </block>
 <!--Below routes that are handy for testing only when in the field and performing several tests -->
diff --git a/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml b/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml
index d8fdc307c0..07e6c0d4c0 100644
--- a/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml
+++ b/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml
@@ -131,11 +131,11 @@ Should be unified for Hybrid, FW and rotorcraft
       <!--<while cond="!GpsFixValid() || gps.pacc @GT 1500 || stateIsAttitudeValid()"/>-->
       <while cond="!GpsFixValid()"/>
       <!-- Wiggle Wiggle when we have GPS , time to fly...-->
-      <!--<set var="ap_state->commands[COMMAND_YAW]" value="-MAX_PPRZ"/>
+      <!--<set var="commands[COMMAND_YAW]" value="-MAX_PPRZ"/>
       <while cond="LessThan(NavBlockTime(), 2)"/>
-      <set var="ap_state->commands[COMMAND_YAW]" value="MAX_PPRZ"/>
+      <set var="commands[COMMAND_YAW]" value="MAX_PPRZ"/>
       <while cond="LessThan(NavBlockTime(), 3)"/>
-      <set var="ap_state->commands[COMMAND_YAW]" value="-MAX_PPRZ"/>-->
+      <set var="commands[COMMAND_YAW]" value="-MAX_PPRZ"/>-->
     </block>
 
 <!-- *********** Set the ground reference height and the home position *********** -->
@@ -165,8 +165,8 @@ Should be unified for Hybrid, FW and rotorcraft
     -->
 
     <block name="ReadyForDeparture" >
-      <!-- <set value="0" var="ap_state->commands[COMMAND_CAMBER]"/> -->
-      <!-- <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
+      <!-- <set value="0" var="commands[COMMAND_CAMBER]"/> -->
+      <!-- <set value="0" var="commands[COMMAND_BRAKE]"/> -->
       <set var="v_ctl_speed_mode" value="V_CTL_SPEED_AIRSPEED" /><!-- set the preferred flight mode -->
       <!-- Must have an RC for this flightplan and on at least for the start, we wait till the throttle is high -->
       <exception cond="ThrottleHigh()" deroute="Takeoff"/>
@@ -174,7 +174,7 @@ Should be unified for Hybrid, FW and rotorcraft
     </block>
 
     <block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="Ops">
-      <!-- <set var="ap_state->commands[COMMAND_LIGHT]" value="PPRZ_MAX"/> -->
+      <!-- <set var="commands[COMMAND_LIGHT]" value="PPRZ_MAX"/> -->
       <set var="autopilot.kill_throttle" value="0"/>
       <!--<var="autopilot.launch" set value="1"  />-->
       <while cond="1 @GT autopilot.launch"/> <!-- Setting of launch performed with takeoff_detect module disable -->
diff --git a/conf/modules/system_core.xml b/conf/modules/system_core.xml
index 79a911e28a..a1960f6724 100644
--- a/conf/modules/system_core.xml
+++ b/conf/modules/system_core.xml
@@ -28,6 +28,7 @@
     <include name="$(SRC_MODULES)"/>
     <file name="sys_time.c" dir="mcu_periph"/>
     <file name="commands.c"/>
+    <define name="USE_COMMANDS"/>
     <file_arch name="sys_time_arch.c" dir="mcu_periph"/>
     <file_arch name="led_hw.c" dir="." cond="ifeq ($(ARCH), stm32)"/>
     <file_arch name="led_hw.c" dir="." cond="ifeq ($(TARGET), sim)"/>
diff --git a/conf/settings/control/ctl_adaptive.xml b/conf/settings/control/ctl_adaptive.xml
index 1754b50a76..0144772efc 100644
--- a/conf/settings/control/ctl_adaptive.xml
+++ b/conf/settings/control/ctl_adaptive.xml
@@ -6,11 +6,6 @@
   <dl_settings>
 
     <dl_settings NAME="control">
-      <dl_settings NAME="trim">
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
-      </dl_settings>
-
       <dl_settings NAME="attitude">
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
diff --git a/conf/settings/control/ctl_basic.xml b/conf/settings/control/ctl_basic.xml
index 80b781cff0..3e8e8ccaec 100644
--- a/conf/settings/control/ctl_basic.xml
+++ b/conf/settings/control/ctl_basic.xml
@@ -6,12 +6,6 @@
   <dl_settings>
     <dl_settings NAME="control">
 
-      <dl_settings NAME="trim">
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
-        <dl_setting MAX="9000" MIN="-9000" STEP="1" VAR="ap_state->command_yaw_trim" shortname="yaw_trim" param="COMMAND_YAW_TRIM"/>
-      </dl_settings>
-
       <dl_settings NAME="attitude">
         <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="60" MIN="0" STEP="1." VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg"/>
diff --git a/conf/settings/control/ctl_energy.xml b/conf/settings/control/ctl_energy.xml
index 3f4f777dc5..b389656825 100644
--- a/conf/settings/control/ctl_energy.xml
+++ b/conf/settings/control/ctl_energy.xml
@@ -6,11 +6,6 @@
   <dl_settings>
     <dl_settings NAME="control">
 
-      <dl_settings NAME="trim">
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
-      </dl_settings>
-
       <dl_settings NAME="attitude">
         <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="60" MIN="0" STEP="1." VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg"/>
diff --git a/conf/settings/control/ctl_energyadaptive.xml b/conf/settings/control/ctl_energyadaptive.xml
index 2713cd4d50..e85fff8c9f 100644
--- a/conf/settings/control/ctl_energyadaptive.xml
+++ b/conf/settings/control/ctl_energyadaptive.xml
@@ -6,13 +6,6 @@
   <dl_settings>
     <dl_settings NAME="control">
 
-      <dl_settings NAME="trim">
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
-      </dl_settings>
-
-
-
       <dl_settings NAME="attitude">
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
diff --git a/conf/settings/control/ctl_new.xml b/conf/settings/control/ctl_new.xml
index a998149e56..51eb502139 100644
--- a/conf/settings/control/ctl_new.xml
+++ b/conf/settings/control/ctl_new.xml
@@ -6,11 +6,6 @@
   <dl_settings>
 
     <dl_settings NAME="control">
-      <dl_settings NAME="trim">
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
-      </dl_settings>
-
       <dl_settings NAME="attitude">
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
diff --git a/conf/settings/control/ctl_new_airspeed.xml b/conf/settings/control/ctl_new_airspeed.xml
index f43408a362..bed1cecacb 100644
--- a/conf/settings/control/ctl_new_airspeed.xml
+++ b/conf/settings/control/ctl_new_airspeed.xml
@@ -6,11 +6,6 @@
   <dl_settings>
 
     <dl_settings NAME="control">
-      <dl_settings NAME="trim">
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
-        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
-      </dl_settings>
-
       <dl_settings NAME="attitude">
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
diff --git a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c
index fb2ab5f6cd..673a751d97 100644
--- a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c
+++ b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c
@@ -27,7 +27,6 @@
  */
 
 #include "modules/radio_control/radio_control.h"
-#include "modules/radio_control/spektrum_arch.h"
 #include "modules/radio_control/spektrum.h"
 #include "modules/core/abi.h"
 #include "std.h"
diff --git a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.h b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.h
deleted file mode 100644
index 7c504783e2..0000000000
--- a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * Copyright (C) 2010 Eric Parsonage <eric@eparsonage.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, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#ifndef RADIO_CONTROL_SPEKTRUM_ARCH_H
-#define RADIO_CONTROL_SPEKTRUM_ARCH_H
-
-#include "modules/radio_control/spektrum_radio.h"
-
-#if USE_NPS
-extern void radio_control_feed(void);
-#endif
-
-#endif /* RADIO_CONTROL_SPEKTRUM_ARCH_H */
diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h
index 5a13375a3a..870d6391d2 100644
--- a/sw/airborne/firmwares/fixedwing/nav.h
+++ b/sw/airborne/firmwares/fixedwing/nav.h
@@ -136,7 +136,7 @@ extern float baseleg_out_qdr;
 extern void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius);
 extern void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide);
 
-#define RCLost() bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST)
+#define RCLost() RadioControlIsLost()
 
 extern void nav_follow(uint8_t _ac_id, float _distance, float _height);
 #define NavFollow(_ac_id, _distance, _height) nav_follow(_ac_id, _distance, _height)
diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c
index 56f07bb007..8ebc93f8cf 100644
--- a/sw/airborne/main_fbw.c
+++ b/sw/airborne/main_fbw.c
@@ -37,15 +37,6 @@
 #include "modules/datalink/datalink.h"
 #include "modules/datalink/telemetry.h"
 
-/* So one can use these in command_laws section */
-#define And(x, y) ((x) && (y))
-#define Or(x, y) ((x) || (y))
-#define Min(x,y) (x < y ? x : y)
-#define Max(x,y) (x > y ? x : y)
-#define LessThan(_x, _y) ((_x) < (_y))
-#define MoreThan(_x, _y) ((_x) > (_y))
-
-
 /** Fly by wire modes */
 uint8_t fbw_mode;
 bool fbw_motors_on = false;
diff --git a/sw/airborne/modules/actuators/actuators.c b/sw/airborne/modules/actuators/actuators.c
index 6d2ca0bc32..9595c836b1 100644
--- a/sw/airborne/modules/actuators/actuators.c
+++ b/sw/airborne/modules/actuators/actuators.c
@@ -87,6 +87,7 @@ void actuators_init(void)
  */
 void actuators_periodic(void)
 {
+#if USE_COMMANDS
   pprz_t trimmed_commands[COMMANDS_NB];
   int i;
   for (i = 0; i < COMMANDS_NB; i++) {trimmed_commands[i] = commands[i];}
@@ -113,6 +114,7 @@ void actuators_periodic(void)
   SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode());
   // TODO SetApOnlyActuatorsFromCommands(ap_commands, autopilot_get_mode());
 #endif
+#endif // USE_COMMANDS
 }
 
 #else // No command_laws section or no actuators
diff --git a/sw/airborne/modules/core/commands.h b/sw/airborne/modules/core/commands.h
index dd77b96441..3a0f9b8319 100644
--- a/sw/airborne/modules/core/commands.h
+++ b/sw/airborne/modules/core/commands.h
@@ -31,6 +31,8 @@
 #include "paparazzi.h"
 #include "generated/airframe.h"
 
+#if COMMANDS_NB // commands are defined and nb is > 0
+
 /** Storage of intermediate command values.
  *  These values come from the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
  *  They are asyncronisly used to set the servos
@@ -74,6 +76,8 @@ static inline pprz_t command_get(uint8_t idx)
   return 0; // is it the best value ???
 }
 
+#endif
+
 extern void commands_init(void);
 
 #endif /*  COMMANDS_H */
diff --git a/sw/airborne/modules/datalink/telemetry.c b/sw/airborne/modules/datalink/telemetry.c
index 65c8a934f2..f9a899a2b1 100644
--- a/sw/airborne/modules/datalink/telemetry.c
+++ b/sw/airborne/modules/datalink/telemetry.c
@@ -30,7 +30,9 @@
 #include "modules/datalink/telemetry_common.h"
 #include "modules/datalink/telemetry.h"
 #include "generated/periodic_telemetry.h"
+#if FIXEDWING_FIRMWARE || ROTORCRAFT_FIRMWARE || ROVER_FIRMWARE
 #include "autopilot.h"
+#endif
 
 
 /* Implement global structures from generated header.
@@ -74,6 +76,7 @@ int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id,
  */
 void telemetry_reporting_task(void)
 {
+#if FIXEDWING_FIRMWARE || ROTORCRAFT_FIRMWARE || ROVER_FIRMWARE
   static uint8_t boot = true;
 
   /* initialisation phase during boot */
@@ -96,6 +99,7 @@ void telemetry_reporting_task(void)
     periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
 #endif
   }
+#endif
 }
 
 
diff --git a/sw/airborne/modules/nav/common_flight_plan.h b/sw/airborne/modules/nav/common_flight_plan.h
index d19ae94a62..665b7aefd9 100644
--- a/sw/airborne/modules/nav/common_flight_plan.h
+++ b/sw/airborne/modules/nav/common_flight_plan.h
@@ -56,13 +56,6 @@ void nav_goto_block(uint8_t block_id);
 #define Goto(x) { goto label_ ## x; }
 #define Return(x) { nav_block=last_block; if (x==1) {nav_stage=0;} else {nav_stage=last_stage;} block_time=0;}
 
-#define And(x, y) ((x) && (y))
-#define Or(x, y) ((x) || (y))
-#define Min(x,y) (x < y ? x : y)
-#define Max(x,y) (x > y ? x : y)
-#define LessThan(_x, _y) ((_x) < (_y))
-#define MoreThan(_x, _y) ((_x) > (_y))
-
 /** Time in s since the entrance in the current block */
 #define NavBlockTime() (block_time)
 
diff --git a/sw/airborne/modules/px4_gimbal/px4_gimbal.c b/sw/airborne/modules/px4_gimbal/px4_gimbal.c
index d39b1619da..d5fd9cf8c8 100644
--- a/sw/airborne/modules/px4_gimbal/px4_gimbal.c
+++ b/sw/airborne/modules/px4_gimbal/px4_gimbal.c
@@ -12,6 +12,7 @@
 
 #include "modules/px4_gimbal/px4_gimbal.h"
 #include "modules/radio_control/radio_control.h"
+#include "generated/modules.h" // for RC channels definition
 
 #include "generated/airframe.h" // AC_ID is required
 #include "modules/actuators/actuators.h"
diff --git a/sw/airborne/modules/radio_control/spektrum.h b/sw/airborne/modules/radio_control/spektrum.h
index b6ac1b8597..c8b9f18e38 100644
--- a/sw/airborne/modules/radio_control/spektrum.h
+++ b/sw/airborne/modules/radio_control/spektrum.h
@@ -67,4 +67,8 @@ extern void spektrum_init(void);
 extern void spektrum_event(void);
 extern void spektrum_try_bind(void);
 
+#if USE_NPS
+extern void radio_control_feed(void);
+#endif
+
 #endif /* RADIO_CONTROL_SPEKTRUM_H */
diff --git a/sw/airborne/modules/switching/auto1_commands.c b/sw/airborne/modules/switching/auto1_commands.c
index 09cb997467..33bb71ecbe 100644
--- a/sw/airborne/modules/switching/auto1_commands.c
+++ b/sw/airborne/modules/switching/auto1_commands.c
@@ -24,6 +24,7 @@
 #include "generated/airframe.h"
 #include "autopilot.h"
 #include "modules/core/commands.h"
+#include "generated/modules.h" // for radio_control channels
 
 void periodic_auto1_commands(void)
 {
diff --git a/sw/include/std.h b/sw/include/std.h
index 6fecdcf727..6243cfd627 100644
--- a/sw/include/std.h
+++ b/sw/include/std.h
@@ -93,8 +93,12 @@ typedef uint8_t unit_t;
 #define MOfCm(_x) (((float)(_x))/100.)
 #define MOfMm(_x) (((float)(_x))/1000.)
 
+#define And(x, y) ((x) && (y))
+#define Or(x, y) ((x) || (y))
 #define Min(x,y) (x < y ? x : y)
 #define Max(x,y) (x > y ? x : y)
+#define LessThan(_x, _y) ((_x) < (_y))
+#define MoreThan(_x, _y) ((_x) > (_y))
 
 #ifndef ABS
 #define ABS(val) ((val) < 0 ? -(val) : (val))
diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
index 377f119d72..e3c2944bb3 100644
--- a/sw/tools/generators/gen_flight_plan.ml
+++ b/sw/tools/generators/gen_flight_plan.ml
@@ -1089,6 +1089,7 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
     List.map (fun w -> incr i; (name_of w, !i)) waypoints in
 
   (* print sectors *)
+  lprintf out "\n#ifndef FBW\n\n"; (* workaround to hide sector functions on FBW side *)
   let sectors_element = try ExtXml.child xml "sectors" with Not_found -> Xml.Element ("", [], []) in
   let sectors = List.filter (fun x -> String.lowercase_ascii (Xml.tag x) = "sector") (Xml.children sectors_element) in
   List.iter (fun x -> match ExtXml.attrib_opt x "type" with
@@ -1106,6 +1107,7 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
     with
         _ -> ()
   end;
+  lprintf out "\n#endif\n"; (* workaround to hide sector functions on FBW side *)
 
   (* start "C" part *)
   lprintf out "\n#ifdef NAV_C\n\n";