diff --git a/conf/airframes/ENAC/hybrid/falcon_v1.xml b/conf/airframes/ENAC/hybrid/falcon_v1.xml
index 4bc4ee633a..847eefa6fe 100644
--- a/conf/airframes/ENAC/hybrid/falcon_v1.xml
+++ b/conf/airframes/ENAC/hybrid/falcon_v1.xml
@@ -297,6 +297,8 @@
   <section name="NAV">
     <define name="NAV_CLIMB_VSPEED" value="1.5"/>
     <define name="NAV_DESCEND_VSPEED" value="-0.8"/>
+    <define name="NAV_HYBRID_MAX_AIRSPEED" value="GUIDANCE_INDI_MAX_AIRSPEED"/>
+    <define name="NAV_HYBRID_POS_GAIN" value="GUIDANCE_INDI_POS_GAIN"/>
   </section>
 
   <section name="AHRS" prefix="AHRS_">
diff --git a/conf/airframes/ENAC/hybrid/falcon_v2.xml b/conf/airframes/ENAC/hybrid/falcon_v2.xml
index 1465b04909..a7868a1967 100644
--- a/conf/airframes/ENAC/hybrid/falcon_v2.xml
+++ b/conf/airframes/ENAC/hybrid/falcon_v2.xml
@@ -237,9 +237,6 @@
     <define name="REF_RATE_Q" value="18.0"/>
     <define name="REF_RATE_R" value="11.0"/> 
 
-    <!--Maxium yaw rate, to avoid instability-->
-    <define name="MAX_R" value="60.0" unit="deg/s"/>
-
     <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
     <define name="FILT_CUTOFF" value="5.0"/>
 
@@ -253,6 +250,10 @@
     <define name="ADAPTIVE_MU" value="0.0001"/>
   </section>
 
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <define name="MAX_BANK" value="60" unit="deg"/>
+  </section>
+
   <section name="GUIDANCE_INDI">
     <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
     <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
@@ -272,6 +273,7 @@
     <define name="GUIDANCE_INDI_PITCH_OFFSET_GAIN" value="0.06"/>
     <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="1.5"/>
     <define name="GUIDANCE_INDI_LINE_GAIN" value="0.7"/>
+    <define name="FWD_SIDESLIP_GAIN" value="0.20"/>
     <!--Flap effectiveness on lift-->
     <define name="FE_LIFT_A_PITCH" value="0.00018"/>
     <define name="FE_LIFT_B_PITCH" value="0.00072"/>
@@ -279,15 +281,6 @@
     <define name="FE_LIFT_B_AS" value="0.00009"/>
   </section>
 
-  <!-- Gains for vertical navigation -->
-  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
-    <define name="HOVER_KP" value="174"/>
-    <define name="HOVER_KD" value="92"/>
-    <define name="HOVER_KI" value="72"/>
-    <define name="NOMINAL_HOVER_THROTTLE" value="0.4"/>
-    <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
-  </section>
-
   <section name="NAV">
     <define name="NAV_CLIMB_VSPEED" value="1.5"/>
     <define name="NAV_DESCEND_VSPEED" value="-1.5"/>
@@ -297,6 +290,7 @@
     <define name="NAV_LANDING_DESCEND_SPEED" value="-1.0"/>
     <define name="SURVEY_HYBRID_MAX_SWEEP_BACK" value="1"/>
     <define name="MISSION_ALT_PROXIMITY" value="4."/>
+    <define name="NAV_HYBRID_LINE_GAIN" value="0.8"/>
   </section>
 
   <section name="AHRS" prefix="AHRS_">
@@ -305,18 +299,9 @@
 
   <include href="conf/mag/toulouse_muret.xml"/>
 
-  <!-- Gains for horizontal navigation-->
-  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-    <define name="MAX_BANK" value="50" unit="deg"/>
-    <define name="PGAIN" value="100"/>
-    <define name="DGAIN" value="100"/>
-    <define name="IGAIN" value="10"/>
-  </section>
-
   <section name="MISC">
     <!--The Quadshot uses (when TRUE) a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
     <define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
-    <!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
     <define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
     <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
     <define name="COORDINATED_TURN_AIRSPEED" value="18.0"/>
@@ -324,11 +309,7 @@
     <define name="BARO_PERIODIC_FREQUENCY" value="50"/>
     <define name="USE_AIRSPEED" value="TRUE"/>
 
-    <define name="FWD_SIDESLIP_GAIN" value="0.20"/> <!-- most flight done with 0.3 -->
-
-    <define name="EFF_SCHED_USE_FUNCTION" value="TRUE"/>
-
-    <define name="ARRIVED_AT_WAYPOINT" value="4.0"/> <!-- For outdoor -->
+    <define name="ARRIVED_AT_WAYPOINT" value="5.0"/> <!-- For outdoor -->
     <define name="DEFAULT_CIRCLE_RADIUS" value="60"/> <!-- For outdoor -->
     <define name="CARROT" value="3.0"/>
   </section>
diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml
index 91ce962c4f..9ee07c24fb 100644
--- a/conf/airframes/examples/bebop2_opticflow.xml
+++ b/conf/airframes/examples/bebop2_opticflow.xml
@@ -9,6 +9,7 @@
   </description>
 
   <firmware name="rotorcraft">
+    <autopilot name="rotorcraft_control_loop"/>
 
     <target name="ap" board="bebop">
       <define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
index fafaaa6a24..d4ccefa03b 100644
--- a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
@@ -3,6 +3,8 @@
 <airframe name="bebop2_optitrack_visionfront">
 
   <firmware name="rotorcraft">
+    <autopilot name="rotorcraft_control_loop"/>
+
     <target name="ap" board="bebop2"/>
 
     <module name="telemetry" type="transparent_udp"/>
diff --git a/conf/airframes/tudelft/bebop_OF_hover.xml b/conf/airframes/tudelft/bebop_OF_hover.xml
index 1771c7ff95..3063038b4f 100644
--- a/conf/airframes/tudelft/bebop_OF_hover.xml
+++ b/conf/airframes/tudelft/bebop_OF_hover.xml
@@ -2,6 +2,8 @@
 
 <airframe name="tudelft_tb_bebop">
   <firmware name="rotorcraft">
+    <autopilot name="rotorcraft_control_loop"/>
+
     <target name="ap" board="bebop"/>
 	<define name="USE_SONAR" value="FALSE" />
 		
@@ -210,10 +212,10 @@
 	</section>
 
   <section name="AUTOPILOT">
-    <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
+    <!--define name="MODE_STARTUP" value="AP_MODE_NAV"/>
     <define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
     <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
-    <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
+    <define name="MODE_AUTO2" value="AP_MODE_NAV"/-->
     <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
   </section>
 
diff --git a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
index 5db8d9205b..8db3d7098e 100644
--- a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
+++ b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
@@ -6,6 +6,8 @@ ARDrone2 with optical_flow landing.
   </description>
 
   <firmware name="rotorcraft">
+    <autopilot name="rotorcraft_control_loop"/>
+
     <target name="ap" board="ardrone2">
       <configure name="USE_BARO_BOARD" value="FALSE"/>
     </target>
diff --git a/conf/autopilot/autopilot.dtd b/conf/autopilot/autopilot.dtd
index e1b62d4531..80f0e6d392 100644
--- a/conf/autopilot/autopilot.dtd
+++ b/conf/autopilot/autopilot.dtd
@@ -65,7 +65,8 @@ name CDATA #REQUIRED>
 
 <!ATTLIST call
 fun CDATA #REQUIRED
-cond CDATA #IMPLIED>
+cond CDATA #IMPLIED
+store CDATA #IMPLIED>
 
 <!ATTLIST call_block
 name CDATA #REQUIRED>
diff --git a/conf/autopilot/rotorcraft_autopilot.xml b/conf/autopilot/rotorcraft_autopilot.xml
index 361cdccb35..2f4ee474ae 100644
--- a/conf/autopilot/rotorcraft_autopilot.xml
+++ b/conf/autopilot/rotorcraft_autopilot.xml
@@ -10,7 +10,7 @@
       <include name="autopilot_rc_helpers.h"/>
       <include name="navigation.h"/>
       <include name="guidance.h"/>
-      <include name="stabilization/stabilization_attitude.h"/>
+      <include name="stabilization.h"/>
       <include name="modules/radio_control/radio_control.h"/>
       <include name="modules/gps/gps.h"/>
       <include name="modules/actuators/actuators.h"/>
@@ -29,25 +29,18 @@
     </settings>
 
     <control_block name="set_commands">
-      <call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
+      <call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
     </control_block>
 
-    <control_block name="attitude_loop">
-      <call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
-      <call fun="stabilization_attitude_run(autopilot_in_flight())"/>
+    <control_block name="run_attitude_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
     </control_block>
 
-    <control_block name="throttle_direct">
-      <call fun="guidance_v_read_rc()"/>
-      <call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/>
-      <call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
-    </control_block>
-
-    <control_block name="altitude_loop">
-      <call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/>
-      <call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
-      <call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
-      <!--call fun="SaturateThrottle(rc_values)"/-->
+    <control_block name="run_guidance_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
     </control_block>
 
     <exceptions>
@@ -58,14 +51,15 @@
     <mode name="ATTITUDE_DIRECT" shortname="ATT">
       <select cond="RCMode0()"/>
       <on_enter>
-        <call fun="stabilization_attitude_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call_block name="attitude_loop"/>
-        <call_block name="throttle_direct"/>
+        <call_block name="run_attitude_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="RCLost()" deroute="FAILSAFE"/>
@@ -74,15 +68,15 @@
     <mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
       <select cond="RCMode1()"/>
       <on_enter>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v_z_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call_block name="attitude_loop"/>
-        <call_block name="altitude_loop"/>
+        <call_block name="run_attitude_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="RCLost()" deroute="FAILSAFE"/>
@@ -91,18 +85,15 @@
     <mode name="NAV">
       <select cond="RCMode2() && DLModeNav()" exception="HOME"/>
       <on_enter>
-        <call fun="guidance_h_nav_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v_z_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -111,19 +102,15 @@
     <mode name="GUIDED">
       <select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
       <on_enter>
-        <call fun="guidance_h_hover_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
-        <call fun="guidance_v_guided_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_guided_run(autopilot_in_flight())"/>
-        <call fun="guidance_h_guided_run(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -131,18 +118,15 @@
 
     <mode name="HOME">
       <on_enter>
-        <call fun="guidance_h_nav_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v_z_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_home()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost()" deroute="FAILSAFE"/>
@@ -151,17 +135,15 @@
     <!-- Safe landing -->
     <mode name="FAILSAFE" shortname="FAIL">
       <on_enter>
-        <call fun="stabilization_attitude_set_failsafe_setpoint()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
         <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
-        <call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+        <call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
       </on_enter>
       <control>
-        <call fun="stabilization_attitude_set_failsafe_setpoint()"/>
-        <call fun="stabilization_attitude_run(autopilot_in_flight())"/>
-        <call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
-        <call fun="guidance_v_update_ref()"/>
-        <call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
-        <call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
+        <call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
+        <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+        <call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
@@ -172,9 +154,12 @@
       <select cond="$DEFAULT_MODE"/>
       <select cond="kill_switch_is_on()"/>
       <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
         <call fun="autopilot_set_in_flight(false)"/>
         <call fun="autopilot_set_motors_on(false)"/>
-        <call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
+        <!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
       </on_enter>
       <control>
         <call fun="SetCommands(commands_failsafe)"/>
diff --git a/conf/autopilot/rotorcraft_control_loop.xml b/conf/autopilot/rotorcraft_control_loop.xml
new file mode 100644
index 0000000000..d2d5484e54
--- /dev/null
+++ b/conf/autopilot/rotorcraft_control_loop.xml
@@ -0,0 +1,147 @@
+<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
+
+<autopilot name="Quadrotor Autopilot (Basic version)">
+
+  <state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
+
+    <includes>
+      <include name="generated/airframe.h"/>
+      <include name="autopilot.h"/>
+      <include name="autopilot_rc_helpers.h"/>
+      <include name="navigation.h"/>
+      <include name="guidance.h"/>
+      <include name="stabilization.h"/>
+      <include name="modules/radio_control/radio_control.h"/>
+      <include name="modules/gps/gps.h"/>
+      <include name="modules/actuators/actuators.h"/>
+      <define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
+      <define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
+    </includes>
+
+    <settings>
+      <dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
+      <dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
+    </settings>
+
+    <control_block name="set_commands">
+      <call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
+    </control_block>
+
+    <control_block name="run_attitude_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
+    </control_block>
+
+    <control_block name="run_guidance_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
+    </control_block>
+
+    <exceptions>
+      <exception cond="nav.too_far_from_home" deroute="HOME"/>
+      <exception cond="kill_switch_is_on()" deroute="KILL"/>
+    </exceptions>
+
+    <mode name="ATTITUDE_DIRECT" shortname="ATT">
+      <select cond="RCMode0()"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+      </control>
+      <control>
+        <call_block name="run_attitude_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="RCLost()" deroute="FAILSAFE"/>
+    </mode>
+
+    <mode name="MODULE">
+      <select cond="RCMode1()"/>
+      <on_enter>
+        <call fun="guidance_module_enter()"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+      </control>
+      <control>
+        <call fun="guidance_module_run(autopilot_in_flight())"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="RCLost()" deroute="FAILSAFE"/>
+    </mode>
+
+    <mode name="NAV">
+      <select cond="RCMode2()" exception="HOME"/>
+      <select cond="$DEFAULT_MODE"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+      </control>
+      <control>
+        <call_block name="run_guidance_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
+    </mode>
+
+    <mode name="HOME">
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_home()"/>
+      </control>
+      <control>
+        <call_block name="run_guidance_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="GpsIsLost()" deroute="FAILSAFE"/>
+    </mode>
+
+    <!-- Safe landing -->
+    <mode name="FAILSAFE" shortname="FAIL">
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+        <call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
+      </on_enter>
+      <control>
+        <call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
+        <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+        <call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
+    </mode>
+
+    <!-- Kill throttle mode -->
+    <mode name="KILL">
+      <select cond="kill_switch_is_on()"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
+        <call fun="autopilot_set_in_flight(false)"/>
+        <call fun="autopilot_set_motors_on(false)"/>
+        <!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
+      </on_enter>
+      <control>
+        <call fun="SetCommands(commands_failsafe)"/>
+      </control>
+    </mode>
+
+  </state_machine>
+
+</autopilot>
diff --git a/conf/autopilot/rotorcraft_oneloop.xml b/conf/autopilot/rotorcraft_oneloop.xml
index 1b8efa7822..93d32faf25 100644
--- a/conf/autopilot/rotorcraft_oneloop.xml
+++ b/conf/autopilot/rotorcraft_oneloop.xml
@@ -11,7 +11,7 @@
       <include name="navigation.h"/>
       <include name="guidance.h"/>
       <include name="oneloop/oneloop_andi.h"/>
-      <include name="stabilization/stabilization_attitude.h"/>
+      <include name="stabilization.h"/>
       <include name="modules/radio_control/radio_control.h"/>
       <include name="modules/gps/gps.h"/>
       <include name="modules/actuators/actuators.h"/>
@@ -31,7 +31,18 @@
     </settings>
 
     <control_block name="set_commands">
-      <call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
+      <call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
+    </control_block>
+
+    <control_block name="run_attitude_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
+    </control_block>
+
+    <control_block name="run_guidance_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
     </control_block>
 
     <exceptions>
@@ -43,13 +54,16 @@
       <select cond="RCMode0()"/>
       <select cond="$DEFAULT_MODE"/>
       <on_enter>
-        <call fun="stabilization_attitude_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="stabilization_attitude_run(autopilot_in_flight())"/>
+        <call_block name="run_attitude_control"/>
+        <call_block name="set_commands"/>
       </control>
       <exception cond="RCLost()" deroute="FAILSAFE"/>
     </mode>
@@ -57,57 +71,49 @@
     <mode name="NAV">
       <select cond="RCMode2() && DLModeNav()" exception="HOME"/>
       <on_enter>
-        <call fun="guidance_h_nav_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
-      </control>
-      <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
-    </mode>
-  
-    <mode name="GUIDED">
-      <select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
-      <on_enter>
-        <call fun="guidance_h_hover_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
-        <call fun="guidance_v_guided_enter()"/>
-      </on_enter>
-      <control freq="NAVIGATION_FREQUENCY">
-        <call fun="nav_periodic_task()"/>
-      </control>
-      <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_guided_run(autopilot_in_flight())"/>
-        <call fun="guidance_h_guided_run(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
-    </mode>  
+    </mode>
+
+    <mode name="GUIDED">
+      <select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+      </control>
+      <control>
+        <call_block name="run_guidance_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
+    </mode>
 
     <mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
       <select cond="RCMode1() && DLModeAZH()"/>
       <on_enter>
-        <call fun="guidance_h_hover_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
-        <call fun="guidance_v_guided_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_guided_run(autopilot_in_flight())"/>
-        <call fun="guidance_h_guided_run(autopilot_in_flight())"/>
+        <call_block name="run_attitude_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -129,16 +135,16 @@
 
     <mode name="HOME">
       <on_enter>
-        <call fun="guidance_h_nav_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_home()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
+        <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost()" deroute="FAILSAFE"/>
     </mode>
@@ -147,13 +153,15 @@
     <mode name="FAILSAFE" shortname="FAIL">
     <!-- Failsafe does not work needs to be fixed-->
       <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
         <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
-        <call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+        <call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
       </on_enter>
       <control>
-        <call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
-        <call fun="guidance_v_update_ref()"/>
-        <call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
+        <call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
+        <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+        <call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
@@ -163,9 +171,12 @@
     <mode name="KILL">
       <select cond="kill_switch_is_on()"/>
       <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
         <call fun="autopilot_set_in_flight(false)"/>
         <call fun="autopilot_set_motors_on(false)"/>
-        <call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
+        <!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
       </on_enter>
       <control>
         <call fun="SetCommands(commands_failsafe)"/>
diff --git a/conf/autopilot/rotorcraft_oneloop_with_backup.xml b/conf/autopilot/rotorcraft_oneloop_with_backup.xml
index e2f3ea02e0..10105bffdf 100644
--- a/conf/autopilot/rotorcraft_oneloop_with_backup.xml
+++ b/conf/autopilot/rotorcraft_oneloop_with_backup.xml
@@ -11,7 +11,7 @@
       <include name="navigation.h"/>
       <include name="guidance.h"/>
       <include name="oneloop/oneloop_andi.h"/>
-      <include name="stabilization/stabilization_attitude.h"/>
+      <include name="stabilization.h"/>
       <include name="modules/radio_control/radio_control.h"/>
       <include name="modules/gps/gps.h"/>
       <include name="modules/actuators/actuators.h"/>
@@ -31,24 +31,18 @@
     </settings>
 
     <control_block name="set_commands">
-      <call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
+      <call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
     </control_block>
 
-    <control_block name="attitude_loop">
-      <call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
-      <call fun="stabilization_attitude_run(autopilot_in_flight())"/>
+    <control_block name="run_attitude_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
     </control_block>
 
-    <control_block name="throttle_direct">
-      <call fun="guidance_v_read_rc()"/>
-      <call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/>
-      <call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
-    </control_block>
-
-    <control_block name="altitude_loop">
-      <call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/>
-      <call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
-      <call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
+    <control_block name="run_guidance_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
     </control_block>
 
     <exceptions>
@@ -59,14 +53,15 @@
     <mode name="ATTITUDE_DIRECT" shortname="ATT">
       <select cond="RCMode0()"/>
       <on_enter>
-        <call fun="stabilization_attitude_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call_block name="attitude_loop"/>
-        <call_block name="throttle_direct"/>
+        <call_block name="run_attitude_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="RCLost()" deroute="FAILSAFE"/>
@@ -75,60 +70,49 @@
     <mode name="NAV">
       <select cond="RCMode2() && DLModeNav()" exception="HOME"/>
       <on_enter>
-        <call fun="guidance_h_nav_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v_z_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
-    </mode>    
+    </mode>
 
     <mode name="GUIDED">
       <select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
       <on_enter>
-        <call fun="guidance_h_hover_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
-        <call fun="guidance_v_guided_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_guided_run(autopilot_in_flight())"/>
-        <call fun="guidance_h_guided_run(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
-    </mode>  
+    </mode>
 
     <mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
       <select cond="RCMode1() && DLModeAZH()"/>
       <on_enter>
-        <call fun="guidance_h_hover_enter()"/>
-        <call fun="stabilization_attitude_enter()"/>
-        <call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
-        <call fun="guidance_v_guided_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_guided_run(autopilot_in_flight())"/>
-        <call fun="guidance_h_guided_run(autopilot_in_flight())"/>
+        <call_block name="run_attitude_control"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -150,16 +134,16 @@
 
     <mode name="HOME">
       <on_enter>
-        <call fun="guidance_h_nav_enter()"/>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
       </on_enter>
       <control freq="NAVIGATION_FREQUENCY">
         <call fun="nav_home()"/>
       </control>
       <control>
-        <call fun="guidance_v_read_rc()"/>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
+        <call_block name="run_guidance_control"/>
+        <call_block name="set_commands"/>
       </control>
       <exception cond="GpsIsLost()" deroute="FAILSAFE"/>
     </mode>
@@ -168,13 +152,15 @@
     <mode name="FAILSAFE" shortname="FAIL">
     <!-- Failsafe does not work needs to be fixed-->
       <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
         <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
-        <call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+        <call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
       </on_enter>
       <control>
-        <call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
-        <call fun="guidance_v_update_ref()"/>
-        <call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
+        <call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
+        <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+        <call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
         <call_block name="set_commands"/>
       </control>
       <exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
@@ -184,9 +170,12 @@
     <mode name="KILL">
       <select cond="kill_switch_is_on()"/>
       <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
         <call fun="autopilot_set_in_flight(false)"/>
         <call fun="autopilot_set_motors_on(false)"/>
-        <call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
+        <!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
       </on_enter>
       <control>
         <call fun="SetCommands(commands_failsafe)"/>
diff --git a/conf/modules/ctrl_module_innerloop_demo.xml b/conf/modules/ctrl_module_innerloop_demo.xml
index 31d68cd2a7..9476a6e953 100644
--- a/conf/modules/ctrl_module_innerloop_demo.xml
+++ b/conf/modules/ctrl_module_innerloop_demo.xml
@@ -22,9 +22,11 @@
   <header>
     <file name="ctrl_module_innerloop_demo.h"/>
   </header>
+  <init fun="ctrl_module_init()"/>
 
   <makefile>
     <file name="ctrl_module_innerloop_demo.c"/>
+    <test/>
   </makefile>
 
 </module>
diff --git a/conf/modules/ctrl_module_outerloop_demo.xml b/conf/modules/ctrl_module_outerloop_demo.xml
index f6dec8d529..f0c8c72785 100644
--- a/conf/modules/ctrl_module_outerloop_demo.xml
+++ b/conf/modules/ctrl_module_outerloop_demo.xml
@@ -19,9 +19,11 @@
   <header>
     <file name="ctrl_module_outerloop_demo.h"/>
   </header>
+  <init fun="ctrl_module_init()"/>
 
   <makefile>
     <file name="ctrl_module_outerloop_demo.c"/>
+    <test firmware="rotorcraft"/>
   </makefile>
 
 </module>
diff --git a/conf/modules/ctrl_windtunnel.xml b/conf/modules/ctrl_windtunnel.xml
index be8fed3a2f..4e29b67567 100644
--- a/conf/modules/ctrl_windtunnel.xml
+++ b/conf/modules/ctrl_windtunnel.xml
@@ -37,13 +37,19 @@
   <header>
     <file name="ctrl_windtunnel.h"/>
   </header>
+  <init fun="ctrl_windtunnel_init()"/>
 
   <makefile>
     <file name="ctrl_windtunnel.c"/>
-  </makefile>
-  <makefile target="ap">
-    <define name="GUIDANCE_V_MODE_MODULE_SETTING" value="GUIDANCE_V_MODE_HOVER"/>
-    <define name="DGUIDANCE_H_MODE_MODULE_SETTING" value="GUIDANCE_H_MODE_MODULE"/>
+    <test>
+      <define name="CTRL_WINDTUNNEL_STEPTIME" value="8.0"/>
+      <define name="CTRL_WINDTUNNEL_THR_MIN" value="0"/>
+      <define name="CTRL_WINDTUNNEL_THR_MAX" value="9600"/>
+      <define name="CTRL_WINDTUNNEL_THR_STEP" value="1200"/>
+      <define name="CTRL_WINDTUNNEL_FLAP_MIN" value="0"/>
+      <define name="CTRL_WINDTUNNEL_FLAP_MAX" value="9600"/>
+      <define name="CTRL_WINDTUNNEL_FLAP_STEP" value="2400"/>
+    </test>
   </makefile>
 
 </module>
diff --git a/conf/modules/guidance_rotorcraft.xml b/conf/modules/guidance_rotorcraft.xml
index 647d852bb1..94a06dc8f0 100644
--- a/conf/modules/guidance_rotorcraft.xml
+++ b/conf/modules/guidance_rotorcraft.xml
@@ -7,7 +7,6 @@
       It provides:
       - horizontal guidance with reference
       - vertical guidance with reference and adaptive control
-      - flip mode
     </description>
   </doc>
   <settings target="ap|nps">
@@ -43,6 +42,8 @@
     <file name="guidance_v.c" dir="$(SRC_FIRMWARE)/guidance"/>
     <file name="guidance_v_ref.c" dir="$(SRC_FIRMWARE)/guidance"/>
     <file name="guidance_v_adapt.c" dir="$(SRC_FIRMWARE)/guidance"/>
-    <file name="guidance_flip.c" dir="$(SRC_FIRMWARE)/guidance"/>
+    <test firmware="rotorcraft">
+      <define name="PERIODIC_FREQUENCY" value="500"/>
+    </test>
   </makefile>
 </module>
diff --git a/conf/modules/hard_fault_recovery.xml b/conf/modules/hard_fault_recovery.xml
index 5a9cc841e6..2d1849fdac 100644
--- a/conf/modules/hard_fault_recovery.xml
+++ b/conf/modules/hard_fault_recovery.xml
@@ -22,7 +22,6 @@
     <file name="main_recovery.c" dir="firmwares/fixedwing"/>
     <test firmware="fixedwing">
       <define name="RADIO_CONTROL"/>
-      <define name="RADIO_MODE" value="0"/>
       <define name="DOWNLINK_DEVICE" value="uart1"/>
       <define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
       <define name="USE_UART1"/>
diff --git a/conf/modules/obstacle_avoidance.xml b/conf/modules/obstacle_avoidance.xml
index fd9259a196..cf6cb4d54f 100644
--- a/conf/modules/obstacle_avoidance.xml
+++ b/conf/modules/obstacle_avoidance.xml
@@ -63,12 +63,6 @@
   <makefile>
     <file name="obstacle_avoidance.c"/>
     <file name="guidance_OA.c"/>
-
-    <raw>
-        ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
-    ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
-    </raw>
-
   </makefile>
 
 </module>
diff --git a/conf/modules/optical_flow_hover.xml b/conf/modules/optical_flow_hover.xml
index 696c42c88a..dd5e3da7e7 100644
--- a/conf/modules/optical_flow_hover.xml
+++ b/conf/modules/optical_flow_hover.xml
@@ -90,6 +90,13 @@
   <makefile target="ap">
     <file name="optical_flow_hover.c" />
     <file name="optical_flow_functions.c" />
+    <test firmware="rotorcraft">
+      <define name="COV_WINDOW_SIZE" value="300"/>
+      <define name="GUIDANCE_H_MAX_BANK" value="0.5"/>
+      <define name="GUIDANCE_H_PGAIN" value="1"/>
+      <define name="GUIDANCE_H_IGAIN" value="1"/>
+      <define name="GUIDANCE_H_DGAIN" value="1"/>
+    </test>
   </makefile>
 
 </module>
diff --git a/conf/modules/optical_flow_landing.xml b/conf/modules/optical_flow_landing.xml
index 35ec927f8f..245dd4ae64 100644
--- a/conf/modules/optical_flow_landing.xml
+++ b/conf/modules/optical_flow_landing.xml
@@ -85,11 +85,13 @@
   <header>
     <file name="optical_flow_landing.h"/>
   </header>
+  <init fun="optical_flow_landing_init()"/>
 
   <makefile target="ap|nps">
     <file name="optical_flow_landing.c"/>
     <file name="pprz_algebra_float.c" dir="math"/>
     <file name="pprz_matrix_decomp_float.c" dir="math"/>
+    <test firmware="rotorcraft"/>
   </makefile>
 
 </module>
diff --git a/conf/modules/opticflow_hover.xml b/conf/modules/opticflow_hover.xml
index 047ea27daf..927666ca4f 100644
--- a/conf/modules/opticflow_hover.xml
+++ b/conf/modules/opticflow_hover.xml
@@ -23,7 +23,7 @@
   </doc>
 
   <settings>
-	<dl_settings NAME="Vision stabilization">
+    <dl_settings NAME="Vision stabilization">
       <!-- Satabilization loop parameters and gains -->
       <dl_settings name="vision_stab">
         <dl_setting var="opticflow_stab.phi_pgain" module="guidance_opticflow/guidance_opticflow_hover" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
@@ -43,13 +43,10 @@
   <header>
     <file name="guidance_opticflow_hover.h"/>
   </header>
-
+  <init fun="guidance_opticflow_hover_init()"/>
   <makefile>
     <file name="guidance_opticflow_hover.c"/>
-    <raw>
-      ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
-      ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
-    </raw>
+    <test firmware="rotorcraft"/>
   </makefile>
 
 </module>
diff --git a/conf/modules/stabilization_float_euler.xml b/conf/modules/stabilization_float_euler.xml
index 0a1c031464..3b4f40cea5 100644
--- a/conf/modules/stabilization_float_euler.xml
+++ b/conf/modules/stabilization_float_euler.xml
@@ -62,14 +62,11 @@
     <provides>commands</provides>
   </dep>
   <header>
-    <file name="stabilization_attitude.h"/>
     <file name="stabilization_attitude_euler_float.h"/>
   </header>
-  <init fun="stabilization_attitude_init()"/>
+  <init fun="stabilization_attitude_euler_float_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_attitude_euler_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <file name="stabilization_attitude_ref_euler_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_FLOAT"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_euler_float.h" type="string"/>
   </makefile>
 </module>
diff --git a/conf/modules/stabilization_float_quat.xml b/conf/modules/stabilization_float_quat.xml
index a2d6f432bf..9353e7a5b0 100644
--- a/conf/modules/stabilization_float_quat.xml
+++ b/conf/modules/stabilization_float_quat.xml
@@ -68,14 +68,11 @@
     <provides>commands</provides>
   </dep>
   <header>
-    <file name="stabilization_attitude.h"/>
     <file name="stabilization_attitude_quat_float.h"/>
   </header>
-  <init fun="stabilization_attitude_init()"/>
+  <init fun="stabilization_attitude_quat_float_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_attitude_quat_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <file name="stabilization_attitude_ref_quat_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_FLOAT"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_quat_float.h" type="string"/>
   </makefile>
 </module>
diff --git a/conf/modules/stabilization_heli_indi.xml b/conf/modules/stabilization_heli_indi.xml
index 23d955f806..109367e626 100644
--- a/conf/modules/stabilization_heli_indi.xml
+++ b/conf/modules/stabilization_heli_indi.xml
@@ -51,13 +51,11 @@
     <provides>commands</provides>
   </dep>
   <header>
-    <file name="stabilization_attitude.h"/>
+    <file name="stabilization_attitude_heli_indi.h"/>
   </header>
-  <init fun="stabilization_attitude_init()"/>
+  <init fun="stabilization_attitude_heli_indi_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_attitude_heli_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_heli_indi.h" type="string"/>
   </makefile>
 </module>
 
diff --git a/conf/modules/stabilization_indi.xml b/conf/modules/stabilization_indi.xml
index f18cc8bfba..d1884abf22 100644
--- a/conf/modules/stabilization_indi.xml
+++ b/conf/modules/stabilization_indi.xml
@@ -70,7 +70,7 @@
     <provides>commands</provides>
   </dep>
   <header>
-    <file name="stabilization_indi.h"/>
+    <file name="stabilization_attitude_quat_indi.h"/>
   </header>
   <init fun="stabilization_indi_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
@@ -80,8 +80,6 @@
     <configure name="INDI_NUM_ACT" default="4"/>
     <define name="INDI_OUTPUTS" value="$(INDI_OUTPUTS)"/>
     <define name="INDI_NUM_ACT" value="$(INDI_NUM_ACT)"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_quat_indi.h" type="string"/>
     <define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/>
   </makefile>
 </module>
diff --git a/conf/modules/stabilization_int_euler.xml b/conf/modules/stabilization_int_euler.xml
index d032ace351..f222ef5239 100644
--- a/conf/modules/stabilization_int_euler.xml
+++ b/conf/modules/stabilization_int_euler.xml
@@ -59,14 +59,11 @@
     <provides>commands</provides>
   </dep>
   <header>
-    <file name="stabilization_attitude.h"/>
     <file name="stabilization_attitude_euler_int.h"/>
   </header>
-  <init fun="stabilization_attitude_init()"/>
+  <init fun="stabilization_attitude_euler_int_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_attitude_euler_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <file name="stabilization_attitude_ref_euler_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_euler_int.h" type="string"/>
   </makefile>
 </module>
diff --git a/conf/modules/stabilization_int_quat.xml b/conf/modules/stabilization_int_quat.xml
index 601f9b35ee..4b22a082ca 100644
--- a/conf/modules/stabilization_int_quat.xml
+++ b/conf/modules/stabilization_int_quat.xml
@@ -65,14 +65,11 @@
     <provides>commands</provides>
   </dep>
   <header>
-    <file name="stabilization_attitude.h"/>
     <file name="stabilization_attitude_quat_int.h"/>
   </header>
-  <init fun="stabilization_attitude_init()"/>
+  <init fun="stabilization_attitude_quat_int_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_attitude_quat_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <file name="stabilization_attitude_ref_quat_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_quat_int.h" type="string"/>
   </makefile>
 </module>
diff --git a/conf/modules/stabilization_oneloop.xml b/conf/modules/stabilization_oneloop.xml
index dd9440b76c..4666fe7eea 100644
--- a/conf/modules/stabilization_oneloop.xml
+++ b/conf/modules/stabilization_oneloop.xml
@@ -15,7 +15,5 @@
   </header>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_oneloop.c" dir="$(SRC_FIRMWARE)/stabilization"/>
-    <!-- <define name="STABILIZATION_ATTITUDE_TYPE_INT"/> -->
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_oneloop.h" type="string"/>
   </makefile>
 </module>
diff --git a/conf/modules/stabilization_passthrough.xml b/conf/modules/stabilization_passthrough.xml
index b2ccfa8aec..efc357da05 100644
--- a/conf/modules/stabilization_passthrough.xml
+++ b/conf/modules/stabilization_passthrough.xml
@@ -13,12 +13,9 @@
   <header>
     <file name="stabilization_attitude_passthrough.h"/>
   </header>
-  <init fun="stabilization_attitude_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_attitude_passthrough.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <define name="STABILIZATION_ATTITUDE_NO_REF"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
-    <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_passthrough.h" type="string"/>
   </makefile>
 </module>
 
diff --git a/conf/modules/stabilization_rotorcraft.xml b/conf/modules/stabilization_rotorcraft.xml
index 6d437fc25d..fe033cb4fb 100644
--- a/conf/modules/stabilization_rotorcraft.xml
+++ b/conf/modules/stabilization_rotorcraft.xml
@@ -20,14 +20,17 @@
   </dep>
   <header>
     <file name="stabilization.h" dir="firmwares/rotorcraft"/>
-    <file name="stabilization_none.h"/>
+    <file name="stabilization_direct.h"/>
   </header>
   <init fun="stabilization_init()"/>
-  <init fun="stabilization_none_init()"/>
+  <init fun="stabilization_direct_init()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization.c" dir="$(SRC_FIRMWARE)"/>
-    <file name="stabilization_none.c" dir="$(SRC_FIRMWARE)/stabilization"/>
+    <file name="stabilization_direct.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <file name="stabilization_attitude_rc_setpoint.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <file name="stabilization_attitude_quat_transformations.c" dir="$(SRC_FIRMWARE)/stabilization"/>
+    <test firmware="rotorcraft">
+      <define name="PERIODIC_FREQUENCY" value="500"/>
+    </test>
   </makefile>
 </module>
diff --git a/conf/modules/vertical_ctrl_module_demo.xml b/conf/modules/vertical_ctrl_module_demo.xml
index d5bbe00509..146b632622 100644
--- a/conf/modules/vertical_ctrl_module_demo.xml
+++ b/conf/modules/vertical_ctrl_module_demo.xml
@@ -26,9 +26,11 @@
   <header>
     <file name="vertical_ctrl_module_demo.h"/>
   </header>
+  <init fun="vertical_ctrl_module_init()"/>
 
   <makefile>
     <file name="vertical_ctrl_module_demo.c"/>
+    <test firmware="rotorcraft"/>
   </makefile>
 
 </module>
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 39e5a8fca2..32a08503a4 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -469,7 +469,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_optitrack.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
    gui_color="red"
   />
   <aircraft
diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c
index edc1e53780..d1eae6e314 100644
--- a/sw/airborne/boards/bebop/actuators.c
+++ b/sw/airborne/boards/bebop/actuators.c
@@ -39,10 +39,10 @@
 static void send_bebop_actuators(struct transport_tx *trans, struct link_device *dev)
 {
   pprz_msg_send_BEBOP_ACTUATORS(trans, dev, AC_ID,
-                                &stabilization_cmd[COMMAND_THRUST],
-                                &stabilization_cmd[COMMAND_ROLL],
-                                &stabilization_cmd[COMMAND_PITCH],
-                                &stabilization_cmd[COMMAND_YAW],
+                                &stabilization.cmd[COMMAND_THRUST],
+                                &stabilization.cmd[COMMAND_ROLL],
+                                &stabilization.cmd[COMMAND_PITCH],
+                                &stabilization.cmd[COMMAND_YAW],
                                 &actuators_bebop.rpm_ref[0],
                                 &actuators_bebop.rpm_ref[1],
                                 &actuators_bebop.rpm_ref[2],
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
index 87bd7e7c09..93cf086fcf 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
@@ -92,7 +92,7 @@ bool WEAK autopilot_ground_detection(void) {
 bool WEAK autopilot_in_flight_end_detection(bool motors_on UNUSED) {
   if (autopilot_in_flight_counter > 0) {
     /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
-    if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
+    if ((stabilization.cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
         (fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
         (fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
       autopilot_in_flight_counter--;
@@ -229,10 +229,10 @@ static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *d
 static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
 {
   pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
-                               &stabilization_cmd[COMMAND_ROLL],
-                               &stabilization_cmd[COMMAND_PITCH],
-                               &stabilization_cmd[COMMAND_YAW],
-                               &stabilization_cmd[COMMAND_THRUST]);
+                               &stabilization.cmd[COMMAND_ROLL],
+                               &stabilization.cmd[COMMAND_PITCH],
+                               &stabilization.cmd[COMMAND_YAW],
+                               &stabilization.cmd[COMMAND_THRUST]);
 }
 #else
 static void send_rotorcraft_cmd(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED) {}
@@ -242,7 +242,9 @@ static void send_rotorcraft_cmd(struct transport_tx *trans UNUSED, struct link_d
 void autopilot_firmware_init(void)
 {
   autopilot_in_flight_counter = 0;
+#ifdef MODE_AUTO2
   autopilot_mode_auto2 = MODE_AUTO2;
+#endif
 
   // register messages
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
@@ -296,7 +298,7 @@ void autopilot_check_in_flight(bool motors_on)
       /* if thrust above min threshold, assume in_flight.
        * Don't check for velocity and acceleration above threshold here...
        */
-      if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
+      if (stabilization.cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
         autopilot_in_flight_counter++;
         if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) {
           autopilot.in_flight = true;
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_generated.c b/sw/airborne/firmwares/rotorcraft/autopilot_generated.c
index 80abeb0057..96a52212e4 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_generated.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_generated.c
@@ -125,9 +125,6 @@ void autopilot_generated_on_rc_frame(void)
 //      SetCommandsFromRC(commands, radio_control.values);
 //    }
 //#endif
-//
-//    guidance_v_read_rc();
-//    guidance_h_read_rc(autopilot.in_flight);
 //  }
 
 }
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c
index b33fcd8458..792bcbc982 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c
@@ -32,6 +32,7 @@
 #include "state.h"
 #include "pprzlink/dl_protocol.h"
 
+#ifdef AP_MODE_GUIDED
 
 bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
 {
@@ -162,3 +163,58 @@ void autopilot_guided_parse_GUIDED(uint8_t *buf) {
       DL_GUIDED_SETPOINT_NED_yaw(buf));
 }
 
+#else
+
+bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
+{
+  (void) x;
+  (void) y;
+  (void) z;
+  (void) heading;
+  return false;
+}
+
+bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
+{
+  (void) dx;
+  (void) dy;
+  (void) dz;
+  (void) dyaw;
+  return false;
+}
+
+bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
+{
+  (void) dx;
+  (void) dy;
+  (void) dz;
+  (void) dyaw;
+  return false;
+}
+
+bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
+{
+  (void) vx;
+  (void) vy;
+  (void) vz;
+  (void) heading;
+  return false;
+}
+
+void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
+{
+  (void) flags;
+  (void) x;
+  (void) y;
+  (void) z;
+  (void) yaw;
+}
+
+/** Parse GUIDED_SETPOINT_NED messages from datalink
+ */
+void autopilot_guided_parse_GUIDED(uint8_t *buf) {
+  (void) buf;
+}
+
+#endif
+
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
index 6b6ff3dcea..ab847587c7 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
@@ -53,6 +53,10 @@
   (radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
    radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
 
+// macros with pointer to radio control struct
+#define THROTTLE_STICK_DOWN_FROM_RC(_rc)                                \
+  (_rc->values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_THRESHOLD)
+
 /** RC mode switch position helper
  *  switch positions threshold are evenly spaced
  *
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.c b/sw/airborne/firmwares/rotorcraft/autopilot_static.c
index e853189e36..c031f8ee21 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_static.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.c
@@ -39,7 +39,6 @@
 #include "firmwares/rotorcraft/guidance.h"
 
 #include "firmwares/rotorcraft/stabilization.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 
 #if USE_STABILIZATION_RATE
@@ -158,14 +157,35 @@ void autopilot_static_periodic(void)
    * If in FAILSAFE mode, run normal loops with failsafe attitude and
    * downwards velocity setpoints.
    */
-  if (autopilot.mode == AP_MODE_KILL) {
-    SetCommands(commands_failsafe);
-  } else {
-    guidance_v_run(autopilot_in_flight());
-    guidance_h_run(autopilot_in_flight());
-    SetRotorcraftCommands(stabilization_cmd, autopilot.in_flight, autopilot.motors_on);
+  struct StabilizationSetpoint stab_sp;
+  struct ThrustSetpoint thrust_sp;
+  switch (autopilot.mode) {
+    case AP_MODE_FAILSAFE:
+#ifndef KILL_AS_FAILSAFE
+      thrust_sp = guidance_v_run(autopilot_in_flight());
+      stab_sp = stabilization_get_failsafe_sp();
+      stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd);
+      SetRotorcraftCommands(stabilization.cmd, autopilot.in_flight, autopilot.motors_on);
+      break;
+#endif
+    case AP_MODE_KILL:
+      SetCommands(commands_failsafe);
+      break;
+    default:
+      thrust_sp = guidance_v_run(autopilot_in_flight());
+      if (guidance_h.mode != GUIDANCE_H_MODE_NONE) {
+        stab_sp = guidance_h_run(autopilot_in_flight());
+      } else {
+        stab_sp = stabilization.rc_sp;
+      }
+      stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd);
+      // TODO maybe add RC limiter here as an option ?
+      SetRotorcraftCommands(stabilization.cmd, autopilot.in_flight, autopilot.motors_on);
+      break;
   }
+#ifdef COMMAND_THRUST
   autopilot.throttle = commands[COMMAND_THRUST];
+#endif
 
 }
 
@@ -180,7 +200,7 @@ void autopilot_static_SetModeHandler(float mode)
     autopilot_static_set_mode(mode);
   } else {
     if (radio_control.status != RC_OK &&
-        (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
+        (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED)) {
       // without RC, only nav-like modes are accessible
       autopilot_static_set_mode(mode);
     }
@@ -196,22 +216,26 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
     switch (new_autopilot_mode) {
       case AP_MODE_FAILSAFE:
 #ifndef KILL_AS_FAILSAFE
-        stabilization_attitude_set_failsafe_setpoint();
-        guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
+        stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
+            STABILIZATION_ATT_SUBMODE_HEADING);
         break;
 #endif
       case AP_MODE_KILL:
         autopilot_set_in_flight(false);
-        guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
+        stabilization_mode_changed(STABILIZATION_MODE_NONE, 0);
         break;
       case AP_MODE_RC_DIRECT:
-        guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
+        stabilization_mode_changed(STABILIZATION_MODE_DIRECT, 0);
         break;
       case AP_MODE_RATE_RC_CLIMB:
       case AP_MODE_RATE_DIRECT:
       case AP_MODE_RATE_Z_HOLD:
 #if USE_STABILIZATION_RATE
-        guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
+        stabilization_mode_changed(STABILIZATION_MODE_RATE, 0);
 #else
         return;
 #endif
@@ -220,33 +244,37 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
       case AP_MODE_ATTITUDE_DIRECT:
       case AP_MODE_ATTITUDE_CLIMB:
       case AP_MODE_ATTITUDE_Z_HOLD:
-        guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
+        stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
+            STABILIZATION_ATT_SUBMODE_HEADING);
         break;
       case AP_MODE_FORWARD:
-        guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
+        stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
+            STABILIZATION_ATT_SUBMODE_FORWARD);
         break;
       case AP_MODE_CARE_FREE_DIRECT:
-        guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
+        stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
+            STABILIZATION_ATT_SUBMODE_CARE_FREE);
         break;
       case AP_MODE_HOVER_DIRECT:
       case AP_MODE_HOVER_CLIMB:
       case AP_MODE_HOVER_Z_HOLD:
         guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
+        stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
+            STABILIZATION_ATT_SUBMODE_HEADING);
         break;
       case AP_MODE_HOME:
       case AP_MODE_NAV:
         guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
-        break;
-      case AP_MODE_MODULE:
-#ifdef GUIDANCE_H_MODE_MODULE_SETTING
-        guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
-#endif
-        break;
-      case AP_MODE_FLIP:
-        guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP);
+        stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
+            STABILIZATION_ATT_SUBMODE_HEADING); // TODO check
         break;
       case AP_MODE_GUIDED:
         guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED);
+        stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
+            STABILIZATION_ATT_SUBMODE_HEADING);
         break;
       default:
         break;
@@ -260,8 +288,10 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
         break;
 #endif
       case AP_MODE_KILL:
-        autopilot_set_motors_on(FALSE);
-        stabilization_cmd[COMMAND_THRUST] = 0;
+        autopilot_set_motors_on(false);
+#ifdef COMMAND_THRUST
+        stabilization.cmd[COMMAND_THRUST] = 0; // FIXME maybe not needed ?
+#endif
         guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
         break;
       case AP_MODE_RC_DIRECT:
@@ -289,14 +319,6 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
       case AP_MODE_NAV:
         guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
         break;
-      case AP_MODE_MODULE:
-#ifdef GUIDANCE_V_MODE_MODULE_SETTING
-        guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
-#endif
-        break;
-      case AP_MODE_FLIP:
-        guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP);
-        break;
       case AP_MODE_GUIDED:
         guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED);
         break;
@@ -374,9 +396,6 @@ void autopilot_static_on_rc_frame(void)
       SetCommandsFromRC(commands, radio_control.values);
     }
 #endif
-
-    guidance_v_read_rc();
-    guidance_h_read_rc(autopilot_in_flight());
   }
 
 }
@@ -393,8 +412,6 @@ void autopilot_failsafe_checks(void)
       autopilot_get_mode() != AP_MODE_HOME &&
       autopilot_get_mode() != AP_MODE_FAILSAFE &&
       autopilot_get_mode() != AP_MODE_NAV &&
-      autopilot_get_mode() != AP_MODE_MODULE &&
-      autopilot_get_mode() != AP_MODE_FLIP &&
       autopilot_get_mode() != AP_MODE_GUIDED) {
     autopilot_set_mode(RC_LOST_MODE);
   }
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.h b/sw/airborne/firmwares/rotorcraft/autopilot_static.h
index b89c243314..7b006e5d62 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_static.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.h
@@ -49,8 +49,6 @@
 #define AP_MODE_RC_DIRECT         14  // Safety Pilot Direct Commands for helicopter direct control
 #define AP_MODE_CARE_FREE_DIRECT  15
 #define AP_MODE_FORWARD           16
-#define AP_MODE_MODULE            17
-#define AP_MODE_FLIP              18
 #define AP_MODE_GUIDED            19
 
 
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_utils.c b/sw/airborne/firmwares/rotorcraft/autopilot_utils.c
index 457af67cc5..640fed2a23 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_utils.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_utils.c
@@ -35,6 +35,7 @@
 /** Display descent speed in failsafe mode if needed */
 PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
 
+#if (defined MODE_MANUAL) && (defined MODE_AUTO1)
 
 #if defined RADIO_MODE_2x3
 
@@ -113,6 +114,7 @@ uint8_t ap_mode_of_two_switches(void)
 }
 #endif
 
+#endif // MODE_MANUAL && MODE_AUTO1
 
 /** Set Rotorcraft commands.
  *  Limit thrust and/or yaw depending of the in_flight
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c
index 57bf409771..4a28c64546 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c
@@ -77,10 +77,11 @@ void guidance_flip_run(void)
     case 0:
       flip_cmd_earth.x = 0;
       flip_cmd_earth.y = 0;
-      stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
-                                             heading_save);
-      stabilization_attitude_run(autopilot_in_flight());
-      stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
+      // FIXME maybe better remove the flip guidance
+      //stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
+      //                                       heading_save);
+      //stabilization_attitude_run(autopilot_in_flight());
+      stabilization.cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
       timer_save = 0;
 
       if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
@@ -89,10 +90,10 @@ void guidance_flip_run(void)
       break;
 
     case 1:
-      stabilization_cmd[COMMAND_ROLL]   = 9000; // Rolling command
-      stabilization_cmd[COMMAND_PITCH]  = 0;
-      stabilization_cmd[COMMAND_YAW]    = 0;
-      stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
+      stabilization.cmd[COMMAND_ROLL]   = 9000; // Rolling command
+      stabilization.cmd[COMMAND_PITCH]  = 0;
+      stabilization.cmd[COMMAND_YAW]    = 0;
+      stabilization.cmd[COMMAND_THRUST] = 1000; //Min thrust?
 
       if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
         flip_state++;
@@ -100,10 +101,10 @@ void guidance_flip_run(void)
       break;
 
     case 2:
-      stabilization_cmd[COMMAND_ROLL]   = 0;
-      stabilization_cmd[COMMAND_PITCH]  = 0;
-      stabilization_cmd[COMMAND_YAW]    = 0;
-      stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
+      stabilization.cmd[COMMAND_ROLL]   = 0;
+      stabilization.cmd[COMMAND_PITCH]  = 0;
+      stabilization.cmd[COMMAND_YAW]    = 0;
+      stabilization.cmd[COMMAND_THRUST] = 1000; //Min thrust?
 
       if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
         timer_save = timer;
@@ -118,7 +119,7 @@ void guidance_flip_run(void)
                                              heading_save);
       stabilization_attitude_run(autopilot_in_flight());
 
-      stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
+      stabilization.cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
 
       if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
         flip_state++;
@@ -134,10 +135,10 @@ void guidance_flip_run(void)
       timer_save = 0;
       flip_state = 0;
 
-      stabilization_cmd[COMMAND_ROLL]   = 0;
-      stabilization_cmd[COMMAND_PITCH]  = 0;
-      stabilization_cmd[COMMAND_YAW]    = 0;
-      stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
+      stabilization.cmd[COMMAND_ROLL]   = 0;
+      stabilization.cmd[COMMAND_PITCH]  = 0;
+      stabilization.cmd[COMMAND_YAW]    = 0;
+      stabilization.cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
       break;
   }
 #else
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 5e0dcc14b2..803556bc59 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -26,18 +26,13 @@
 
 #include "generated/airframe.h"
 
-#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
 #include "firmwares/rotorcraft/guidance/guidance_h.h"
-#include "firmwares/rotorcraft/guidance/guidance_flip.h"
 #include "firmwares/rotorcraft/guidance/guidance_module.h"
 #include "firmwares/rotorcraft/stabilization.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "firmwares/rotorcraft/navigation.h"
+#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
 #include "modules/radio_control/radio_control.h"
-
-#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "modules/core/abi.h"
 
 /* for guidance_v.thrust_coeff */
 #include "firmwares/rotorcraft/guidance/guidance_v.h"
@@ -49,16 +44,19 @@ PRINT_CONFIG_VAR(GUIDANCE_H_USE_SPEED_REF)
 
 struct HorizontalGuidance guidance_h;
 
-int32_t transition_percentage;
-
 /** horizontal guidance command.
  * In north/east with #INT32_ANGLE_FRAC
  */
 struct StabilizationSetpoint guidance_h_cmd;
 
 static void guidance_h_update_reference(void);
-static inline void transition_run(bool to_forward);
-static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight);
+
+#ifndef GUIDANCE_H_RC_ID
+#define GUIDANCE_H_RC_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_H_RC_ID)
+static abi_event rc_ev;
+static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc);
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -83,52 +81,31 @@ static void send_href(struct transport_tx *trans, struct link_device *dev)
                                    &guidance_h.ref.accel.y);
 }
 
-#if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
-static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
-{
-  pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
-                                      &radio_control.values[RADIO_ROLL],
-                                      &radio_control.values[RADIO_PITCH],
-                                      &radio_control.values[RADIO_YAW],
-                                      &stabilization_cmd[COMMAND_ROLL],
-                                      &stabilization_cmd[COMMAND_PITCH],
-                                      &stabilization_cmd[COMMAND_YAW],
-                                      &stabilization_cmd[COMMAND_THRUST],
-                                      &(stateGetNedToBodyEulers_i()->phi),
-                                      &(stateGetNedToBodyEulers_i()->theta),
-                                      &(stateGetNedToBodyEulers_i()->psi));
-}
-#else
-static void send_tune_hover(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED) {}
-#endif
-
 #endif
 
 void guidance_h_init(void)
 {
 
-  guidance_h.mode = GUIDANCE_H_MODE_KILL;
+  guidance_h.mode = GUIDANCE_H_MODE_NONE;
   guidance_h.use_ref = GUIDANCE_H_USE_REF;
 
   INT_VECT2_ZERO(guidance_h.sp.pos);
-  FLOAT_EULERS_ZERO(guidance_h.rc_sp);
-  guidance_h.sp.heading = 0.0;
-  guidance_h.sp.heading_rate = 0.0;
+  guidance_h.sp.heading = 0.f;
+  guidance_h.sp.heading_rate = 0.f;
   guidance_h.sp.h_mask = GUIDANCE_H_SP_POS;
   guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW;
-  transition_percentage = 0;
-  transition_theta_offset = 0;
+  INT_VECT2_ZERO(guidance_h.rc_sp.vect);
+  guidance_h.rc_sp.heading = 0.f;
+  guidance_h.rc_sp.last_ts = 0.f;
 
   gh_ref_init();
 
-#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
-  guidance_h_module_init();
-#endif
+  // bind ABI messages
+  AbiBindMsgRADIO_CONTROL(GUIDANCE_H_RC_ID, &rc_ev, rc_cb);
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
-  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
 #endif
 
 }
@@ -155,63 +132,13 @@ void guidance_h_mode_changed(uint8_t new_mode)
   }
 
   switch (new_mode) {
-    case GUIDANCE_H_MODE_RC_DIRECT:
-      stabilization_none_enter();
-      break;
-
-#if USE_STABILIZATION_RATE
-    case GUIDANCE_H_MODE_RATE:
-      stabilization_rate_enter();
-      break;
-#endif
-
-    case GUIDANCE_H_MODE_CARE_FREE:
-      stabilization_attitude_reset_care_free_heading();
-      /* Falls through. */
-    case GUIDANCE_H_MODE_FORWARD:
-    case GUIDANCE_H_MODE_ATTITUDE:
-#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
-      /* reset attitude stabilization if previous mode was not using it */
-      if (guidance_h.mode == GUIDANCE_H_MODE_KILL ||
-          guidance_h.mode == GUIDANCE_H_MODE_RATE ||
-          guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT)
-#endif
-        stabilization_attitude_enter();
-      break;
-
-    case GUIDANCE_H_MODE_GUIDED:
     case GUIDANCE_H_MODE_HOVER:
+    case GUIDANCE_H_MODE_GUIDED:
       guidance_h_hover_enter();
-#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
-      /* reset attitude stabilization if previous mode was not using it */
-      if (guidance_h.mode == GUIDANCE_H_MODE_KILL ||
-          guidance_h.mode == GUIDANCE_H_MODE_RATE ||
-          guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT)
-#endif
-        stabilization_attitude_enter();
       break;
-
-#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
-    case GUIDANCE_H_MODE_MODULE:
-      guidance_h_module_enter();
-      break;
-#endif
-
     case GUIDANCE_H_MODE_NAV:
       guidance_h_nav_enter();
-#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
-      /* reset attitude stabilization if previous mode was not using it */
-      if (guidance_h.mode == GUIDANCE_H_MODE_KILL ||
-          guidance_h.mode == GUIDANCE_H_MODE_RATE ||
-          guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT)
-#endif
-        stabilization_attitude_enter();
       break;
-
-    case GUIDANCE_H_MODE_FLIP:
-      guidance_flip_enter();
-      break;
-
     default:
       break;
   }
@@ -220,126 +147,126 @@ void guidance_h_mode_changed(uint8_t new_mode)
 
 }
 
+// If not defined, use attitude max yaw setpoint (or 60 deg/s) by default
+#ifndef GUIDANCE_H_SP_MAX_R
+#ifdef STABILIZATION_ATTITUDE_SP_MAX_R
+#define GUIDANCE_H_SP_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
+#else
+#define GUIDANCE_H_SP_MAX_R 60.f
+#endif
+#endif
 
-void guidance_h_read_rc(bool  in_flight)
+#ifndef GUIDANCE_H_DEADBAND_R
+#define GUIDANCE_H_DEADBAND_R 200
+#endif
+
+#define YAW_DEADBAND_EXCEEDED(_rc)                               \
+  (rc->values[RADIO_YAW] >  GUIDANCE_H_DEADBAND_R || \
+   rc->values[RADIO_YAW] < -GUIDANCE_H_DEADBAND_R)
+
+static void read_rc_setpoint_heading(struct HorizontalGuidanceRCInput *rc_sp, bool in_flight, struct RadioControl *rc)
 {
+  if (in_flight) {
+    /* calculate dt for yaw integration */
+    float dt = get_sys_time_float() - rc_sp->last_ts;
+    /* make sure nothing drastically weird happens, bound dt to 0.5sec */
+    Bound(dt, 0, 0.5);
 
+    /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
+    if (YAW_DEADBAND_EXCEEDED(rc) && !THROTTLE_STICK_DOWN_FROM_RC(rc)) {
+      float heading_rate = (float) rc->values[RADIO_YAW] * GUIDANCE_H_SP_MAX_R / MAX_PPRZ;
+      rc_sp->heading += heading_rate * dt;
+      FLOAT_ANGLE_NORMALIZE(rc_sp->heading);
+    }
+  } else { /* if not flying, use current yaw as setpoint */
+    rc_sp->heading = stateGetNedToBodyEulers_f()->psi;
+  }
+  /* update timestamp for dt calculation */
+  rc_sp->last_ts = get_sys_time_float();
+}
+
+/// read speed setpoint from RC
+static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight, struct RadioControl *rc)
+{
+  if (in_flight) {
+    // negative pitch is forward
+    int64_t rc_x = -rc->values[RADIO_PITCH];
+    int64_t rc_y = rc->values[RADIO_ROLL];
+    DeadBand(rc_x, MAX_PPRZ / 20);
+    DeadBand(rc_y, MAX_PPRZ / 20);
+
+    // convert input from MAX_PPRZ range to SPEED_BFP
+    int32_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED);
+    /// @todo calc proper scale while making sure a division by zero can't occur
+    //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y);
+    //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y);
+    rc_x = rc_x * max_speed / MAX_PPRZ;
+    rc_y = rc_y * max_speed / MAX_PPRZ;
+
+    /* Rotate from body to NED frame by negative psi angle */
+    int32_t psi = -stateGetNedToBodyEulers_i()->psi;
+    int32_t s_psi, c_psi;
+    PPRZ_ITRIG_SIN(s_psi, psi);
+    PPRZ_ITRIG_COS(c_psi, psi);
+    speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC);
+    speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC);
+  } else {
+    speed_sp->x = 0;
+    speed_sp->y = 0;
+  }
+}
+
+static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
+{
   switch (guidance_h.mode) {
 
-    case GUIDANCE_H_MODE_RC_DIRECT:
-      stabilization_none_read_rc();
-      break;
-
-#if USE_STABILIZATION_RATE
-    case GUIDANCE_H_MODE_RATE:
-#if SWITCH_STICKS_FOR_RATE_CONTROL
-      stabilization_rate_read_rc_switched_sticks();
-#else
-      stabilization_rate_read_rc();
-#endif
-      break;
-#endif
-
-    case GUIDANCE_H_MODE_CARE_FREE:
-      stabilization_attitude_read_rc(in_flight, TRUE, FALSE);
-      break;
-    case GUIDANCE_H_MODE_FORWARD:
-      stabilization_attitude_read_rc(in_flight, FALSE, TRUE);
-      break;
-    case GUIDANCE_H_MODE_ATTITUDE:
-      stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
-      break;
     case GUIDANCE_H_MODE_HOVER:
-      stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
+      read_rc_setpoint_heading(&guidance_h.rc_sp, autopilot_in_flight(), rc);
+      read_rc_setpoint_speed_i(&guidance_h.rc_sp.vect, autopilot_in_flight(), rc);
 #if GUIDANCE_H_USE_SPEED_REF
-      read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
       /* enable x,y velocity setpoints */
+      guidance_h.sp.speed = guidance_h.rc_sp.vect;
       guidance_h.sp.h_mask = GUIDANCE_H_SP_SPEED;
 #endif
       break;
-
-#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
-    case GUIDANCE_H_MODE_MODULE:
-      guidance_h_module_read_rc();
-      break;
-#endif
-
     case GUIDANCE_H_MODE_NAV:
+      INT_VECT2_ZERO(guidance_h.rc_sp.vect);
       if (radio_control.status == RC_OK) {
-        stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
-      } else {
-        FLOAT_EULERS_ZERO(guidance_h.rc_sp);
+        read_rc_setpoint_heading(&guidance_h.rc_sp, autopilot_in_flight(), rc);
       }
       break;
-    case GUIDANCE_H_MODE_FLIP:
-      stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
-      break;
     default:
       break;
   }
 
 }
 
-void guidance_h_run(bool  in_flight)
+struct StabilizationSetpoint guidance_h_run(bool in_flight)
 {
+  struct StabilizationSetpoint sp;
+  STAB_SP_SET_EULERS_ZERO(sp);
+
   switch (guidance_h.mode) {
 
-    case GUIDANCE_H_MODE_RC_DIRECT:
-      stabilization_none_run(in_flight);
-      break;
-
-#if USE_STABILIZATION_RATE
-    case GUIDANCE_H_MODE_RATE:
-      stabilization_rate_run(in_flight);
-      break;
-#endif
-
-    case GUIDANCE_H_MODE_FORWARD:
-      if (transition_percentage < (100 << INT32_PERCENTAGE_FRAC)) {
-        transition_run(true);
-      }
-      /* Falls through. */
-    case GUIDANCE_H_MODE_CARE_FREE:
-    case GUIDANCE_H_MODE_ATTITUDE:
-      if ((!(guidance_h.mode == GUIDANCE_H_MODE_FORWARD)) && transition_percentage > 0) {
-        transition_run(false);
-      }
-      stabilization_attitude_run(in_flight);
-#if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
-      if (in_flight) {
-        stabilization_filter_commands();
-      }
-#endif
-
-      break;
-
     case GUIDANCE_H_MODE_HOVER:
       /* set psi command from RC */
-      guidance_h.sp.heading = guidance_h.rc_sp.psi;
+      guidance_h.sp.heading = guidance_h.rc_sp.heading;
       /* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
 
       /* Falls through. */
     case GUIDANCE_H_MODE_GUIDED:
-      guidance_h_guided_run(in_flight);
+      sp = guidance_h_guided_run(in_flight);
       break;
 
     case GUIDANCE_H_MODE_NAV:
-      guidance_h_from_nav(in_flight);
-      break;
-
-#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
-    case GUIDANCE_H_MODE_MODULE:
-      guidance_h_module_run(in_flight);
-      break;
-#endif
-
-    case GUIDANCE_H_MODE_FLIP:
-      guidance_flip_run();
+      sp = guidance_h_from_nav(in_flight);
       break;
 
     default:
       break;
   }
+
+  return sp;
 }
 
 
@@ -427,7 +354,7 @@ void guidance_h_hover_enter(void)
   reset_guidance_reference_from_current_position();
 
   /* set guidance to current heading and position */
-  guidance_h.rc_sp.psi = stateGetNedToBodyEulers_f()->psi;
+  guidance_h.rc_sp.heading = stateGetNedToBodyEulers_f()->psi;
   guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi);
 
   /* call specific implementation */
@@ -445,34 +372,32 @@ void guidance_h_nav_enter(void)
   guidance_h_set_heading(nav.heading);
 }
 
-void guidance_h_from_nav(bool in_flight)
+struct StabilizationSetpoint guidance_h_from_nav(bool in_flight)
 {
   if (!in_flight) {
     guidance_h_nav_enter();
   }
 
   if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_NONE) {
-    return; // don't call guidance nor stabilization
+    struct StabilizationSetpoint sp;
+    STAB_SP_SET_EULERS_ZERO(sp);
+    return sp; // don't call guidance, still return attitude zero
   } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) {
     if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
-      // directly apply quat setpoint
-      struct Int32Quat quat_i;
-      QUAT_BFP_OF_REAL(quat_i, nav.quat);
-      stabilization_attitude_set_quat_setpoint_i(&quat_i);
-      stabilization_attitude_run(in_flight);
+      return stab_sp_from_quat_f(&nav.quat);
     }
     else {
       // it should be nav.setpoint_mode == NAV_SETPOINT_MODE_ATTITUDE
       // TODO error handling ?
-      struct Int32Eulers sp_cmd_i;
-      sp_cmd_i.phi = ANGLE_BFP_OF_REAL(nav.roll);
-      sp_cmd_i.theta = ANGLE_BFP_OF_REAL(nav.pitch);
-      sp_cmd_i.psi = ANGLE_BFP_OF_REAL(nav.heading);
-      stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
-      stabilization_attitude_run(in_flight);
+      struct FloatEulers sp_cmd_f = {
+        .phi = nav.roll,
+        .theta = nav.pitch,
+        .psi = nav.heading
+      };
+      return stab_sp_from_eulers_f(&sp_cmd_f);
     }
   } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_GUIDED) {
-    guidance_h_guided_run(in_flight);
+    return guidance_h_guided_run(in_flight);
   } else {
     // update carrot for GCS display and convert ENU float -> NED int
     // even if sp is changed later
@@ -505,62 +430,12 @@ void guidance_h_from_nav(bool in_flight)
         // nothing to do for other cases at the moment
         break;
     }
-    /* set final attitude setpoint */
-    stabilization_attitude_set_stab_sp(&guidance_h_cmd);
-    stabilization_attitude_run(in_flight);
-
+    /* return final attitude setpoint */
+    return guidance_h_cmd;
   }
 }
 
-static inline void transition_run(bool to_forward)
-{
-  if (to_forward) {
-    //Add 0.00625%
-    transition_percentage += 1 << (INT32_PERCENTAGE_FRAC - 4);
-  } else {
-    //Subtract 0.00625%
-    transition_percentage -= 1 << (INT32_PERCENTAGE_FRAC - 4);
-  }
-
-#ifdef TRANSITION_MAX_OFFSET
-  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
-  transition_theta_offset = INT_MULT_RSHIFT((transition_percentage << (INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100,
-                            max_offset, INT32_ANGLE_FRAC);
-#endif
-}
-
-/// read speed setpoint from RC
-static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
-{
-  if (in_flight) {
-    // negative pitch is forward
-    int64_t rc_x = -radio_control.values[RADIO_PITCH];
-    int64_t rc_y = radio_control.values[RADIO_ROLL];
-    DeadBand(rc_x, MAX_PPRZ / 20);
-    DeadBand(rc_y, MAX_PPRZ / 20);
-
-    // convert input from MAX_PPRZ range to SPEED_BFP
-    int32_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED);
-    /// @todo calc proper scale while making sure a division by zero can't occur
-    //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y);
-    //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y);
-    rc_x = rc_x * max_speed / MAX_PPRZ;
-    rc_y = rc_y * max_speed / MAX_PPRZ;
-
-    /* Rotate from body to NED frame by negative psi angle */
-    int32_t psi = -stateGetNedToBodyEulers_i()->psi;
-    int32_t s_psi, c_psi;
-    PPRZ_ITRIG_SIN(s_psi, psi);
-    PPRZ_ITRIG_COS(c_psi, psi);
-    speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC);
-    speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC);
-  } else {
-    speed_sp->x = 0;
-    speed_sp->y = 0;
-  }
-}
-
-void guidance_h_guided_run(bool in_flight)
+struct StabilizationSetpoint guidance_h_guided_run(bool in_flight)
 {
   /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
   if (!in_flight) {
@@ -570,9 +445,8 @@ void guidance_h_guided_run(bool in_flight)
   guidance_h_update_reference();
 
   guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);
-  /* set final attitude setpoint */
-  stabilization_attitude_set_stab_sp(&guidance_h_cmd);
-  stabilization_attitude_run(in_flight);
+  /* return final attitude setpoint */
+  return guidance_h_cmd;
 }
 
 void guidance_h_set_pos(float x, float y)
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
index 30d76bc76a..a32a6fb8d6 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
@@ -53,17 +53,10 @@ extern "C" {
 #define GUIDANCE_H_USE_SPEED_REF TRUE
 #endif
 
-#define GUIDANCE_H_MODE_KILL        0
-#define GUIDANCE_H_MODE_RATE        1
-#define GUIDANCE_H_MODE_ATTITUDE    2
-#define GUIDANCE_H_MODE_HOVER       3
-#define GUIDANCE_H_MODE_NAV         4
-#define GUIDANCE_H_MODE_RC_DIRECT   5
-#define GUIDANCE_H_MODE_CARE_FREE   6
-#define GUIDANCE_H_MODE_FORWARD     7
-#define GUIDANCE_H_MODE_MODULE      8
-#define GUIDANCE_H_MODE_FLIP        9
-#define GUIDANCE_H_MODE_GUIDED      10
+#define GUIDANCE_H_MODE_NONE        0
+#define GUIDANCE_H_MODE_HOVER       1
+#define GUIDANCE_H_MODE_NAV         2
+#define GUIDANCE_H_MODE_GUIDED      3
 
 /** Max bank controlled by guidance
  */
@@ -101,6 +94,12 @@ struct HorizontalGuidanceReference {
   struct Int32Vect2 accel;   ///< with #INT32_ACCEL_FRAC
 };
 
+struct HorizontalGuidanceRCInput {
+  struct Int32Vect2 vect;
+  float heading;
+  float last_ts;
+};
+
 struct HorizontalGuidance {
   uint8_t mode;
   /* configuration options */
@@ -108,19 +107,15 @@ struct HorizontalGuidance {
 
   struct HorizontalGuidanceSetpoint sp;   ///< setpoints
   struct HorizontalGuidanceReference ref; ///< reference calculated from setpoints
-
-  struct FloatEulers rc_sp;               ///< remote control setpoint
+  struct HorizontalGuidanceRCInput rc_sp; ///< remote control setpoint
 };
 
 extern struct HorizontalGuidance guidance_h;
 
-extern int32_t transition_percentage;
-
 extern void guidance_h_init(void);
 extern void guidance_h_mode_changed(uint8_t new_mode);
-extern void guidance_h_read_rc(bool in_flight);
-extern void guidance_h_run(bool in_flight);
 extern void guidance_h_run_enter(void);
+extern struct StabilizationSetpoint guidance_h_run(bool in_flight);
 extern struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh);
 extern struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh);
 extern struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh);
@@ -130,11 +125,11 @@ extern void guidance_h_nav_enter(void);
 
 /** Set horizontal guidance from NAV and run control loop
  */
-extern void guidance_h_from_nav(bool in_flight);
+extern struct StabilizationSetpoint guidance_h_from_nav(bool in_flight);
 
 /** Run GUIDED mode control
  */
-extern void guidance_h_guided_run(bool in_flight);
+extern struct StabilizationSetpoint guidance_h_guided_run(bool in_flight);
 
 /** Set horizontal position setpoint.
  * @param x North position (local NED frame) in meters.
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
index 29543b99db..ae2f751616 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
@@ -33,18 +33,14 @@
  */
 #include "generated/airframe.h"
 #include "firmwares/rotorcraft/guidance/guidance_indi.h"
-#include "modules/ins/ins_int.h"
 #include "modules/radio_control/radio_control.h"
-#include "state.h"
-#include "modules/imu/imu.h"
+#include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/guidance/guidance_h.h"
 #include "firmwares/rotorcraft/guidance/guidance_v.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/autopilot_rc_helpers.h"
 #include "mcu_periph/sys_time.h"
+#include "state.h"
 #include "autopilot.h"
-#include "stabilization/stabilization_attitude_ref_quat_int.h"
-#include "firmwares/rotorcraft/stabilization.h"
 #include "filters/low_pass_filter.h"
 #include "modules/core/abi.h"
 
@@ -124,6 +120,7 @@ float time_of_accel_sp_2d = 0.0;
 float time_of_accel_sp_3d = 0.0;
 
 struct FloatEulers guidance_euler_cmd;
+struct ThrustSetpoint thrust_sp;
 float thrust_in;
 
 static void guidance_indi_propagate_filters(struct FloatEulers *eulers);
@@ -155,6 +152,8 @@ static void send_indi_guidance(struct transport_tx *trans, struct link_device *d
  */
 void guidance_indi_init(void)
 {
+  FLOAT_EULERS_ZERO(guidance_euler_cmd);
+  THRUST_SP_SET_ZERO(thrust_sp);
   AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);
 
 #if PERIODIC_TELEMETRY
@@ -171,7 +170,7 @@ void guidance_indi_enter(void)
   /* set nav_heading to current heading */
   nav.heading = stateGetNedToBodyEulers_f()->psi;
 
-  thrust_in = stabilization_cmd[COMMAND_THRUST];
+  thrust_in = stabilization.cmd[COMMAND_THRUST];
   thrust_act = thrust_in;
 
 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
@@ -273,31 +272,32 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   //Calculate roll,pitch and thrust command
   MAT33_VECT3_MUL(control_increment, Ga_inv, a_diff);
 
-  struct FloatVect3 thrust_vect;
-  thrust_vect.x = 0.0;  // Fill for quadplanes
-  thrust_vect.y = 0.0;
-  thrust_vect.z = control_increment.z;
-  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);
-
   guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
   guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y;
   guidance_euler_cmd.psi = heading_sp;
 
+  // Compute and store thust setpoint
 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
   guidance_indi_filter_thrust();
-
   //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
   thrust_in = thrust_filt.o[0] + control_increment.z * guidance_indi_specific_force_gain;
   Bound(thrust_in, 0, 9600);
-
 #if GUIDANCE_INDI_RC_DEBUG
   if (radio_control.values[RADIO_THROTTLE] < 300) {
     thrust_in = 0;
   }
 #endif
+  // return required thrust
+  thrust_sp = th_sp_from_thrust_i(thrust_in, THRUST_AXIS_Z);
 
-  //Overwrite the thrust command from guidance_v
-  stabilization_cmd[COMMAND_THRUST] = thrust_in;
+#else
+  float thrust_vect[3];
+  thrust_vect[0] = 0.0f;  // Fill for quadplanes
+  thrust_vect[1] = 0.0f;
+  thrust_vect[2] = control_increment.z;
+
+  // specific force not defined, return required increment
+  thrust_sp = th_sp_from_incr_vect_f(thrust_vect);
 #endif
 
   //Bound euler angles to prevent flipping
@@ -372,8 +372,7 @@ void guidance_indi_filter_thrust(void)
   // Actuator dynamics
   thrust_act = thrust_act + thrust_dyn * (thrust_in - thrust_act);
 
-  // same filter as for the acceleration
-  update_butterworth_2_low_pass(&thrust_filt, thrust_act);
+  // same filter as for the acceleorth_2_low_pass(&thrust_filt, thrust_act);
 }
 #endif
 
@@ -504,25 +503,25 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
   return guidance_indi_run_mode(in_flight, gh, _gv, GUIDANCE_INDI_H_ACCEL, _v_mode);
 }
 
-int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
 {
   _gv = gv;
   _v_mode = GUIDANCE_INDI_V_POS;
-  return 0; // nothing to do
+  return thrust_sp;
 }
 
-int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
 {
   _gv = gv;
   _v_mode = GUIDANCE_INDI_V_SPEED;
-  return 0; // nothing to do
+  return thrust_sp;
 }
 
-int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
 {
   _gv = gv;
   _v_mode = GUIDANCE_INDI_V_ACCEL;
-  return 0; // nothing to do
+  return thrust_sp;
 }
 
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index 27990b3ef3..80c3e0d6a4 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -30,15 +30,14 @@
 #include "generated/airframe.h"
 #include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
 #include "modules/radio_control/radio_control.h"
-#include "state.h"
 #include "firmwares/rotorcraft/autopilot_rc_helpers.h"
+#include "state.h"
 #include "mcu_periph/sys_time.h"
 #include "autopilot.h"
-#include "stabilization/stabilization_attitude_ref_quat_int.h"
 #include "stdio.h"
 #include "filters/low_pass_filter.h"
 #include "modules/core/abi.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
+#include "firmwares/rotorcraft/navigation.h"
 
 
 // The acceleration reference is calculated with these gains. If you use GPS,
@@ -176,7 +175,7 @@ float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH;
 struct FloatEulers eulers_zxy;
 
 float thrust_dyn = 0.f;
-float thrust_act = 0;
+float thrust_act = 0.f;
 Butterworth2LowPass filt_accel_ned[3];
 Butterworth2LowPass roll_filt;
 Butterworth2LowPass pitch_filt;
@@ -216,6 +215,7 @@ float guidance_indi_airspeed_filt_cutoff = GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF;
 
 float guidance_indi_hybrid_heading_sp = 0.f;
 struct FloatEulers guidance_euler_cmd;
+struct ThrustSetpoint thrust_sp;
 float thrust_in;
 
 struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0};
@@ -339,12 +339,13 @@ void guidance_indi_init(void)
  *
  * Call upon entering indi guidance
  */
-void guidance_indi_enter(void) {
+void guidance_indi_enter(void)
+{
   /*Obtain eulers with zxy rotation order*/
   float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
   nav.heading = eulers_zxy.psi;
 
-  thrust_in = stabilization_cmd[COMMAND_THRUST];
+  thrust_in = stabilization.cmd[COMMAND_THRUST];
   thrust_act = thrust_in;
   guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi;
 
@@ -366,7 +367,6 @@ void guidance_indi_enter(void) {
   init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0);
 }
 
-#include "firmwares/rotorcraft/navigation.h"
 /**
  * @param accel_sp accel setpoint in NED frame [m/s^2]
  * @param heading_sp the desired heading [rad]
@@ -382,12 +382,9 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   /* Obtain eulers with zxy rotation order */
   float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
 
-  /* Calculate the transition percentage so that the ctrl_effecitveness scheduling works */
-  transition_percentage = BFP_OF_REAL((eulers_zxy.theta/RadOfDeg(-75.0f))*100,INT32_PERCENTAGE_FRAC);
-  Bound(transition_percentage,0,BFP_OF_REAL(100.0f,INT32_PERCENTAGE_FRAC));
-  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
-  transition_theta_offset = INT_MULT_RSHIFT((transition_percentage <<
-        (INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100, max_offset, INT32_ANGLE_FRAC);
+  /* Calculate the transition ratio so that the ctrl_effecitveness scheduling works */
+  stabilization.transition_ratio = eulers_zxy.theta / RadOfDeg(-75.0f);
+  Bound(stabilization.transition_ratio, 0.f, 1.f);
 
   // filter accel to get rid of noise and filter attitude to synchronize with accel
   guidance_indi_propagate_filters();
@@ -411,9 +408,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   accel_filt.z = filt_accel_ned[2].o[0];
 
   struct FloatVect3 a_diff;
-  a_diff.x = sp_accel.x - accel_filt.x;
-  a_diff.y = sp_accel.y - accel_filt.y;
-  a_diff.z = sp_accel.z - accel_filt.z;
+  VECT3_DIFF(a_diff, sp_accel, accel_filt);
 
   // Bound the acceleration error so that the linearization still holds
   Bound(a_diff.x, -6.0, 6.0);
@@ -428,7 +423,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
 #endif
 #endif
 
-
   // Calculate matrix of partial derivatives and control objective
   guidance_indi_calcg_wing(Ga, a_diff, v_gih);
 
@@ -456,16 +450,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   float_mat3_mult(&euler_cmd, Ga_inv, a_diff);
 #endif
 
-  struct FloatVect3 thrust_vect;
-#if GUIDANCE_INDI_HYBRID_U > 3
-  thrust_vect.x = du_gih[3];
-#else
-  thrust_vect.x = 0;
-#endif
-  thrust_vect.y = 0;
-  thrust_vect.z = euler_cmd.z;
-  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);
-
   // Coordinated turn
   // feedforward estimate angular rotation omega = g*tan(phi)/v
   float omega;
@@ -546,21 +530,31 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
     guidance_euler_cmd.psi = guidance_indi_hybrid_heading_sp;
   }
 
+  // compute required thrust setpoint
 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
   guidance_indi_filter_thrust();
-
   // Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
   thrust_in = thrust_filt.o[0] + euler_cmd.z * guidance_indi_specific_force_gain;
   Bound(thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
-
 #if GUIDANCE_INDI_RC_DEBUG
   if (radio_control.values[RADIO_THROTTLE] < 300) {
     thrust_in = 0;
   }
 #endif
+  // return required thrust
+  thrust_sp = th_sp_from_thrust_i(thrust_in, THRUST_AXIS_Z);
 
-  // Overwrite the thrust command from guidance_v
-  stabilization_cmd[COMMAND_THRUST] = thrust_in;
+#else
+  float thrust_vect[3];
+#if GUIDANCE_INDI_HYBRID_U > 3
+  thrust_vect[0] = du_gih[3];
+#else
+  thrust_vect[0] = 0;
+#endif
+  thrust_vect[1] = 0;
+  thrust_vect[2] = euler_cmd.z;
+  // specific force not defined, return required increment
+  thrust_sp = th_sp_from_incr_vect_f(thrust_vect);
 #endif
 
   // Set the quaternion setpoint from eulers_zxy
@@ -791,7 +785,8 @@ void guidance_indi_filter_thrust(void)
  * acceleration
  * Called as a periodic function with PERIODIC_FREQ
  */
-void guidance_indi_propagate_filters(void) {
+void guidance_indi_propagate_filters(void)
+{
   struct NedCoor_f *accel = stateGetAccelNed_f();
   update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
   update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
@@ -888,25 +883,25 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
   return guidance_indi_run_mode(in_flight, gh, _gv, GUIDANCE_INDI_HYBRID_H_ACCEL, _v_mode);
 }
 
-int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
 {
   _gv = gv;
   _v_mode = GUIDANCE_INDI_HYBRID_V_POS;
-  return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
+  return thrust_sp;
 }
 
-int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
 {
   _gv = gv;
   _v_mode = GUIDANCE_INDI_HYBRID_V_SPEED;
-  return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
+  return thrust_sp;
 }
 
-int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
 {
   _gv = gv;
   _v_mode = GUIDANCE_INDI_HYBRID_V_ACCEL;
-  return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
+  return thrust_sp;
 }
 
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
index 3db04d33be..eed4cb0829 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
@@ -25,13 +25,11 @@
  */
 
 #include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
-#include "stabilization/stabilization_attitude_ref_quat_int.h"
 #include "filters/low_pass_filter.h"
 #include "state.h"
 #include "autopilot.h"
 #include "generated/modules.h"
 
-
 #ifndef COMMAND_THRUST_X
 #error "Quadplanes require a forward thrust actuator"
 #endif
@@ -48,13 +46,10 @@ float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
 float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF;
 #endif
 
-
 float bodyz_filter_cutoff = 0.2;
 
 Butterworth2LowPass accel_bodyz_filt;
 
-
-
 /**
  *
  * Call upon entering indi guidance
@@ -65,7 +60,6 @@ void guidance_indi_quadplane_init(void) {
   init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81);
 }
 
-
 /**
  * Low pass the accelerometer measurements to remove noise from vibrations.
  * The roll and pitch also need to be filtered to synchronize them with the
@@ -78,9 +72,6 @@ void guidance_indi_quadplane_propagate_filters(void) {
   update_butterworth_2_low_pass(&accel_bodyz_filt, accelz);
 }
 
-
-
-
 /**
  * Perform WLS
  *
@@ -145,14 +136,14 @@ void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angl
   // Set lower limits
   du_min_gih[0] = -roll_limit_rad - roll_angle; //roll
   du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch
-  du_min_gih[2] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff;
-  du_min_gih[3] = -stabilization_cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff;
+  du_min_gih[2] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff;
+  du_min_gih[3] = -stabilization.cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff;
 
   // Set upper limits limits
   du_max_gih[0] = roll_limit_rad - roll_angle; //roll
   du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch
-  du_max_gih[2] = -stabilization_cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff;
-  du_max_gih[3] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff;
+  du_max_gih[2] = -stabilization.cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff;
+  du_max_gih[3] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff;
 
   // Set prefered states
   du_pref_gih[0] = -roll_angle; // prefered delta roll angle
@@ -165,4 +156,5 @@ void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angl
 /** Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts with all thrust off */
 bool autopilot_in_flight_end_detection(bool motors_on UNUSED) {
   return ! motors_on;
-}
\ No newline at end of file
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
index cadc822ec3..8ea53140b7 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
@@ -31,17 +31,15 @@
  *
  * The guidance that the module implement must be activated with following defines:
  *
- * a) Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE
- * One must then implement:
- * - void guidance_h_module_init(void);
- * - void guidance_h_module_enter(void);
- * - void guidance_h_module_read_rc(void);
- * - void guidance_h_module_run(bool in_flight);
+ * a) Implement own loops
+ * - void guidance_module_enter(void);
+ * - void guidance_module_run(bool in_flight);
  *
  *
- * b) Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE
- * - void guidance_v_module_enter(void);
- * - void guidance_v_module_run(bool in_flight);
+ * b) Use or copy the generated autopilot file 'conf/autopilot/rotorcraft_control_loop.xml'
+ * - load custom autopilot from your airframe filewith by adding
+ *   <autopilot name="rotorcraft_control_loop"/>
+ *   to the firmware section
  *
  * If the module implements both V and H mode, take into account that the V is called first and later H
  *
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c
index bd6e7bf3c0..a45138b9da 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c
@@ -261,7 +261,7 @@ static struct StabilizationSetpoint guidance_pid_h_run(bool in_flight, struct Ho
   if (guidance_pid.approx_force_by_thrust && in_flight) {
     static int32_t thrust_cmd_filt;
     // FIXME strong coupling with guidance_v here !!!
-    int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
+    int32_t vertical_thrust = (stabilization.cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
     thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
                       (GUIDANCE_H_THRUST_CMD_FILTER + 1);
     guidance_pid.cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_pid.cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2),
@@ -318,7 +318,7 @@ struct StabilizationSetpoint guidance_pid_h_run_accel(bool in_flight UNUSED, str
 
 #define FF_CMD_FRAC 18
 
-static int32_t guidance_pid_v_run(bool in_flight, struct VerticalGuidance *gv)
+static struct ThrustSetpoint guidance_pid_v_run(bool in_flight, struct VerticalGuidance *gv)
 {
   /* compute the error to our reference */
   int32_t err_z  = gv->z_ref - stateGetPositionNed_i()->z;
@@ -365,23 +365,25 @@ static int32_t guidance_pid_v_run(bool in_flight, struct VerticalGuidance *gv)
   /* bound the result */
   Bound(guidance_pid.cmd_thrust, 0, MAX_PPRZ);
 
-  return guidance_pid.cmd_thrust;
+  return th_sp_from_thrust_i(guidance_pid.cmd_thrust, THRUST_AXIS_Z);
 }
 
-int32_t guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
 {
   return guidance_pid_v_run(in_flight, gv);
 }
 
-int32_t guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
 {
   return guidance_pid_v_run(in_flight, gv);
 }
 
-int32_t guidance_pid_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+struct ThrustSetpoint guidance_pid_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
 {
   // TODO
-  return 0;
+  struct ThrustSetpoint sp;
+  THRUST_SP_SET_ZERO(sp);
+  return sp;
 }
 
 void guidance_pid_h_enter(void)
@@ -445,17 +447,17 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
   return guidance_pid_h_run_accel(in_flight, gh);
 }
 
-int32_t guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
 {
   return guidance_pid_v_run_pos(in_flight, gv);
 }
 
-int32_t guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
 {
   return guidance_pid_v_run_speed(in_flight, gv);
 }
 
-int32_t guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv)
 {
   return guidance_pid_v_run_accel(in_flight, gv);
 }
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h
index 5b4bfd1967..3a9c7a84f5 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h
@@ -64,9 +64,9 @@ extern void guidance_pid_v_enter(void);
 extern struct StabilizationSetpoint guidance_pid_h_run_pos(bool in_flight, struct HorizontalGuidance *gh);
 extern struct StabilizationSetpoint guidance_pid_h_run_speed(bool in_flight, struct HorizontalGuidance *gh);
 extern struct StabilizationSetpoint guidance_pid_h_run_accel(bool in_flight, struct HorizontalGuidance *gh);
-extern int32_t guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
-extern int32_t guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
-extern int32_t guidance_pid_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
+extern struct ThrustSetpoint guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
+extern struct ThrustSetpoint guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
+extern struct ThrustSetpoint guidance_pid_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
 
 extern void guidance_pid_set_h_igain(uint32_t igain);
 extern void guidance_pid_set_v_igain(uint32_t igain);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 3a41f92e41..0f63b6622b 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -32,6 +32,7 @@
 #include "modules/radio_control/radio_control.h"
 #include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/navigation.h"
+#include "modules/core/abi.h"
 
 #include "state.h"
 
@@ -66,6 +67,13 @@ static bool desired_zd_updated;
 
 static int guidance_v_guided_mode;
 
+#ifndef GUIDANCE_V_RC_ID
+#define GUIDANCE_V_RC_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_RC_ID)
+static abi_event rc_ev;
+static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc);
+
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -92,24 +100,21 @@ void guidance_v_init(void)
 
   gv_adapt_init();
 
-#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
-  guidance_v_module_init();
-#endif
+  // bind ABI messages
+  AbiBindMsgRADIO_CONTROL(GUIDANCE_V_RC_ID, &rc_ev, rc_cb);
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_TUNE_VERT, send_tune_vert);
 #endif
 }
 
-
-void guidance_v_read_rc(void)
+static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
 {
-
   /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
-  guidance_v.rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE];
+  guidance_v.rc_delta_t = (int32_t)rc->values[RADIO_THROTTLE];
 
   /* used in RC_CLIMB */
-  guidance_v.rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)radio_control.values[RADIO_THROTTLE];
+  guidance_v.rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)rc->values[RADIO_THROTTLE];
   DeadBand(guidance_v.rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND);
 
   static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
@@ -146,15 +151,6 @@ void guidance_v_mode_changed(uint8_t new_mode)
       GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
       break;
 
-#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
-    case GUIDANCE_V_MODE_MODULE:
-      guidance_v_module_enter();
-      break;
-#endif
-
-    case GUIDANCE_V_MODE_FLIP:
-      break;
-
     default:
       break;
 
@@ -210,9 +206,10 @@ void guidance_v_thrust_adapt(bool in_flight)
      * This means that the estimation is not updated when using direct throttle commands.
      *
      * FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co
+     * FIXME get this out of here !!!! and don't get stab cmd directly
      */
     if (desired_zd_updated) {
-      int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
+      int32_t vertical_thrust = (stabilization.cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
       gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v.zd_ref);
     }
   } else {
@@ -221,63 +218,49 @@ void guidance_v_thrust_adapt(bool in_flight)
   }
 }
 
-void guidance_v_run(bool in_flight)
+struct ThrustSetpoint guidance_v_run(bool in_flight)
 {
   guidance_v_thrust_adapt(in_flight);
 
   /* reset flag indicating if desired zd was updated */
   desired_zd_updated = false;
+  /* reset setpoint */
+  THRUST_SP_SET_ZERO(guidance_v.thrust);
 
   switch (guidance_v.mode) {
 
     case GUIDANCE_V_MODE_RC_DIRECT:
       guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
-      stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t;
+      guidance_v.thrust = th_sp_from_thrust_i(guidance_v.rc_delta_t, THRUST_AXIS_Z);
       break;
 
     case GUIDANCE_V_MODE_RC_CLIMB:
       guidance_v.zd_sp = guidance_v.rc_zd_sp;
       gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
-      guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
-      stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
+      guidance_v.thrust = guidance_v_run_speed(in_flight, &guidance_v);
       break;
 
     case GUIDANCE_V_MODE_CLIMB:
       gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
-      guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
-#if !NO_RC_THRUST_LIMIT
-      /* use rc limitation if available */
-      if (radio_control.status == RC_OK) {
-        stabilization_cmd[COMMAND_THRUST] = Min(guidance_v.rc_delta_t, guidance_v.delta_t);
-      } else
-#endif
-        stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
+      guidance_v.thrust = guidance_v_run_speed(in_flight, &guidance_v);
       break;
 
     case GUIDANCE_V_MODE_HOVER:
       guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
       /* Falls through. */
     case GUIDANCE_V_MODE_GUIDED:
-      guidance_v_guided_run(in_flight);
+      guidance_v.thrust = guidance_v_guided_run(in_flight);
       break;
 
-#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
-    case GUIDANCE_V_MODE_MODULE:
-      guidance_v_module_run(in_flight);
-      break;
-#endif
-
     case GUIDANCE_V_MODE_NAV: {
-      guidance_v_from_nav(in_flight);
+      guidance_v.thrust = guidance_v_from_nav(in_flight);
       break;
     }
 
-    case GUIDANCE_V_MODE_FLIP:
-      break;
-
     default:
       break;
   }
+  return guidance_v.thrust;
 }
 
 
@@ -314,36 +297,32 @@ void guidance_v_update_ref(void)
   desired_zd_updated = true;
 }
 
-void guidance_v_from_nav(bool in_flight)
+struct ThrustSetpoint guidance_v_from_nav(bool in_flight)
 {
+  struct ThrustSetpoint sp;
+  THRUST_SP_SET_ZERO(sp);
   if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
     guidance_v.z_sp = -POS_BFP_OF_REAL(nav.nav_altitude);
     guidance_v.zd_sp = 0;
     gv_update_ref_from_z_sp(guidance_v.z_sp);
     guidance_v_update_ref();
-    guidance_v.delta_t = guidance_v_run_pos(in_flight, &guidance_v);
+    sp = guidance_v_run_pos(in_flight, &guidance_v);
   } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
     guidance_v.z_sp = stateGetPositionNed_i()->z;
     guidance_v.zd_sp = -SPEED_BFP_OF_REAL(nav.climb);
     gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
     guidance_v_update_ref();
-    guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
+    sp = guidance_v_run_speed(in_flight, &guidance_v);
   } else if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
     guidance_v.z_sp = stateGetPositionNed_i()->z;
     guidance_v.zd_sp = stateGetSpeedNed_i()->z;
     GuidanceVSetRef(guidance_v.z_sp, guidance_v.zd_sp, 0);
     guidance_v_run_enter();
-    guidance_v.delta_t = nav.throttle;
+    sp = th_sp_from_thrust_i((int32_t)nav.throttle, THRUST_AXIS_Z);
   } else if (nav.vertical_mode == NAV_VERTICAL_MODE_GUIDED) {
-    guidance_v_guided_run(in_flight);
+    sp = guidance_v_guided_run(in_flight);
   }
-#if !NO_RC_THRUST_LIMIT
-  /* use rc limitation if available */
-  if (radio_control.status == RC_OK) {
-    stabilization_cmd[COMMAND_THRUST] = Min(guidance_v.rc_delta_t, guidance_v.delta_t);
-  } else
-#endif
-    stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
+  return sp;
 }
 
 void guidance_v_guided_enter(void)
@@ -356,8 +335,10 @@ void guidance_v_guided_enter(void)
   GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
 }
 
-void guidance_v_guided_run(bool in_flight)
+struct ThrustSetpoint guidance_v_guided_run(bool in_flight)
 {
+  struct ThrustSetpoint sp;
+  THRUST_SP_SET_ZERO(sp);
   switch(guidance_v_guided_mode)
   {
     case GUIDANCE_V_GUIDED_MODE_ZHOLD:
@@ -365,29 +346,23 @@ void guidance_v_guided_run(bool in_flight)
       guidance_v.zd_sp = 0;
       gv_update_ref_from_z_sp(guidance_v.z_sp);
       guidance_v_update_ref();
-      guidance_v.delta_t = guidance_v_run_pos(in_flight, &guidance_v);
+      sp = guidance_v_run_pos(in_flight, &guidance_v);
       break;
     case GUIDANCE_V_GUIDED_MODE_CLIMB:
       // Climb
       gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
       guidance_v_update_ref();
-      guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
+      sp = guidance_v_run_speed(in_flight, &guidance_v);
       break;
     case GUIDANCE_V_GUIDED_MODE_THROTTLE:
       // Throttle
       guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
-      guidance_v.delta_t = guidance_v.th_sp;
+      sp = th_sp_from_thrust_i(guidance_v.th_sp, THRUST_AXIS_Z);
       break;
     default:
       break;
   }
-#if !NO_RC_THRUST_LIMIT
-  /* use rc limitation if available */
-  if (radio_control.status == RC_OK) {
-    stabilization_cmd[COMMAND_THRUST] = Min(guidance_v.rc_delta_t, guidance_v.delta_t);
-  } else
-#endif
-    stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
+  return sp;
 }
 
 void guidance_v_set_z(float z)
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
index 11c32674fb..0ee3f173a4 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
@@ -30,6 +30,7 @@
 
 #include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
 #include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
+#include "firmwares/rotorcraft/stabilization.h"
 
 #define GUIDANCE_V_MODE_KILL      0
 #define GUIDANCE_V_MODE_RC_DIRECT 1
@@ -37,9 +38,7 @@
 #define GUIDANCE_V_MODE_CLIMB     3
 #define GUIDANCE_V_MODE_HOVER     4
 #define GUIDANCE_V_MODE_NAV       5
-#define GUIDANCE_V_MODE_MODULE    6
-#define GUIDANCE_V_MODE_FLIP      7
-#define GUIDANCE_V_MODE_GUIDED    8
+#define GUIDANCE_V_MODE_GUIDED    6
 
 struct VerticalGuidance {
   uint8_t mode;
@@ -85,16 +84,16 @@ struct VerticalGuidance {
    */
   int32_t rc_zd_sp;
 
-  /** thrust setpoint.
+  /** input thrust setpoint.
    *  valid range 0 : #MAX_PPRZ
    */
   int32_t th_sp;
 
-  /** thrust command.
+  /** Final thrust setpoint
    *  summation of feed-forward and feed-back commands,
-   *  valid range 0 : #MAX_PPRZ
+   *  can be a total thrust or increment, float or int
    */
-  int32_t delta_t;
+  struct ThrustSetpoint thrust; // FIXME maybe not needed to store the value ?
 
   /** nominal throttle for hover.
    * This is only used if #GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined!
@@ -108,18 +107,20 @@ struct VerticalGuidance {
 extern struct VerticalGuidance guidance_v;
 
 extern void guidance_v_init(void);
-extern void guidance_v_read_rc(void);
 extern void guidance_v_mode_changed(uint8_t new_mode);
 extern void guidance_v_notify_in_flight(bool in_flight);
 extern void guidance_v_thrust_adapt(bool in_flight);
 extern void guidance_v_update_ref(void);
-extern void guidance_v_run(bool in_flight);
 extern void guidance_v_z_enter(void);
-
 extern void guidance_v_run_enter(void);
-extern int32_t guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
-extern int32_t guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
-extern int32_t guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
+
+/** Guidance vertical run functions
+ * @return a thrust setpoint structure
+ */
+extern struct ThrustSetpoint guidance_v_run(bool in_flight);
+extern struct ThrustSetpoint guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
+extern struct ThrustSetpoint guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
+extern struct ThrustSetpoint guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
 
 /** Set guidance ref parameters
 */
@@ -129,7 +130,7 @@ extern void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel);
 
 /** Set guidance setpoint from NAV and run hover loop
 */
-extern void guidance_v_from_nav(bool in_flight);
+extern struct ThrustSetpoint guidance_v_from_nav(bool in_flight);
 
 /** Enter GUIDED mode control
 */
@@ -137,7 +138,7 @@ extern void guidance_v_guided_enter(void);
 
 /** Run GUIDED mode control
 */
-extern void guidance_v_guided_run(bool in_flight);
+extern struct ThrustSetpoint guidance_v_guided_run(bool in_flight);
 
 /** Set z position setpoint.
  * @param z Setpoint (down is positive) in meters.
diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
index 577108da3f..0fb1ffc665 100644
--- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
+++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
@@ -1339,12 +1339,12 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des,
   }
 
   /*Commit the actuator command*/
-  stabilization_cmd[COMMAND_THRUST] = 0;
+  stabilization.cmd[COMMAND_THRUST] = 0;
   for (i = 0; i < ANDI_NUM_ACT; i++) {
     actuators_pprz[i] = (int16_t) andi_u[i];
-    stabilization_cmd[COMMAND_THRUST] += actuator_state_1l[i];
+    stabilization.cmd[COMMAND_THRUST] += actuator_state_1l[i];
   }
-  stabilization_cmd[COMMAND_THRUST] = stabilization_cmd[COMMAND_THRUST]/num_thrusters_oneloop;
+  stabilization.cmd[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST]/num_thrusters_oneloop;
   if(autopilot.mode==AP_MODE_ATTITUDE_DIRECT){
     eulers_zxy_des.phi   =  andi_u[ANDI_NUM_ACT];
     eulers_zxy_des.theta =  andi_u[ANDI_NUM_ACT+1];
diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h
index 56b246cb7d..91e8f5ee7a 100644
--- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h
+++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h
@@ -130,11 +130,11 @@ struct Gains2ndOrder{
 extern struct PolePlacement p_att_e;
 extern struct PolePlacement p_att_rm;
 /*Position Loop*/
-extern struct PolePlacement p_pos_e;  
+extern struct PolePlacement p_pos_e;
 extern struct PolePlacement p_pos_rm;
 /*Altitude Loop*/
-extern struct PolePlacement p_alt_e;   
-extern struct PolePlacement p_alt_rm; 
+extern struct PolePlacement p_alt_e;
+extern struct PolePlacement p_alt_rm;
 /*Heading Loop*/
 extern struct PolePlacement p_head_e;
 extern struct PolePlacement p_head_rm;
@@ -142,12 +142,11 @@ extern struct PolePlacement p_head_rm;
 extern struct Gains3rdOrder k_att_e;
 extern struct Gains3rdOrder k_att_rm;
 extern struct Gains2ndOrder k_head_e;
-extern struct Gains2ndOrder k_head_rm;  
+extern struct Gains2ndOrder k_head_rm;
 extern struct Gains3rdOrder k_pos_e;
-extern struct Gains3rdOrder k_pos_rm; 
+extern struct Gains3rdOrder k_pos_rm;
 extern void oneloop_andi_init(void);
 extern void oneloop_andi_enter(bool half_loop_sp);
-extern void oneloop_andi_set_failsafe_setpoint(void);
 extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
 extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
 extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.c b/sw/airborne/firmwares/rotorcraft/stabilization.c
index c43b8b522a..7e4c04a250 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization.c
@@ -24,14 +24,22 @@
  */
 
 #include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_direct.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "modules/radio_control/radio_control.h"
+#include "modules/core/abi.h"
+#include "autopilot.h"
 #include "state.h"
 
 #if (STABILIZATION_FILTER_COMMANDS_ROLL_PITCH || STABILIZATION_FILTER_COMMANDS_YAW)
 #include "filters/low_pass_filter.h"
 #endif
 
-int32_t stabilization_cmd[COMMANDS_NB];
+struct Stabilization stabilization;
+//int32_t stabilization.cmd[COMMANDS_NB];
 
 #if STABILIZATION_FILTER_CMD_ROLL_PITCH
 #ifndef STABILIZATION_FILTER_CMD_ROLL_CUTOFF
@@ -54,10 +62,43 @@ struct SecondOrderLowPass_int filter_pitch;
 struct SecondOrderLowPass_int filter_yaw;
 #endif
 
+#ifndef STABILIZATION_RC_ID
+#define STABILIZATION_RC_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(STABILIZATION_RC_ID)
+static abi_event rc_ev;
+static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc);
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+static void send_tune_hover(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
+{
+#if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
+  pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
+                                      &radio_control.values[RADIO_ROLL],
+                                      &radio_control.values[RADIO_PITCH],
+                                      &radio_control.values[RADIO_YAW],
+                                      &stabilization.cmd[COMMAND_ROLL],
+                                      &stabilization.cmd[COMMAND_PITCH],
+                                      &stabilization.cmd[COMMAND_YAW],
+                                      &stabilization.cmd[COMMAND_THRUST],
+                                      &(stateGetNedToBodyEulers_i()->phi),
+                                      &(stateGetNedToBodyEulers_i()->theta),
+                                      &(stateGetNedToBodyEulers_i()->psi));
+#endif
+}
+
+#endif
+
 void stabilization_init(void)
 {
+  stabilization.mode = STABILIZATION_MODE_NONE;
+  stabilization.att_submode = STABILIZATION_ATT_SUBMODE_HEADING;
+  stabilization_attitude_rc_setpoint_init(&stabilization.rc_in);
+  STAB_SP_SET_EULERS_ZERO(stabilization.rc_sp);
   for (uint8_t i = 0; i < COMMANDS_NB; i++) {
-    stabilization_cmd[i] = 0;
+    stabilization.cmd[i] = 0;
   }
 
   // Initialize low pass filters
@@ -72,6 +113,173 @@ void stabilization_init(void)
   init_second_order_low_pass_int(&filter_yaw, STABILIZATION_FILTER_CMD_YAW_CUTOFF, 0.7071, 1.0 / PERIODIC_FREQUENCY, 0.0);
 #endif
 
+  // bind ABI messages
+  AbiBindMsgRADIO_CONTROL(STABILIZATION_RC_ID, &rc_ev, rc_cb);
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
+#endif
+}
+
+#if USE_STABILIZATION_RATE
+static void stabilization_rate_reset_rc(void)
+{
+  struct FloatRates zero = { 0., 0., 0. };
+  stabilization.rc_sp = stab_sp_from_rates_f(&zero);
+}
+#endif
+
+static void stabilization_attitude_reset_rc(void)
+{
+  stabilization_attitude_reset_rc_setpoint(&stabilization.rc_in);
+  stabilization.rc_sp = stab_sp_from_quat_f(stateGetNedToBodyQuat_f());
+}
+
+void stabilization_mode_changed(uint8_t new_mode, uint8_t submode)
+{
+  if (new_mode == stabilization.mode && submode == stabilization.att_submode) {
+    return;
+  }
+
+  switch (new_mode) {
+    case STABILIZATION_MODE_NONE:
+      // nothing to do
+      break;
+    case STABILIZATION_MODE_DIRECT:
+      stabilization_direct_enter();
+      break;
+#if USE_STABILIZATION_RATE
+    case STABILIZATION_MODE_RATE:
+      stabilization_rate_reset_rc();
+      stabilization_rate_enter();
+      break;
+#endif
+    case STABILIZATION_MODE_ATTITUDE:
+      stabilization_attitude_reset_rc();
+      if (submode == STABILIZATION_ATT_SUBMODE_CARE_FREE) {
+        stabilization_attitude_reset_care_free_heading(&stabilization.rc_in);
+      }
+      stabilization_attitude_enter();
+      break;
+    default:
+      break;
+  }
+
+  stabilization.att_submode = submode;
+  stabilization.mode = new_mode;
+}
+
+struct StabilizationSetpoint WEAK stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
+{
+#if USE_EARTH_BOUND_RC_SETPOINT
+  stabilization_attitude_read_rc_setpoint_earth_bound(&stabilization.rc_in,
+      in_flight, in_carefree, coordinated_turn, rc);
+#else
+  stabilization_attitude_read_rc_setpoint(&stabilization.rc_in,
+      in_flight, in_carefree, coordinated_turn, rc);
+#endif
+  return stab_sp_from_quat_f(&stabilization.rc_in.rc_quat);
+}
+
+static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
+{
+  switch (stabilization.mode) {
+
+    case STABILIZATION_MODE_DIRECT:
+      stabilization_direct_read_rc();
+      break;
+#if USE_STABILIZATION_RATE
+    case STABILIZATION_MODE_RATE:
+      stabilization.rc_sp = stabilization_rate_read_rc(rc);
+      break;
+#endif
+    case STABILIZATION_MODE_ATTITUDE:
+      {
+        switch (stabilization.att_submode) {
+          case STABILIZATION_ATT_SUBMODE_HEADING:
+            stabilization.rc_sp = stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE, rc);
+            break;
+          case STABILIZATION_ATT_SUBMODE_CARE_FREE:
+            stabilization.rc_sp = stabilization_attitude_read_rc(autopilot_in_flight(), TRUE, FALSE, rc);
+            break;
+          case STABILIZATION_ATT_SUBMODE_FORWARD:
+            stabilization.rc_sp = stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, TRUE, rc);
+            break;
+          default:
+            break;
+        }
+      }
+      break;
+    default:
+      break;
+  }
+}
+
+/** Transition from 0 to 100%, used for pitch offset of hybrids
+ */
+#define TRANSITION_TO_HOVER false
+#define TRANSITION_TO_FORWARD true
+
+#ifndef TRANSITION_TIME
+#define TRANSITION_TIME 3.f
+#endif
+
+static const float transition_increment = 1.f / (TRANSITION_TIME * PERIODIC_FREQUENCY);
+
+static inline void transition_run(bool to_forward)
+{
+  if (to_forward && stabilization.transition_ratio < 1.0f) {
+    stabilization.transition_ratio += transition_increment;
+  } else if (!to_forward && stabilization.transition_ratio > 0.f) {
+    stabilization.transition_ratio -= transition_increment;
+  }
+  Bound(stabilization.transition_ratio, 0.f, 1.f);
+#ifdef TRANSITION_MAX_OFFSET
+  stabilization.rc_in.transition_theta_offset = stabilization.transition_ratio * TRANSITION_MAX_OFFSET;
+#endif
+}
+
+void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
+{
+  switch (stabilization.mode) {
+
+    case STABILIZATION_MODE_DIRECT:
+      stabilization_direct_run(in_flight, sp, thrust, cmd);
+      break;
+#if USE_STABILIZATION_RATE
+    case STABILIZATION_MODE_RATE:
+      stabilization_rate_run(in_flight, sp, thrust, cmd);
+      break;
+#endif
+    case STABILIZATION_MODE_ATTITUDE:
+      if (stabilization.att_submode == STABILIZATION_ATT_SUBMODE_FORWARD) {
+        transition_run(TRANSITION_TO_FORWARD);
+      } else {
+        transition_run(TRANSITION_TO_HOVER);
+      }
+      stabilization_attitude_run(in_flight, sp, thrust, cmd);
+#if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
+      if (in_flight) {
+        stabilization_filter_commands();
+      }
+#endif
+      break;
+    default:
+      break;
+  }
+
+  // store last attitude command
+  stabilization.sp = *sp;
+}
+
+struct StabilizationSetpoint stabilization_get_failsafe_sp(void)
+{
+  struct FloatEulers failsafe_sp = {
+    .phi = 0.f,
+    .theta = 0.f,
+    .psi = stateGetNedToBodyEulers_f()->psi
+  };
+  return stab_sp_from_eulers_f(&failsafe_sp);
 }
 
 // compute sp_euler phi/theta for debugging/telemetry FIXME really needed ?
@@ -107,16 +315,16 @@ void stabilization_filter_commands(void)
 {
   /* Filter the commands & bound the result */
 #if STABILIZATION_FILTER_CMD_ROLL_PITCH
-  stabilization_cmd[COMMAND_ROLL] = update_second_order_low_pass_int(&filter_roll, stabilization_cmd[COMMAND_ROLL]);
-  stabilization_cmd[COMMAND_PITCH] = update_second_order_low_pass_int(&filter_pitch, stabilization_cmd[COMMAND_PITCH]);
+  stabilization.cmd[COMMAND_ROLL] = update_second_order_low_pass_int(&filter_roll, stabilization.cmd[COMMAND_ROLL]);
+  stabilization.cmd[COMMAND_PITCH] = update_second_order_low_pass_int(&filter_pitch, stabilization.cmd[COMMAND_PITCH]);
 
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(stabilization.cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(stabilization.cmd[COMMAND_PITCH], MAX_PPRZ);
 #endif
 #if STABILIZATION_FILTER_CMD_YAW
-  stabilization_cmd[COMMAND_YAW] = update_second_order_low_pass_int(&filter_yaw, stabilization_cmd[COMMAND_YAW]);
+  stabilization.cmd[COMMAND_YAW] = update_second_order_low_pass_int(&filter_yaw, stabilization.cmd[COMMAND_YAW]);
 
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(stabilization.cmd[COMMAND_YAW], MAX_PPRZ);
 #endif
 }
 
@@ -316,6 +524,76 @@ struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
   }
 }
 
+int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
+{
+  if (th->type == THRUST_SP) {
+    if (th->format == THRUST_SP_INT) {
+      return th->sp.thrust_i[axis];
+    } else {
+      return (int32_t) (th->sp.thrust_f[axis] * MAX_PPRZ);
+    }
+  } else {
+    if (th->format == THRUST_SP_INT) {
+      return thrust + th->sp.th_incr_i[axis];
+    } else {
+      return thrust + (int32_t)(th->sp.th_incr_f[axis] * MAX_PPRZ);
+    }
+  }
+}
+
+float th_sp_to_thrust_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
+{
+  const float MAX_PPRZ_F = (float) MAX_PPRZ;
+  if (th->type == THRUST_SP) {
+    if (th->format == THRUST_SP_FLOAT) {
+      return th->sp.thrust_f[axis];
+    } else {
+      return (float)th->sp.thrust_i[axis] / MAX_PPRZ_F;
+    }
+  } else {
+    if (th->format == THRUST_SP_FLOAT) {
+      return ((float)thrust / MAX_PPRZ_F) + th->sp.th_incr_f[axis];
+    } else {
+      return (float)(thrust + th->sp.th_incr_f[axis]) / MAX_PPRZ_F;
+    }
+  }
+}
+
+int32_t th_sp_to_incr_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
+{
+  if (th->type == THRUST_INCR_SP) {
+    if (th->format == THRUST_SP_INT) {
+      return th->sp.th_incr_i[axis];
+    } else {
+      return (int32_t) (th->sp.th_incr_f[axis] * MAX_PPRZ);
+    }
+  } else {
+    if (th->format == THRUST_SP_INT) {
+      return th->sp.thrust_i[axis] - thrust;
+    } else {
+      return (int32_t)(th->sp.thrust_f[axis] * MAX_PPRZ) - thrust;
+    }
+  }
+}
+
+float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
+{
+  const float MAX_PPRZ_F = (float) MAX_PPRZ;
+  if (th->type == THRUST_INCR_SP) {
+    if (th->format == THRUST_SP_FLOAT) {
+      return th->sp.th_incr_f[axis];
+    } else {
+      return (float)th->sp.th_incr_i[axis] / MAX_PPRZ_F;
+    }
+  } else {
+    if (th->format == THRUST_SP_FLOAT) {
+      return th->sp.thrust_f[axis] - ((float)thrust / MAX_PPRZ_F);
+    } else {
+      return (float)(th->sp.thrust_i[axis] - thrust) / MAX_PPRZ_F;
+    }
+  }
+}
+
 struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat)
 {
   struct StabilizationSetpoint sp = {
@@ -409,3 +687,91 @@ struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
   return sp;
 }
 
+struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_SP,
+    .format = THRUST_SP_INT
+  };
+  sp.sp.thrust_i[axis] = thrust;
+  return sp;
+}
+
+struct ThrustSetpoint th_sp_from_thrust_f(float thrust, uint8_t axis)
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_SP,
+    .format = THRUST_SP_FLOAT
+  };
+  sp.sp.thrust_f[axis] = thrust;
+  return sp;
+}
+
+struct ThrustSetpoint th_sp_from_incr_i(int32_t th_increment, uint8_t axis)
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_INCR_SP,
+    .format = THRUST_SP_INT
+  };
+  sp.sp.th_incr_i[axis] = th_increment;
+  return sp;
+}
+
+struct ThrustSetpoint th_sp_from_incr_f(float th_increment, uint8_t axis)
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_INCR_SP,
+    .format = THRUST_SP_FLOAT
+  };
+  sp.sp.th_incr_f[axis] = th_increment;
+  return sp;
+}
+
+struct ThrustSetpoint th_sp_from_thrust_vect_i(int32_t thrust[3])
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_SP,
+    .format = THRUST_SP_INT
+  };
+  sp.sp.thrust_i[0] = thrust[0];
+  sp.sp.thrust_i[1] = thrust[1];
+  sp.sp.thrust_i[2] = thrust[2];
+  return sp;
+}
+
+struct ThrustSetpoint th_sp_from_thrust_vect_f(float thrust[3])
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_SP,
+    .format = THRUST_SP_FLOAT
+  };
+  sp.sp.thrust_f[0] = thrust[0];
+  sp.sp.thrust_f[1] = thrust[1];
+  sp.sp.thrust_f[2] = thrust[2];
+  return sp;
+}
+
+struct ThrustSetpoint th_sp_from_incr_vect_i(int32_t th_increment[3])
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_INCR_SP,
+    .format = THRUST_SP_INT
+  };
+  sp.sp.th_incr_i[0] = th_increment[0];
+  sp.sp.th_incr_i[1] = th_increment[1];
+  sp.sp.th_incr_i[2] = th_increment[2];
+  return sp;
+}
+
+struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3])
+{
+  struct ThrustSetpoint sp = {
+    .type = THRUST_INCR_SP,
+    .format = THRUST_SP_FLOAT
+  };
+  sp.sp.th_incr_f[0] = th_increment[0];
+  sp.sp.th_incr_f[1] = th_increment[1];
+  sp.sp.th_incr_f[2] = th_increment[2];
+  return sp;
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.h b/sw/airborne/firmwares/rotorcraft/stabilization.h
index 3dd867b089..8e1efdc18d 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization.h
@@ -31,9 +31,20 @@
 #include "generated/airframe.h"
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_algebra_float.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 
-extern void stabilization_init(void);
-extern void stabilization_filter_commands(void);
+/** Stabilization modes
+ */
+#define STABILIZATION_MODE_NONE     0
+#define STABILIZATION_MODE_DIRECT   1
+#define STABILIZATION_MODE_RATE     2
+#define STABILIZATION_MODE_ATTITUDE 3
+
+/** Stabilization sub-modes for attitude
+ */
+#define STABILIZATION_ATT_SUBMODE_HEADING   0 // direct heading control
+#define STABILIZATION_ATT_SUBMODE_CARE_FREE 1 // care free heading mode
+#define STABILIZATION_ATT_SUBMODE_FORWARD   2 // forward flight for hybrid-like
 
 /** Stabilization setpoint.
  *  Struture to store the desired attitude with different
@@ -65,6 +76,65 @@ struct StabilizationSetpoint {
   } r_sp;
 };
 
+/** Thrust setpoint // TODO to a setpoint header
+ *  Structure to store the desired thrust vector with different format
+ */
+struct ThrustSetpoint {
+  enum {
+    THRUST_SP,        ///< absolute thrust setpoint
+    THRUST_INCR_SP    ///< thrust increment
+  } type;
+  enum {
+    THRUST_SP_INT,    ///< int is assumed to be normalized in [0:MAX_PPRZ]
+    THRUST_SP_FLOAT   ///< float is assumed to be normalized in [0.:1.]
+  } format;
+  union {
+    int32_t thrust_i[3];
+    float thrust_f[3];
+    int32_t th_incr_i[3];
+    float th_incr_f[3];
+  } sp;
+};
+
+
+/** Stabilization structure
+ */
+struct Stabilization {
+  uint8_t mode;                       ///< current mode
+  uint8_t att_submode;                ///< current attitude sub-mode
+  struct AttitudeRCInput rc_in;       ///< RC input
+  struct StabilizationSetpoint rc_sp; ///< Keep it ? FIXME
+  struct StabilizationSetpoint sp;    ///< current attitude setpoint (store for messages)
+  int32_t cmd[COMMANDS_NB];           ///< output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
+  float transition_ratio;             ///< transition percentage for hybrids (0.: hover; 1.: forward)
+};
+
+extern struct Stabilization stabilization;
+
+/** Init function
+ */
+extern void stabilization_init(void);
+
+/** Check mode change
+ */
+extern void stabilization_mode_changed(uint8_t new_mode, uint8_t submode);
+
+/** Call default stabilization control
+ * @param[in] in_flight true if rotorcraft is flying
+ * @param[in] sp pointer to the stabilization setpoint, computed in guidance or from RC
+ * @param[in] thrust pointer to thrust setpoint computed by vertical guidance
+ * @param[out] cmd pointer to the output command vector
+ */
+extern void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd);
+
+/** Get stabilization setpoint for failsafe
+ */
+extern struct StabilizationSetpoint stabilization_get_failsafe_sp(void);
+
+/** Command filter for vibrating airframes
+ */
+extern void stabilization_filter_commands(void);
+
 // helper convert functions
 extern struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp);
 extern struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp);
@@ -72,6 +142,13 @@ extern struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp);
 extern struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp);
 extern struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp);
 extern struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp);
+// thrust setpoint helper functions
+// - first param is the thrust setpoint structure
+// - second param is the current thrust (expected in [0:MAX_PPRZ])
+extern int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
+extern float th_sp_to_thrust_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
+extern int32_t th_sp_to_incr_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
+extern float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
 
 // helper make functions
 extern struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat);
@@ -83,12 +160,33 @@ extern struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect,
 extern struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading);
 extern struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates);
 extern struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates);
+extern struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis);
+extern struct ThrustSetpoint th_sp_from_thrust_f(float thrust, uint8_t axis);
+extern struct ThrustSetpoint th_sp_from_incr_i(int32_t th_increment, uint8_t axis);
+extern struct ThrustSetpoint th_sp_from_incr_f(float th_increment, uint8_t axis);
+extern struct ThrustSetpoint th_sp_from_thrust_vect_i(int32_t thrust[3]);
+extern struct ThrustSetpoint th_sp_from_thrust_vect_f(float thrust[3]);
+extern struct ThrustSetpoint th_sp_from_incr_vect_i(int32_t th_increment[3]);
+extern struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3]);
 
-/** Stabilization commands.
- *  Contains the resulting stabilization commands,
- *  regardless of whether rate or attitude is currently used.
- *  Range -MAX_PPRZ:MAX_PPRZ
- */
-extern int32_t stabilization_cmd[COMMANDS_NB];
+#define THRUST_AXIS_X 0
+#define THRUST_AXIS_Y 1
+#define THRUST_AXIS_Z 2
+
+#define STAB_SP_SET_EULERS_ZERO(_sp) { \
+  _sp.type = STAB_SP_EULERS;  \
+  _sp.format = STAB_SP_INT;   \
+  _sp.sp.eulers_i.phi = 0;    \
+  _sp.sp.eulers_i.theta = 0;  \
+  _sp.sp.eulers_i.psi = 0;    \
+}
+
+#define THRUST_SP_SET_ZERO(_sp) { \
+  _sp.type = THRUST_SP;       \
+  _sp.format = THRUST_SP_INT; \
+  _sp.sp.thrust_i[0] = 0;     \
+  _sp.sp.thrust_i[1] = 0;     \
+  _sp.sp.thrust_i[2] = 0;     \
+}
 
 #endif /* STABILIZATION_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
index 9e574304da..a8e2c4e331 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
@@ -32,18 +32,38 @@ extern "C" {
 #endif
 
 #include "firmwares/rotorcraft/stabilization.h"
-#include "math/pprz_algebra_int.h"
-#include STABILIZATION_ATTITUDE_TYPE_H
+#include "modules/radio_control/radio_control.h"
 
+/** Stabilization init function
+ *
+ * needs to be implemented by the selected controller
+ */
 extern void stabilization_attitude_init(void);
-extern void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
+
+/** Retrun attitude setpoint from RC as euler angles
+ *
+ * weak function that can be re-implemeted if needed
+ *
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ * @return stabilization setpoint
+ */
+extern struct StabilizationSetpoint stabilization_attitude_read_rc(bool in_flight, bool carefree, bool coordinated_turn, struct RadioControl *rc);
+
+/** Attitude control enter function
+ */
 extern void stabilization_attitude_enter(void);
-extern void stabilization_attitude_set_failsafe_setpoint(void);
-extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy);
-extern void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat);
-extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
-extern void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp);
-extern void stabilization_attitude_run(bool in_flight);
+
+/** Attitude control run function
+ *
+ * @param[in] in_flight         true if in flight
+ * @param[in] sp                pointer to the stabilization setpoint structure
+ * @param[in] thrust            pointer to the thrust setoint structure
+ * @param[out] cmd              pointer to the output command vector
+ */
+extern void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd);
 
 #ifdef __cplusplus
 }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
index 46f47492e4..ed77f94846 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
@@ -27,8 +27,8 @@
 
 #include "generated/airframe.h"
 
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h"
 
 #include "std.h"
 #include "paparazzi.h"
@@ -42,8 +42,8 @@
 struct FloatAttitudeGains stabilization_gains;
 struct FloatEulers stabilization_att_sum_err;
 
-struct FloatEulers stab_att_sp_euler;
-struct AttRefEulerFloat att_ref_euler_f;
+static struct FloatEulers stab_att_sp_euler;
+static struct AttRefEulerFloat att_ref_euler_f;
 
 float stabilization_att_fb_cmd[COMMANDS_NB];
 float stabilization_att_ff_cmd[COMMANDS_NB];
@@ -71,9 +71,9 @@ static void send_att(struct transport_tx *trans, struct link_device *dev)
                                     &stabilization_att_ff_cmd[COMMAND_ROLL],
                                     &stabilization_att_ff_cmd[COMMAND_PITCH],
                                     &stabilization_att_ff_cmd[COMMAND_YAW],
-                                    &stabilization_cmd[COMMAND_ROLL],
-                                    &stabilization_cmd[COMMAND_PITCH],
-                                    &stabilization_cmd[COMMAND_YAW],
+                                    &stabilization.cmd[COMMAND_ROLL],
+                                    &stabilization.cmd[COMMAND_PITCH],
+                                    &stabilization.cmd[COMMAND_YAW],
                                     &foo, &foo, &foo);
 }
 
@@ -95,7 +95,7 @@ static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
 }
 #endif
 
-void stabilization_attitude_init(void)
+void stabilization_attitude_euler_float_init(void)
 {
 
   attitude_ref_euler_float_init(&att_ref_euler_f);
@@ -128,66 +128,19 @@ void stabilization_attitude_init(void)
 #endif
 }
 
-void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
-}
-
 void stabilization_attitude_enter(void)
 {
-
-  /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
-
-  attitude_ref_euler_float_enter(&att_ref_euler_f, stab_att_sp_euler.psi);
+  /* reset psi ref to current psi angle */
+  attitude_ref_euler_float_enter(&att_ref_euler_f, stabilization_attitude_get_heading_f());
 
   FLOAT_EULERS_ZERO(stabilization_att_sum_err);
 }
 
-void stabilization_attitude_set_failsafe_setpoint(void)
-{
-  stab_att_sp_euler.phi = 0.0;
-  stab_att_sp_euler.theta = 0.0;
-  stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi;
-}
-
-void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy);
-}
-
-void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  struct FloatQuat quat_f;
-  QUAT_FLOAT_OF_BFP(quat_f, *quat);
-  float_eulers_of_quat(&stab_att_sp_euler, &quat_f);
-}
-
-void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  struct FloatVect2 cmd_f;
-  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
-  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
-
-  /* Rotate horizontal commands to body frame by psi */
-  float psi = stateGetNedToBodyEulers_f()->psi;
-  float s_psi = sinf(psi);
-  float c_psi = cosf(psi);
-  stab_att_sp_euler.phi = -s_psi * cmd_f.x + c_psi * cmd_f.y;
-  stab_att_sp_euler.theta = -c_psi * cmd_f.x - s_psi * cmd_f.y;
-  stab_att_sp_euler.psi = ANGLE_FLOAT_OF_BFP(heading);
-}
-
-void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_f(sp);
-}
-
 #define MAX_SUM_ERR 200
 
-void stabilization_attitude_run(bool  in_flight)
+void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
-
+  stab_att_sp_euler = stab_sp_to_eulers_f(sp);
 #if USE_ATT_REF
   static const float dt = (1./PERIODIC_FREQUENCY);
   attitude_ref_euler_float_update(&att_ref_euler_f, &stab_att_sp_euler, dt);
@@ -243,15 +196,14 @@ void stabilization_attitude_run(bool  in_flight)
     stabilization_gains.i.z  * stabilization_att_sum_err.psi;
 
 
-  stabilization_cmd[COMMAND_ROLL] =
-    (stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]);
-  stabilization_cmd[COMMAND_PITCH] =
-    (stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]);
-  stabilization_cmd[COMMAND_YAW] =
-    (stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]);
+  cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
+  cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
+  cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
 
   /* bound the result */
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
 }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h
index ca9a044d71..6e4b1441ac 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h
@@ -28,14 +28,12 @@
 #ifndef STABILIZATION_ATTITUDE_EULER_FLOAT_H
 #define STABILIZATION_ATTITUDE_EULER_FLOAT_H
 
-#include "math/pprz_algebra_float.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h"
+
+extern stabilization_attitude_euler_float_init(void);
 
 extern struct FloatAttitudeGains stabilization_gains;
 extern struct FloatEulers stabilization_att_sum_err;
 
-extern struct FloatEulers stab_att_sp_euler;
-extern struct AttRefEulerFloat att_ref_euler_f;
-
 #endif /* STABILIZATION_ATTITUDE_EULER_FLOAT_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
index d733d1f8a3..ac1d7b250d 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -25,14 +25,12 @@
  * Rotorcraft attitude stabilization in euler int version.
  */
 
+#include "std.h"
 #include "generated/airframe.h"
 
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
-
-#include "std.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h"
 #include "paparazzi.h"
-#include "math/pprz_algebra_int.h"
 #include "state.h"
 
 /** explicitly define to zero to disable attitude reference generation */
@@ -71,13 +69,13 @@ struct Int32Eulers stabilization_att_sum_err;
 int32_t stabilization_att_fb_cmd[COMMANDS_NB];
 int32_t stabilization_att_ff_cmd[COMMANDS_NB];
 
-struct Int32Eulers stab_att_sp_euler;
-struct AttRefEulerInt att_ref_euler_i;
+static struct Int32Eulers stab_att_sp_euler;
+static struct AttRefEulerInt att_ref_euler_i;
 
 static inline void reset_psi_ref_from_body(void)
 {
   //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
-  att_ref_euler_i.euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
+  att_ref_euler_i.euler.psi = stateGetNedToBodyEulers_i()->psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
   att_ref_euler_i.rate.r = 0;
   att_ref_euler_i.accel.r = 0;
 }
@@ -104,9 +102,9 @@ static void send_att(struct transport_tx *trans, struct link_device *dev)
                                   &stabilization_att_ff_cmd[COMMAND_ROLL],
                                   &stabilization_att_ff_cmd[COMMAND_PITCH],
                                   &stabilization_att_ff_cmd[COMMAND_YAW],
-                                  &stabilization_cmd[COMMAND_ROLL],
-                                  &stabilization_cmd[COMMAND_PITCH],
-                                  &stabilization_cmd[COMMAND_YAW]);
+                                  &stabilization.cmd[COMMAND_ROLL],
+                                  &stabilization.cmd[COMMAND_PITCH],
+                                  &stabilization.cmd[COMMAND_YAW]);
 }
 
 static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
@@ -127,7 +125,7 @@ static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
 }
 #endif
 
-void stabilization_attitude_init(void)
+void stabilization_attitude_euler_int_init(void)
 {
 
   INT_EULERS_ZERO(stab_att_sp_euler);
@@ -168,59 +166,20 @@ void stabilization_attitude_init(void)
 #endif
 }
 
-void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
-}
-
 void stabilization_attitude_enter(void)
 {
-  stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
   reset_psi_ref_from_body();
   INT_EULERS_ZERO(stabilization_att_sum_err);
 }
 
-void stabilization_attitude_set_failsafe_setpoint(void)
-{
-  stab_att_sp_euler.phi = 0;
-  stab_att_sp_euler.theta = 0;
-  stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
-}
-
-void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  stab_att_sp_euler = *rpy;
-}
-
-void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  int32_eulers_of_quat(&stab_att_sp_euler, quat);
-}
-
-void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  /* Rotate horizontal commands to body frame by psi */
-  int32_t psi = stateGetNedToBodyEulers_i()->psi;
-  int32_t s_psi, c_psi;
-  PPRZ_ITRIG_SIN(s_psi, psi);
-  PPRZ_ITRIG_COS(c_psi, psi);
-  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.psi = heading;
-}
-
-void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
-}
-
 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
 
 #define MAX_SUM_ERR 4000000
 
-void stabilization_attitude_run(bool  in_flight)
+void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
+  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
 
   /* update reference */
 #if USE_ATTITUDE_REF
@@ -303,18 +262,21 @@ void stabilization_attitude_run(bool  in_flight)
 #define CMD_SHIFT 11
 
   /* sum feedforward and feedback */
-  stabilization_cmd[COMMAND_ROLL] =
+  cmd[COMMAND_ROLL] =
     OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]), CMD_SHIFT);
 
-  stabilization_cmd[COMMAND_PITCH] =
+  cmd[COMMAND_PITCH] =
     OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]), CMD_SHIFT);
 
-  stabilization_cmd[COMMAND_YAW] =
+  cmd[COMMAND_YAW] =
     OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT);
 
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
+
   /* bound the result */
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
 
 }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h
index 996c5c1a39..7f5d74a60b 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h
@@ -22,12 +22,11 @@
 #ifndef STABILIZATION_ATTITUDE_EULER_INT_H
 #define STABILIZATION_ATTITUDE_EULER_INT_H
 
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h"
+
+extern stabilization_attitude_euler_int_init(void);
 
 extern struct Int32Eulers stabilization_att_sum_err;
 
-extern struct Int32Eulers stab_att_sp_euler;
-extern struct AttRefEulerInt att_ref_euler_i;
-
 #endif /* STABILIZATION_ATTITUDE_EULER_INT_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
index 247b7bac88..6ce2c504e8 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
@@ -33,8 +33,6 @@
 #include "autopilot.h"
 
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
 #include "filters/low_pass_filter.h"
 
@@ -124,9 +122,9 @@
 #define INVG_33 -50000 // Not used at the moment
 #define INT32_INVG_FRAC 16
 
-struct Int32Quat   stab_att_sp_quat;
-struct Int32Eulers stab_att_sp_euler;
-struct Int32Quat sp_offset; // non-zero neutral attitude
+static struct Int32Quat   stab_att_sp_quat;
+static struct Int32Eulers stab_att_sp_euler;
+static struct Int32Quat sp_offset; // non-zero neutral attitude
 
 struct HeliIndiGains heli_indi_gains = {
   STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P,
@@ -136,7 +134,7 @@ struct HeliIndiGains heli_indi_gains = {
 };
 
 /* Main controller struct */
-struct IndiController_int heli_indi_ctl;
+static struct IndiController_int heli_indi_ctl;
 
 /* Filter functions */
 struct delayed_first_order_lowpass_filter_t actuator_model[INDI_DOF];
@@ -157,12 +155,6 @@ void indi_apply_measurement_notch_filters(int32_t _out[], int32_t _in[]);
 void indi_apply_actuator_butterworth_filters(int32_t _out[], int32_t _in[]);
 void indi_apply_measurement_butterworth_filters(int32_t _out[], int32_t _in[]);
 
-#if PERIODIC_TELEMETRY
-#include "modules/datalink/telemetry.h"
-
-/* Telemetry messages here, at the moment there are none */
-
-#endif // PERIODIC_TELEMETRY
 
 static inline void indi_apply_actuator_models(int32_t _out[], int32_t _in[])
 {
@@ -335,11 +327,11 @@ void stabilization_attitude_heli_indi_set_steadystate_pitchroll()
 }
 
 /**
- * @brief stabilization_attitude_init
+ * @brief stabilization_attitude_heli_indi_init
  *
  * Initialize the heli indi attitude controller.
  */
-void stabilization_attitude_init(void)
+void stabilization_attitude_heli_indi_init(void)
 {
   /* Initialization code INDI */
   struct IndiController_int *c = &heli_indi_ctl;
@@ -425,17 +417,17 @@ void stabilization_attitude_init(void)
   c->apply_measurement_filters[1] = &indi_apply_measurement_butterworth_filters;
   c->apply_actuator_filters[0] = &indi_apply_actuator_notch_filters;
   c->apply_actuator_filters[1] = &indi_apply_actuator_butterworth_filters;
-
-#if PERIODIC_TELEMETRY
-  //register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_<<MSG>>, function);
-#endif
 }
 
-void stabilization_attitude_run(bool in_flight)
+void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
   (void) in_flight; // unused variable
   struct IndiController_int *c = &heli_indi_ctl;
 
+  /* set setpoint */
+  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+  stab_att_sp_quat = stab_sp_to_quat_i(sp);
+
   /* calculate acceleration in body frame */
   struct NedCoor_i *ltp_accel_nedcoor = stateGetAccelNed_i();
   struct Int32Vect3 ltp_accel;
@@ -532,7 +524,7 @@ void stabilization_attitude_run(bool in_flight)
 
   /* Take the current (filtered) actuator position and add the incremental value. */
   int32_vect_sum(c->u_setpoint, c->filtered_actuator[INDI_NR_FILTERS - 1], c->du, INDI_DOF);
-  //c->u_setpoint[INDI_THRUST] = stabilization_cmd[COMMAND_THRUST];
+  //c->u_setpoint[INDI_THRUST] = stabilization.cmd[COMMAND_THRUST];
 
   /* bound the result */
   BoundAbs(c->u_setpoint[INDI_ROLL], MAX_PPRZ);
@@ -549,79 +541,20 @@ void stabilization_attitude_run(bool in_flight)
   /* Two correction angles, don't rotate but just add.
    * sin/cos = tan
    */
-  stabilization_cmd[COMMAND_ROLL] = c->command_out[__k][INDI_ROLL]
-                                    + c->command_out[__k][INDI_PITCH] * pprz_itrig_sin(c->pitch_comp_angle) / pprz_itrig_cos(c->pitch_comp_angle);
-  stabilization_cmd[COMMAND_PITCH] = c->command_out[__k][INDI_PITCH]
-                                     + c->command_out[__k][INDI_ROLL] * pprz_itrig_sin(c->roll_comp_angle) / pprz_itrig_cos(c->roll_comp_angle);
-  stabilization_cmd[COMMAND_YAW] = c->command_out[__k][INDI_YAW];
-  /* Thrust is not applied */
+  cmd[COMMAND_ROLL] = c->command_out[__k][INDI_ROLL]
+                      + c->command_out[__k][INDI_PITCH] * pprz_itrig_sin(c->pitch_comp_angle) / pprz_itrig_cos(c->pitch_comp_angle);
+  cmd[COMMAND_PITCH] = c->command_out[__k][INDI_PITCH]
+                       + c->command_out[__k][INDI_ROLL] * pprz_itrig_sin(c->roll_comp_angle) / pprz_itrig_cos(c->roll_comp_angle);
+  cmd[COMMAND_YAW] = c->command_out[__k][INDI_YAW];
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
 
   /* Disable tail when not armed, because this thing goes crazy */
   if (!autopilot_get_motors_on()) {
-    stabilization_cmd[COMMAND_YAW] = 0;
+    cmd[COMMAND_YAW] = 0;
   }
 }
 
 void stabilization_attitude_enter(void)
 {
-  /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
 }
 
-void stabilization_attitude_set_failsafe_setpoint(void)
-{
-  /* set failsafe to zero roll/pitch and current heading */
-  int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
-  PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
-  stab_att_sp_quat.qx = 0;
-  stab_att_sp_quat.qy = 0;
-  PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
-}
-
-void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler = *rpy;
-
-  int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
-}
-
-void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  stab_att_sp_quat = *quat;
-  int32_eulers_of_quat(&stab_att_sp_euler, quat);
-}
-
-void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler.psi = heading;
-
-  // compute sp_euler phi/theta for debugging/telemetry
-  /* Rotate horizontal commands to body frame by psi */
-  int32_t psi = stateGetNedToBodyEulers_i()->psi;
-  int32_t s_psi, c_psi;
-  PPRZ_ITRIG_SIN(s_psi, psi);
-  PPRZ_ITRIG_COS(c_psi, psi);
-  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
-
-  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
-}
-
-void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
-  stab_att_sp_quat = stab_sp_to_quat_i(sp);
-}
-
-void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  struct FloatQuat q_sp;
-#if USE_EARTH_BOUND_RC_SETPOINT
-  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#else
-  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#endif
-  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
-}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.h
index 128c6b727a..4f7502fefd 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.h
@@ -22,8 +22,8 @@
 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_H
 #define STABILIZATION_ATTITUDE_HELI_INDI_H
 
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "math/pprz_algebra_int.h"
-
 #include "filters/notch_filter.h"
 #include "filters/delayed_first_order_lowpass_filter.h"
 
@@ -67,13 +67,10 @@ struct IndiController_int {
   void (*apply_measurement_filters[INDI_NR_FILTERS])(int32_t _out[], int32_t _in[]);
 };
 
-//extern struct IndiController_int heli_indi_ctl; // keep private
-
 extern struct delayed_first_order_lowpass_filter_t actuator_model[INDI_DOF];
-extern struct Int32Quat   stab_att_sp_quat;  ///< with #INT32_QUAT_FRAC
-extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
 extern struct HeliIndiGains heli_indi_gains;
 
+extern void stabilization_attitude_heli_indi_init(void);
 extern void stabilization_attitude_heli_indi_set_steadystate_pitch(float pitch);
 extern void stabilization_attitude_heli_indi_set_steadystate_roll(float roll);
 extern void stabilization_attitude_heli_indi_set_steadystate_pitchroll(void);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
index 68e8577cc0..dad0b8d236 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
@@ -28,85 +28,37 @@
  * software onboard or just does not need it at all.
  */
 
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h"
 #include "state.h"
-#include "firmwares/rotorcraft/stabilization.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "paparazzi.h"
-
+#include "firmwares/rotorcraft/stabilization.h"
 #include "generated/airframe.h"
 
 #define TRAJ_MAX_BANK (int32_t)ANGLE_BFP_OF_REAL(GUIDANCE_H_MAX_BANK)
 
-struct Int32Eulers stab_att_sp_euler;
-
-
-void stabilization_attitude_init(void)
-{
-  INT_EULERS_ZERO(stab_att_sp_euler);
-}
-
-void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  //Read from RC
-  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
-}
-
 void stabilization_attitude_enter(void)
 {
-
 }
 
-void stabilization_attitude_run(bool  in_flight __attribute__((unused)))
+void stabilization_attitude_run(bool in_flight __attribute__((unused)), struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
+  struct Int32Eulers sp_euler = stab_sp_to_eulers_i(sp);
 
   /* For roll and pitch we pass trough the desired angles as stabilization command */
   const int32_t angle2cmd = (MAX_PPRZ / TRAJ_MAX_BANK);
-  stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd;
-  stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd;
+  cmd[COMMAND_ROLL] = sp_euler.phi * angle2cmd;
+  cmd[COMMAND_PITCH] = sp_euler.theta * angle2cmd;
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
 
   //TODO: Fix yaw with PID controller
-  int32_t yaw_error = stateGetNedToBodyEulers_i()->psi - stab_att_sp_euler.psi;
+  int32_t yaw_error = stateGetNedToBodyEulers_i()->psi - sp_euler.psi;
   INT32_ANGLE_NORMALIZE(yaw_error);
-  //  stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI;
+  //  cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI;
 
   /* bound the result */
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
-}
-
-void stabilization_attitude_set_failsafe_setpoint(void)
-{
-  stab_att_sp_euler.phi = 0;
-  stab_att_sp_euler.theta = 0;
-  stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
-}
-
-void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  stab_att_sp_euler = *rpy;
-}
-
-void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  int32_eulers_of_quat(&stab_att_sp_euler, quat);
-}
-
-void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  /* Rotate horizontal commands to body frame by psi */
-  int32_t psi = stateGetNedToBodyEulers_i()->psi;
-  int32_t s_psi, c_psi;
-  PPRZ_ITRIG_SIN(s_psi, psi);
-  PPRZ_ITRIG_COS(c_psi, psi);
-  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.psi = heading;
-}
-
-void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
index 01755a987e..afd6784664 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
@@ -22,9 +22,7 @@
 #ifndef STABILIZATION_ATTITUDE_PASSTHROUGH_H
 #define STABILIZATION_ATTITUDE_PASSTHROUGH_H
 
-#include "math/pprz_algebra_int.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
 
-extern struct Int32Eulers stab_att_sp_euler;
-
 #endif /* STABILIZATION_ATTITUDE_PASSTHROUGH_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
index 3483dab5c1..874d3e7054 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
@@ -25,8 +25,7 @@
 
 #include "generated/airframe.h"
 
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
 
 #include "std.h"
@@ -37,15 +36,15 @@
 
 struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB];
 
-struct FloatEulers stab_att_sp_euler;
-struct FloatQuat   stab_att_sp_quat;
+static struct FloatEulers stab_att_sp_euler;
+static struct FloatQuat   stab_att_sp_quat;
 
 struct AttRefQuatFloat att_ref_quat_f;
 
 struct FloatQuat stabilization_att_sum_err_quat;
 
-struct FloatRates last_body_rate;
-struct FloatRates body_rate_d;
+static struct FloatRates last_body_rate;
+static struct FloatRates body_rate_d;
 
 float stabilization_att_fb_cmd[COMMANDS_NB];
 float stabilization_att_ff_cmd[COMMANDS_NB];
@@ -119,9 +118,9 @@ static void send_att(struct transport_tx *trans, struct link_device *dev)
                                     &stabilization_att_ff_cmd[COMMAND_ROLL],
                                     &stabilization_att_ff_cmd[COMMAND_PITCH],
                                     &stabilization_att_ff_cmd[COMMAND_YAW],
-                                    &stabilization_cmd[COMMAND_ROLL],
-                                    &stabilization_cmd[COMMAND_PITCH],
-                                    &stabilization_cmd[COMMAND_YAW],
+                                    &stabilization.cmd[COMMAND_ROLL],
+                                    &stabilization.cmd[COMMAND_PITCH],
+                                    &stabilization.cmd[COMMAND_YAW],
                                     &body_rate_d.p, &body_rate_d.q, &body_rate_d.r);
 }
 
@@ -159,7 +158,7 @@ static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *d
 }
 #endif
 
-void stabilization_attitude_init(void)
+void stabilization_attitude_quat_float_init(void)
 {
   /* setpoints */
   FLOAT_EULERS_ZERO(stab_att_sp_euler);
@@ -205,58 +204,12 @@ void stabilization_attitude_gain_schedule(uint8_t idx)
 
 void stabilization_attitude_enter(void)
 {
-
-  /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
-
   struct FloatQuat *state_quat = stateGetNedToBodyQuat_f();
-
   attitude_ref_quat_float_enter(&att_ref_quat_f, state_quat);
 
   float_quat_identity(&stabilization_att_sum_err_quat);
 }
 
-void stabilization_attitude_set_failsafe_setpoint(void)
-{
-  /* set failsafe to zero roll/pitch and current heading */
-  float heading2 = stabilization_attitude_get_heading_f() / 2;
-  stab_att_sp_quat.qi = cosf(heading2);
-  stab_att_sp_quat.qx = 0.0;
-  stab_att_sp_quat.qy = 0.0;
-  stab_att_sp_quat.qz = sinf(heading2);
-}
-
-void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  // copy euler setpoint for debugging
-  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy);
-
-  float_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
-}
-
-void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  QUAT_FLOAT_OF_BFP(stab_att_sp_quat, *quat);
-  float_eulers_of_quat(&stab_att_sp_euler, &stab_att_sp_quat);
-}
-
-void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  struct FloatVect2 cmd_f;
-  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
-  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
-  float heading_f;
-  heading_f = ANGLE_FLOAT_OF_BFP(heading);
-
-  quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f);
-}
-
-void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_f(sp);
-  stab_att_sp_quat = stab_sp_to_quat_f(sp);
-}
-
 #ifndef GAIN_PRESCALER_FF
 #define GAIN_PRESCALER_FF 1
 #endif
@@ -323,8 +276,10 @@ static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gain
 #endif
 }
 
-void stabilization_attitude_run(bool enable_integrator)
+void stabilization_attitude_run(bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
+  stab_att_sp_euler = stab_sp_to_eulers_f(sp);
+  stab_att_sp_quat = stab_sp_to_quat_f(sp);
 
   /*
    * Update reference
@@ -370,31 +325,29 @@ void stabilization_attitude_run(bool enable_integrator)
   attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d,
                   &stabilization_att_sum_err_quat);
 
-  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
-  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
-  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
+  cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
+  cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
+  cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
 
 #ifdef HAS_SURFACE_COMMANDS
-  stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] +
+  cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] +
       stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
-  stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] +
+  cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] +
       stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
-  stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] +
+  cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] +
       stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
 #endif
 
   /* bound the result */
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
+#ifdef HAS_SURFACE_COMMANDS
+  BoundAbs(cmd[COMMAND_ROLL_SURFACE], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH_SURFACE], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW_SURFACE], MAX_PPRZ);
+#endif
 }
 
-void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-#if USE_EARTH_BOUND_RC_SETPOINT
-  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&stab_att_sp_quat, in_flight, in_carefree, coordinated_turn);
-#else
-  stabilization_attitude_read_rc_setpoint_quat_f(&stab_att_sp_quat, in_flight, in_carefree, coordinated_turn);
-#endif
-  //float_quat_wrap_shortest(&stab_att_sp_quat);
-}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h
index 7edc3d11ac..372ce4e92e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h
@@ -29,6 +29,7 @@
 #define STABILIZATION_ATTITUDE_QUAT_FLOAT_H
 
 #include "generated/airframe.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h"
 
@@ -43,13 +44,11 @@
 
 extern struct FloatAttitudeGains stabilization_gains[];
 
-extern struct FloatEulers stab_att_sp_euler;
-extern struct FloatQuat   stab_att_sp_quat;
-
 extern struct AttRefQuatFloat att_ref_quat_f;
 
+extern void stabilization_attitude_quat_float_init(void);
 
-void stabilization_attitude_gain_schedule(uint8_t idx);
+extern void stabilization_attitude_gain_schedule(uint8_t idx);
 
 /* settings handlers */
 #define stabilization_attitude_quat_float_SetOmegaP(_val) {      \
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
index 77ee0f0bce..bfcbba277c 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
@@ -29,50 +29,16 @@
  * Dynamic Inversion for Attitude Control of Micro Aerial Vehicles
  */
 
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h"
 
 
-void stabilization_attitude_init(void)
-{
-  // indi init is already done through module init
-}
-
 void stabilization_attitude_enter(void)
 {
   stabilization_indi_enter();
 }
 
-void stabilization_attitude_set_failsafe_setpoint(void)
+void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
-  stabilization_indi_set_failsafe_setpoint();
+  stabilization_indi_attitude_run(in_flight, sp, thrust, cmd);
 }
 
-void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  stabilization_indi_set_rpy_setpoint_i(rpy);
-}
-
-void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  stabilization_indi_set_quat_setpoint_i(quat);
-}
-
-void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  stabilization_indi_set_earth_cmd_i(cmd, heading);
-}
-
-void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stabilization_indi_set_stab_sp(sp);
-}
-
-void stabilization_attitude_run(bool in_flight)
-{
-  stabilization_indi_attitude_run(stab_att_sp_quat, in_flight);
-}
-
-void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  stabilization_indi_read_rc(in_flight, in_carefree, coordinated_turn);
-}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h
index 097de1b2b2..da2e7e9848 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h
@@ -33,6 +33,7 @@
 #ifndef STABILIZATION_ATTITUDE_QUAT_INDI_H
 #define STABILIZATION_ATTITUDE_QUAT_INDI_H
 
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE
 #include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
 #else
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
index 56be04cb93..a102c647e5 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -25,8 +25,7 @@
 
 #include "generated/airframe.h"
 
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
 
 #include "std.h"
@@ -72,8 +71,8 @@ struct Int32Quat stabilization_att_sum_err_quat;
 int32_t stabilization_att_fb_cmd[COMMANDS_NB];
 int32_t stabilization_att_ff_cmd[COMMANDS_NB];
 
-struct Int32Quat   stab_att_sp_quat;
-struct Int32Eulers stab_att_sp_euler;
+static struct Int32Quat   stab_att_sp_quat;
+static struct Int32Eulers stab_att_sp_euler;
 
 struct AttRefQuatInt att_ref_quat_i;
 
@@ -105,9 +104,9 @@ static void send_att(struct transport_tx *trans, struct link_device *dev)   //FI
                                   &stabilization_att_ff_cmd[COMMAND_ROLL],
                                   &stabilization_att_ff_cmd[COMMAND_PITCH],
                                   &stabilization_att_ff_cmd[COMMAND_YAW],
-                                  &stabilization_cmd[COMMAND_ROLL],
-                                  &stabilization_cmd[COMMAND_PITCH],
-                                  &stabilization_cmd[COMMAND_YAW]);
+                                  &stabilization.cmd[COMMAND_ROLL],
+                                  &stabilization.cmd[COMMAND_PITCH],
+                                  &stabilization.cmd[COMMAND_YAW]);
 }
 
 static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
@@ -145,11 +144,9 @@ static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *d
 }
 #endif
 
-void stabilization_attitude_init(void)
+void stabilization_attitude_quat_int_init(void)
 {
-
   attitude_ref_quat_int_init(&att_ref_quat_i);
-
   int32_quat_identity(&stabilization_att_sum_err_quat);
 
 #if PERIODIC_TELEMETRY
@@ -161,63 +158,10 @@ void stabilization_attitude_init(void)
 
 void stabilization_attitude_enter(void)
 {
-
-  /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
-
   struct Int32Quat *state_quat = stateGetNedToBodyQuat_i();
-
   attitude_ref_quat_int_enter(&att_ref_quat_i, state_quat);
 
   int32_quat_identity(&stabilization_att_sum_err_quat);
-
-}
-
-void stabilization_attitude_set_failsafe_setpoint(void)
-{
-  /* set failsafe to zero roll/pitch and current heading */
-  int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
-  PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
-  stab_att_sp_quat.qx = 0;
-  stab_att_sp_quat.qy = 0;
-  PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
-}
-
-void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler = *rpy;
-
-  int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
-}
-
-void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  stab_att_sp_quat = *quat;
-  int32_eulers_of_quat(&stab_att_sp_euler, quat);
-}
-
-void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler.psi = heading;
-
-  // compute sp_euler phi/theta for debugging/telemetry
-  /* Rotate horizontal commands to body frame by psi */
-  int32_t psi = stateGetNedToBodyEulers_i()->psi;
-  int32_t s_psi, c_psi;
-  PPRZ_ITRIG_SIN(s_psi, psi);
-  PPRZ_ITRIG_COS(c_psi, psi);
-  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
-
-  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
-}
-
-void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
-  stab_att_sp_quat = stab_sp_to_quat_i(sp);
 }
 
 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
@@ -253,8 +197,10 @@ static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *ga
 
 }
 
-void stabilization_attitude_run(bool enable_integrator)
+void stabilization_attitude_run(bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
+  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+  stab_att_sp_quat = stab_sp_to_quat_i(sp);
 
   /*
    * Update reference
@@ -307,23 +253,15 @@ void stabilization_attitude_run(bool enable_integrator)
   attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);
 
   /* sum feedforward and feedback */
-  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
-  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
-  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
+  cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
+  cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
+  cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
 
   /* bound the result */
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
 }
 
-void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  struct FloatQuat q_sp;
-#if USE_EARTH_BOUND_RC_SETPOINT
-  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#else
-  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#endif
-  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
-}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
index b9a05837cd..f4ef0a0acb 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
@@ -22,16 +22,16 @@
 #ifndef STABILIZATION_ATTITUDE_QUAT_INT_H
 #define STABILIZATION_ATTITUDE_QUAT_INT_H
 
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
 
 #include "math/pprz_algebra_int.h"
 
-extern struct Int32Quat   stab_att_sp_quat;  ///< with #INT32_QUAT_FRAC
-extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
-
 extern struct AttRefQuatInt att_ref_quat_i;
 
+extern void stabilization_attitude_quat_int_init(void);
+
 /* settings handlers for ref model params */
 #define stabilization_attitude_quat_int_SetOmegaP(_val) {   \
     attitude_ref_quat_int_set_omega_p(&att_ref_quat_i, _val);   \
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
index 59dff4d9ac..e069b53536 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
@@ -50,341 +50,81 @@
 #define COORDINATED_TURN_AIRSPEED 12.0
 #endif
 
-#define YAW_DEADBAND_EXCEEDED()                                         \
-  (radio_control.values[RADIO_YAW] >  STABILIZATION_ATTITUDE_DEADBAND_R || \
-   radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
+#define YAW_DEADBAND_EXCEEDED(_rc)                               \
+  (rc->values[RADIO_YAW] >  STABILIZATION_ATTITUDE_DEADBAND_R || \
+   rc->values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
 
-float care_free_heading = 0;
-int32_t transition_theta_offset = 0;
-
-static int32_t get_rc_roll(void)
+static int32_t get_rc_roll(struct RadioControl *rc)
 {
   const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
-  int32_t roll = radio_control.values[RADIO_ROLL];
-#if STABILIZATION_ATTITUDE_DEADBAND_A
+  int32_t roll = rc->values[RADIO_ROLL];
   DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
   return roll * max_rc_phi / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
-#else
-  return roll * max_rc_phi / MAX_PPRZ;
-#endif
 }
 
-static int32_t get_rc_pitch(void)
+static int32_t get_rc_pitch(struct RadioControl *rc)
 {
   const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
-  int32_t pitch = radio_control.values[RADIO_PITCH];
-#if STABILIZATION_ATTITUDE_DEADBAND_E
+  int32_t pitch = rc->values[RADIO_PITCH];
   DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
   return pitch * max_rc_theta / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
-#else
-  return pitch * max_rc_theta / MAX_PPRZ;
-#endif
 }
 
-static int32_t get_rc_yaw(void)
+static int32_t get_rc_yaw(struct RadioControl *rc)
 {
   const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
-  int32_t yaw = radio_control.values[RADIO_YAW];
+  int32_t yaw = rc->values[RADIO_YAW];
   DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
   return yaw * max_rc_r / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
 }
 
-static float get_rc_roll_f(void)
+static float get_rc_roll_f(struct RadioControl *rc)
 {
-  int32_t roll = radio_control.values[RADIO_ROLL];
-#if STABILIZATION_ATTITUDE_DEADBAND_A
+  int32_t roll = rc->values[RADIO_ROLL];
   DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
   return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
-#else
-  return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ;
-#endif
 }
 
-static float get_rc_pitch_f(void)
+static float get_rc_pitch_f(struct RadioControl *rc)
 {
-  int32_t pitch = radio_control.values[RADIO_PITCH];
-#if STABILIZATION_ATTITUDE_DEADBAND_E
+  int32_t pitch = rc->values[RADIO_PITCH];
   DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
   return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
-#else
-  return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ;
-#endif
 }
 
-static inline float get_rc_yaw_f(void)
+static inline float get_rc_yaw_f(struct RadioControl *rc)
 {
-  int32_t yaw = radio_control.values[RADIO_YAW];
+  int32_t yaw = rc->values[RADIO_YAW];
   DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
   return yaw * STABILIZATION_ATTITUDE_SP_MAX_R / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
 }
 
-/// reset the heading for care-free mode to current heading
-void stabilization_attitude_reset_care_free_heading(void)
+void stabilization_attitude_rc_setpoint_init(struct AttitudeRCInput *rc_sp)
 {
-  care_free_heading = stateGetNedToBodyEulers_f()->psi;
-}
-
-/*   This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
-     However, when rolling more then 90 degrees in combination with pitch it switches. For a
-     transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
-int32_t stabilization_attitude_get_heading_i(void)
-{
-  struct Int32Eulers *att = stateGetNedToBodyEulers_i();
-
-  int32_t heading;
-
-  if (abs(att->phi) < INT32_ANGLE_PI_2) {
-    int32_t sin_theta;
-    PPRZ_ITRIG_SIN(sin_theta, att->theta);
-    heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
-  } else if (ANGLE_FLOAT_OF_BFP(att->theta) > 0) {
-    heading = att->psi - att->phi;
-  } else {
-    heading = att->psi + att->phi;
-  }
-
-  return heading;
-}
-
-float stabilization_attitude_get_heading_f(void)
-{
-  struct FloatEulers *att = stateGetNedToBodyEulers_f();
-
-  float heading;
-
-  if (fabsf(att->phi) < M_PI / 2) {
-    heading = att->psi - sinf(att->theta) * att->phi;
-  } else if (att->theta > 0) {
-    heading = att->psi - att->phi;
-  } else {
-    heading = att->psi + att->phi;
-  }
-
-  return heading;
-}
-
-
-/** Read attitude setpoint from RC as euler angles
- * @param[in]  coordinated_turn  true if in horizontal mode forward
- * @param[in]  in_carefree       true if in carefree mode
- * @param[in]  in_flight         true if in flight
- * @param[out] sp                attitude setpoint as euler angles
- */
-void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
-    bool coordinated_turn)
-{
-  /* last time this function was called, used to calculate yaw setpoint update */
-  static float last_ts = 0.f;
-
-  sp->phi = get_rc_roll();
-  sp->theta = get_rc_pitch();
-
-  if (in_flight) {
-    /* calculate dt for yaw integration */
-    float dt = get_sys_time_float() - last_ts;
-    /* make sure nothing drastically weird happens, bound dt to 0.5sec */
-    Bound(dt, 0, 0.5);
-
-    /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
-    if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-      sp->psi += get_rc_yaw() * dt;
-      INT32_ANGLE_NORMALIZE(sp->psi);
-    }
-    if (coordinated_turn) {
-      //Coordinated turn
-      //feedforward estimate angular rotation omega = g*tan(phi)/v
-      int32_t omega;
-      const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
-      if (abs(sp->phi) < max_phi) {
-        omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
-      } else { //max 60 degrees roll
-        omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
-      }
-
-      sp->psi += omega * dt;
-    }
-#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
-    // Make sure the yaw setpoint does not differ too much from the real yaw
-    // to prevent a sudden switch at 180 deg
-    const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
-
-    int32_t heading = stabilization_attitude_get_heading_i();
-
-    int32_t delta_psi = sp->psi - heading;
-    INT32_ANGLE_NORMALIZE(delta_psi);
-    if (delta_psi > delta_limit) {
-      sp->psi = heading + delta_limit;
-    } else if (delta_psi < -delta_limit) {
-      sp->psi = heading - delta_limit;
-    }
-    INT32_ANGLE_NORMALIZE(sp->psi);
-#endif
-    //Care Free mode
-    if (in_carefree) {
-      //care_free_heading has been set to current psi when entering care free mode.
-      int32_t cos_psi;
-      int32_t sin_psi;
-      int32_t temp_theta;
-      int32_t care_free_delta_psi_i;
-
-      care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
-
-      INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
-
-      PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
-      PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
-
-      temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
-                   INT32_ANGLE_FRAC);
-      sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
-
-      sp->theta = temp_theta;
-    }
-  } else { /* if not flying, use current yaw as setpoint */
-    sp->psi = stateGetNedToBodyEulers_i()->psi;
-  }
-
-  /* update timestamp for dt calculation */
-  last_ts = get_sys_time_float();
-}
-
-
-void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree,
-    bool coordinated_turn)
-{
-  /* last time this function was called, used to calculate yaw setpoint update */
-  static float last_ts = 0.f;
-
-  sp->phi = get_rc_roll_f();
-  sp->theta = get_rc_pitch_f();
-
-  if (in_flight) {
-    /* calculate dt for yaw integration */
-    float dt = get_sys_time_float() - last_ts;
-    /* make sure nothing drastically weird happens, bound dt to 0.5sec */
-    Bound(dt, 0, 0.5);
-
-    /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
-    if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-      sp->psi += get_rc_yaw_f() * dt;
-      FLOAT_ANGLE_NORMALIZE(sp->psi);
-    }
-    if (coordinated_turn) {
-      //Coordinated turn
-      //feedforward estimate angular rotation omega = g*tan(phi)/v
-      float omega;
-      const float max_phi = RadOfDeg(60.0);
-      if (fabsf(sp->phi) < max_phi) {
-        omega = 9.81 / COORDINATED_TURN_AIRSPEED * tanf(sp->phi);
-      } else { //max 60 degrees roll
-        omega = 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
-      }
-
-      sp->psi += omega * dt;
-    }
-#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
-    // Make sure the yaw setpoint does not differ too much from the real yaw
-    // to prevent a sudden switch at 180 deg
-    float heading = stabilization_attitude_get_heading_f();
-
-    float delta_psi = sp->psi - heading;
-    FLOAT_ANGLE_NORMALIZE(delta_psi);
-    if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
-      sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
-    } else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
-      sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
-    }
-    FLOAT_ANGLE_NORMALIZE(sp->psi);
-#endif
-    //Care Free mode
-    if (in_carefree) {
-      //care_free_heading has been set to current psi when entering care free mode.
-      float cos_psi;
-      float sin_psi;
-      float temp_theta;
-
-      float care_free_delta_psi_f = sp->psi - care_free_heading;
-
-      FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
-
-      sin_psi = sinf(care_free_delta_psi_f);
-      cos_psi = cosf(care_free_delta_psi_f);
-
-      temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
-      sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;
-
-      sp->theta = temp_theta;
-    }
-  } else { /* if not flying, use current yaw as setpoint */
-    sp->psi = stateGetNedToBodyEulers_f()->psi;
-  }
-
-  /* update timestamp for dt calculation */
-  last_ts = get_sys_time_float();
-}
-
-
-/** Read roll/pitch command from RC as quaternion.
- * Interprets the stick positions as axes.
- * @param[out] q quaternion representing the RC roll/pitch input
- */
-void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q)
-{
-  /* orientation vector describing simultaneous rotation of roll/pitch */
-  struct FloatVect3 ov;
-  ov.x = get_rc_roll_f();
-  ov.y = get_rc_pitch_f();
-  ov.z = 0.0;
-
-  /* quaternion from that orientation vector */
-  float_quat_of_orientation_vect(q, &ov);
-}
-
-/** Read roll/pitch command from RC as quaternion.
- * Both angles are are interpreted relative to to the horizontal plane (earth bound).
- * @param[out] q quaternion representing the RC roll/pitch input
- */
-void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q)
-{
-  /* only non-zero entries for roll quaternion */
-  float roll2 = get_rc_roll_f() / 2.0f;
-  float qx_roll = sinf(roll2);
-  float qi_roll = cosf(roll2);
-
-  //An offset is added if in forward mode
-  /* only non-zero entries for pitch quaternion */
-  float pitch2 = (ANGLE_FLOAT_OF_BFP(transition_theta_offset) + get_rc_pitch_f()) / 2.0f;
-  float qy_pitch = sinf(pitch2);
-  float qi_pitch = cosf(pitch2);
-
-  /* only multiply non-zero entries of float_quat_comp(q, &q_roll, &q_pitch) */
-  q->qi = qi_roll * qi_pitch;
-  q->qx = qx_roll * qi_pitch;
-  q->qy = qi_roll * qy_pitch;
-  q->qz = qx_roll * qy_pitch;
+  float_quat_identity(&rc_sp->rc_quat);
+  FLOAT_EULERS_ZERO(rc_sp->rc_eulers);
+  rc_sp->care_free_heading = 0.f;
+  rc_sp->transition_theta_offset = 0.f;
+  rc_sp->last_ts = 0.f;
 }
 
 /** Read attitude setpoint from RC as quaternion
  * Interprets the stick positions as axes.
- * @param[in]  coordinated_turn  true if in horizontal mode forward
- * @param[in]  in_carefree       true if in carefree mode
+ * @param[out] rc_sp             pointer to rc input structure
  * @param[in]  in_flight         true if in flight
- * @param[out] q_sp              attitude setpoint as quaternion
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
  */
-void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree,
-    bool coordinated_turn)
+void stabilization_attitude_read_rc_setpoint(struct AttitudeRCInput *rc_sp, bool in_flight, bool in_carefree,
+    bool coordinated_turn, struct RadioControl *rc)
 {
-
   // FIXME: remove me, do in quaternion directly
   // is currently still needed, since the yaw setpoint integration is done in eulers
-#if defined STABILIZATION_ATTITUDE_TYPE_INT
-  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
-#else
-  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
-#endif
+  stabilization_attitude_read_rc_setpoint_eulers_f(rc_sp, in_flight, in_carefree, coordinated_turn, rc);
 
   struct FloatQuat q_rp_cmd;
-  stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd);
+  stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd, rc);
 
   /* get current heading */
   const struct FloatVect3 zaxis = {0., 0., 1.};
@@ -393,7 +133,7 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool
   //Care Free mode
   if (in_carefree) {
     //care_free_heading has been set to current psi when entering care free mode.
-    float_quat_of_axis_angle(&q_yaw, &zaxis, care_free_heading);
+    float_quat_of_axis_angle(&q_yaw, &zaxis, rc_sp->care_free_heading);
   } else {
     float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
   }
@@ -406,51 +146,43 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool
   if (in_flight) {
     /* get current heading setpoint */
     struct FloatQuat q_yaw_sp;
-#if defined STABILIZATION_ATTITUDE_TYPE_INT
-    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
-#else
-    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
-#endif
+    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, rc_sp->rc_eulers.psi);
 
     /* rotation between current yaw and yaw setpoint */
     struct FloatQuat q_yaw_diff;
     float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);
 
     /* compute final setpoint with yaw */
-    float_quat_comp_norm_shortest(q_sp, &q_rp_sp, &q_yaw_diff);
+    float_quat_comp_norm_shortest(&rc_sp->rc_quat, &q_rp_sp, &q_yaw_diff);
   } else {
-    QUAT_COPY(*q_sp, q_rp_sp);
+    QUAT_COPY(rc_sp->rc_quat, q_rp_sp);
   }
 }
 
-//Function that reads the rc setpoint in an earth bound frame
-void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight,
-    bool in_carefree, bool coordinated_turn)
+/** Read attitude setpoint from RC as quaternion in earth bound frame
+ * @param[out] rc_sp             pointer to rc input structure
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ */
+void stabilization_attitude_read_rc_setpoint_earth_bound(struct AttitudeRCInput *rc_sp, bool in_flight,
+    bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
 {
   // FIXME: remove me, do in quaternion directly
   // is currently still needed, since the yaw setpoint integration is done in eulers
-#if defined STABILIZATION_ATTITUDE_TYPE_INT
-  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
-#else
-  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
-#endif
+  stabilization_attitude_read_rc_setpoint_eulers_f(rc_sp, in_flight, in_carefree, coordinated_turn, rc);
 
   const struct FloatVect3 zaxis = {0., 0., 1.};
 
   struct FloatQuat q_rp_cmd;
-  stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);
+  stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd, rc_sp->transition_theta_offset, rc);
 
   if (in_flight) {
     /* get current heading setpoint */
     struct FloatQuat q_yaw_sp;
-
-#if defined STABILIZATION_ATTITUDE_TYPE_INT
-    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
-#else
-    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
-#endif
-
-    float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd);
+    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, rc_sp->rc_eulers.psi);
+    float_quat_comp(&rc_sp->rc_quat, &q_yaw_sp, &q_rp_cmd);
   } else {
     struct FloatQuat q_yaw;
     float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
@@ -460,6 +192,269 @@ void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat
     float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
     float_quat_normalize(&q_rp_sp);
 
-    QUAT_COPY(*q_sp, q_rp_sp);
+    QUAT_COPY(rc_sp->rc_quat, q_rp_sp);
   }
 }
+
+/// reset to current state
+void stabilization_attitude_reset_rc_setpoint(struct AttitudeRCInput *rc_sp)
+{
+  rc_sp->rc_eulers = *stateGetNedToBodyEulers_f();
+  rc_sp->rc_quat = *stateGetNedToBodyQuat_f();
+}
+
+/// reset the heading for care-free mode to current heading
+void stabilization_attitude_reset_care_free_heading(struct AttitudeRCInput *rc_sp)
+{
+  rc_sp->care_free_heading = stateGetNedToBodyEulers_f()->psi;
+}
+
+/*   This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
+     However, when rolling more then 90 degrees in combination with pitch it switches. For a
+     transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
+int32_t stabilization_attitude_get_heading_i(void)
+{
+  struct Int32Eulers *att = stateGetNedToBodyEulers_i();
+  int32_t heading;
+  if (abs(att->phi) < INT32_ANGLE_PI_2) {
+    int32_t sin_theta;
+    PPRZ_ITRIG_SIN(sin_theta, att->theta);
+    heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
+  } else if (ANGLE_FLOAT_OF_BFP(att->theta) > 0) {
+    heading = att->psi - att->phi;
+  } else {
+    heading = att->psi + att->phi;
+  }
+  return heading;
+}
+
+float stabilization_attitude_get_heading_f(void)
+{
+  struct FloatEulers *att = stateGetNedToBodyEulers_f();
+  float heading;
+  if (fabsf(att->phi) < M_PI / 2) {
+    heading = att->psi - sinf(att->theta) * att->phi;
+  } else if (att->theta > 0) {
+    heading = att->psi - att->phi;
+  } else {
+    heading = att->psi + att->phi;
+  }
+  return heading;
+}
+
+
+/** Read attitude setpoint from RC as euler angles
+ * Only the euler format is updated and returned
+ * @param[out] rc_sp             pointer to rc input structure
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ * @return attitude setpoint in eulers (int)
+ */
+struct Int32Eulers stabilization_attitude_read_rc_setpoint_eulers(struct AttitudeRCInput *rc_sp, bool in_flight,
+    bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
+{
+  struct Int32Eulers sp_i;
+  EULERS_BFP_OF_REAL(sp_i, rc_sp->rc_eulers);
+
+  sp_i.phi = get_rc_roll(rc);
+  sp_i.theta = get_rc_pitch(rc);
+
+  if (in_flight) {
+    /* calculate dt for yaw integration */
+    float dt = get_sys_time_float() - rc_sp->last_ts;
+    /* make sure nothing drastically weird happens, bound dt to 0.5sec */
+    Bound(dt, 0, 0.5);
+
+    /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
+    if (YAW_DEADBAND_EXCEEDED(rc) && !THROTTLE_STICK_DOWN_FROM_RC(rc)) {
+      sp_i.psi += get_rc_yaw(rc) * dt;
+      INT32_ANGLE_NORMALIZE(sp_i.psi);
+    }
+    if (coordinated_turn) {
+      //Coordinated turn
+      //feedforward estimate angular rotation omega = g*tan(phi)/v
+      int32_t omega;
+      const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
+      if (abs(sp_i.phi) < max_phi) {
+        omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp_i.phi)));
+      } else { //max 60 degrees roll
+        omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp_i.phi > 0) - (sp_i.phi < 0)));
+      }
+
+      sp_i.psi += omega * dt;
+    }
+#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
+    // Make sure the yaw setpoint does not differ too much from the real yaw
+    // to prevent a sudden switch at 180 deg
+    const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
+
+    int32_t heading = stabilization_attitude_get_heading_i();
+
+    int32_t delta_psi = sp_i.psi - heading;
+    INT32_ANGLE_NORMALIZE(delta_psi);
+    if (delta_psi > delta_limit) {
+      sp_i.psi = heading + delta_limit;
+    } else if (delta_psi < -delta_limit) {
+      sp_i.psi = heading - delta_limit;
+    }
+    INT32_ANGLE_NORMALIZE(sp_i.psi);
+#endif
+    //Care Free mode
+    if (in_carefree) {
+      //care_free_heading has been set to current psi when entering care free mode.
+      int32_t cos_psi;
+      int32_t sin_psi;
+      int32_t temp_theta;
+      int32_t care_free_delta_psi_i;
+
+      care_free_delta_psi_i = sp_i.psi - ANGLE_BFP_OF_REAL(rc_sp->care_free_heading);
+
+      INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
+
+      PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
+      PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
+
+      temp_theta = INT_MULT_RSHIFT(cos_psi, sp_i.theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp_i.phi,
+                   INT32_ANGLE_FRAC);
+      sp_i.phi = INT_MULT_RSHIFT(cos_psi, sp_i.phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp_i.theta, INT32_ANGLE_FRAC);
+
+      sp_i.theta = temp_theta;
+    }
+  } else { /* if not flying, use current yaw as setpoint */
+    sp_i.psi = stateGetNedToBodyEulers_i()->psi;
+  }
+
+  /* update timestamp for dt calculation */
+  rc_sp->last_ts = get_sys_time_float();
+  EULERS_FLOAT_OF_BFP(rc_sp->rc_eulers, sp_i);
+  return sp_i;
+}
+
+
+/** Read attitude setpoint from RC as float euler angles
+ * Only the euler format is updated and returned
+ * @param[out] rc_sp             pointer to rc input structure
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ * @return attitude setpoint in eulers (float)
+ */
+struct FloatEulers stabilization_attitude_read_rc_setpoint_eulers_f(struct AttitudeRCInput *rc_sp, bool in_flight,
+    bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
+{
+  rc_sp->rc_eulers.phi = get_rc_roll_f(rc);
+  rc_sp->rc_eulers.theta = get_rc_pitch_f(rc);
+
+  if (in_flight) {
+    /* calculate dt for yaw integration */
+    float dt = get_sys_time_float() - rc_sp->last_ts;
+    /* make sure nothing drastically weird happens, bound dt to 0.5sec */
+    Bound(dt, 0, 0.5);
+
+    /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
+    if (YAW_DEADBAND_EXCEEDED(rc) && !THROTTLE_STICK_DOWN_FROM_RC(rc)) {
+      rc_sp->rc_eulers.psi += get_rc_yaw_f(rc) * dt;
+      FLOAT_ANGLE_NORMALIZE(rc_sp->rc_eulers.psi);
+    }
+    if (coordinated_turn) {
+      //Coordinated turn
+      //feedforward estimate angular rotation omega = g*tan(phi)/v
+      float omega;
+      const float max_phi = RadOfDeg(60.0);
+      if (fabsf(rc_sp->rc_eulers.phi) < max_phi) {
+        omega = 9.81 / COORDINATED_TURN_AIRSPEED * tanf(rc_sp->rc_eulers.phi);
+      } else { //max 60 degrees roll
+        omega = 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((rc_sp->rc_eulers.phi > 0) - (rc_sp->rc_eulers.phi < 0));
+      }
+
+      rc_sp->rc_eulers.psi += omega * dt;
+    }
+#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
+    // Make sure the yaw setpoint does not differ too much from the real yaw
+    // to prevent a sudden switch at 180 deg
+    float heading = stabilization_attitude_get_heading_f();
+
+    float delta_psi = rc_sp->rc_eulers.psi - heading;
+    FLOAT_ANGLE_NORMALIZE(delta_psi);
+    if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
+      rc_sp->rc_eulers.psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
+    } else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
+      rc_sp->rc_eulers.psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
+    }
+    FLOAT_ANGLE_NORMALIZE(rc_sp->rc_eulers.psi);
+#endif
+    //Care Free mode
+    if (in_carefree) {
+      //care_free_heading has been set to current psi when entering care free mode.
+      float cos_psi;
+      float sin_psi;
+      float temp_theta;
+
+      float care_free_delta_psi_f = rc_sp->rc_eulers.psi - rc_sp->care_free_heading;
+
+      FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
+
+      sin_psi = sinf(care_free_delta_psi_f);
+      cos_psi = cosf(care_free_delta_psi_f);
+
+      temp_theta = cos_psi * rc_sp->rc_eulers.theta - sin_psi * rc_sp->rc_eulers.phi;
+      rc_sp->rc_eulers.phi = cos_psi * rc_sp->rc_eulers.phi - sin_psi * rc_sp->rc_eulers.theta;
+
+      rc_sp->rc_eulers.theta = temp_theta;
+    }
+  } else { /* if not flying, use current yaw as setpoint */
+    rc_sp->rc_eulers.psi = stateGetNedToBodyEulers_f()->psi;
+  }
+
+  /* update timestamp for dt calculation */
+  rc_sp->last_ts = get_sys_time_float();
+  return rc_sp->rc_eulers;
+}
+
+
+/** Read roll/pitch command from RC as quaternion.
+ * Interprets the stick positions as axes.
+ * @param[out] q quaternion representing the RC roll/pitch input
+ * @param[in] rc pointer to radio control structure
+ */
+void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q, struct RadioControl *rc)
+{
+  /* orientation vector describing simultaneous rotation of roll/pitch */
+  struct FloatVect3 ov;
+  ov.x = get_rc_roll_f(rc);
+  ov.y = get_rc_pitch_f(rc);
+  ov.z = 0.0;
+
+  /* quaternion from that orientation vector */
+  float_quat_of_orientation_vect(q, &ov);
+}
+
+/** Read roll/pitch command from RC as quaternion.
+ * Both angles are are interpreted relative to to the horizontal plane (earth bound).
+ * @param[out] q quaternion representing the RC roll/pitch input
+ * @param[in] theta_offset pitch offset for forward flight
+ * @param[in] rc pointer to radio control structure
+ */
+void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q, float theta_offset, struct RadioControl *rc)
+{
+  /* only non-zero entries for roll quaternion */
+  float roll2 = get_rc_roll_f(rc) / 2.0f;
+  float qx_roll = sinf(roll2);
+  float qi_roll = cosf(roll2);
+
+  //An offset is added if in forward mode
+  /* only non-zero entries for pitch quaternion */
+  float pitch2 = (theta_offset + get_rc_pitch_f(rc)) / 2.0f;
+  float qy_pitch = sinf(pitch2);
+  float qi_pitch = cosf(pitch2);
+
+  /* only multiply non-zero entries of float_quat_comp(q, &q_roll, &q_pitch) */
+  q->qi = qi_roll * qi_pitch;
+  q->qx = qx_roll * qi_pitch;
+  q->qy = qi_roll * qy_pitch;
+  q->qz = qx_roll * qy_pitch;
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
index cd5f9b2ce4..23889c7183 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
@@ -29,21 +29,101 @@
 #include "std.h"
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_algebra_float.h"
+#include "modules/radio_control/radio_control.h"
 
-extern int32_t transition_theta_offset; // Pitch offset added for hybrid vehicle when in forward mode
+/** Attitude (and Rate) Remote Control input
+ */
+struct AttitudeRCInput {
+  struct FloatQuat    rc_quat;    ///< RC input in quaternion
+  struct FloatEulers  rc_eulers;  ///< RC input in eulers (needed even for quat for yaw integration)
+  float care_free_heading;        ///< care_free heading
+  float transition_theta_offset;  ///< pitch offset for hybrids, add when in forward mode
+  float last_ts;                  //< last timestamp (in seconds)
+};
 
-extern void stabilization_attitude_reset_care_free_heading(void);
+/** Init rc input
+ * @param[out] rc_sp pointer to rc input structure
+ */
+extern void stabilization_attitude_rc_setpoint_init(struct AttitudeRCInput *rc_sp);
+
+/** Read attitude setpoint from RC as quaternion
+ * Interprets the stick positions as axes.
+ * Both eulers and quaternion format are updated.
+ * @param[out] rc_sp             pointer to rc input structure
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ */
+extern void stabilization_attitude_read_rc_setpoint(struct AttitudeRCInput *rc_sp, bool in_flight,
+    bool in_carefree, bool coordinated_turn, struct RadioControl *rc);
+
+/** Read attitude setpoint from RC as quaternion in earth bound frame
+ * Interprets the stick positions as axes.
+ * Both eulers and quaternion format are updated.
+ * @param[out] rc_sp             pointer to rc input structure
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ */
+extern void stabilization_attitude_read_rc_setpoint_earth_bound(struct AttitudeRCInput *rc_sp, bool in_flight,
+    bool in_carefree, bool coordinated_turn, struct RadioControl *rc);
+
+/** Read attitude setpoint from RC as euler angles
+ * Only the euler format is updated and returned
+ * @param[out] rc_sp             pointer to rc input structure
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ * @return attitude setpoint in eulers (int)
+ */
+extern struct Int32Eulers stabilization_attitude_read_rc_setpoint_eulers(struct AttitudeRCInput *rc_sp, bool in_flight,
+    bool in_carefree, bool coordinated_turn, struct RadioControl *rc);
+
+/** Read attitude setpoint from RC as float euler angles
+ * Only the euler format is updated and returned
+ * @param[out] rc_sp             pointer to rc input structure
+ * @param[in]  in_flight         true if in flight
+ * @param[in]  in_carefree       true if in carefree mode
+ * @param[in]  coordinated_turn  true if in horizontal mode forward
+ * @param[in]  rc                pointer to radio control structure
+ * @return attitude setpoint in eulers (float)
+ */
+extern struct FloatEulers stabilization_attitude_read_rc_setpoint_eulers_f(struct AttitudeRCInput *rc_sp, bool in_flight,
+    bool in_carefree, bool coordinated_turn, struct RadioControl *rc);
+
+/** Reset rc input to current state
+ * @param[in/out] rc_sp   pointer to rc input structure
+ */
+extern void stabilization_attitude_reset_rc_setpoint(struct AttitudeRCInput *rc_sp);
+
+/** Reset care free heading to current heading
+ * @param[in/out] rc_sp   pointer to rc input structure
+ */
+extern void stabilization_attitude_reset_care_free_heading(struct AttitudeRCInput *rc_sp);
+
+/** Read RC roll and pitch only
+ * @param[out] q quaternion representing the RC roll/pitch input
+ * @param[in] rc pointer to radio control structure
+ */
+extern void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q, struct RadioControl *rc);
+
+/** Read RC roll and pitch only, in earth bounded frame
+ * @param[out] q quaternion representing the RC roll/pitch input
+ * @param[in] theta_offset pitch offset for forward flight
+ * @param[in] rc pointer to radio control structure
+ */
+extern void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q, float theta_offset, struct RadioControl *rc);
+
+/** Get attitude heading as int (avoiding jumps)
+ */
 extern int32_t stabilization_attitude_get_heading_i(void);
+
+/** Get attitude heading as float (avoiding jumps)
+ */
 extern float stabilization_attitude_get_heading_f(void);
-extern void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
-    bool coordinated_turn);
-extern void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight,
-    bool in_carefree, bool coordinated_turn);
-extern void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q);
-extern void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q);
-extern void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree,
-    bool coordinated_turn);
-extern void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight,
-    bool in_carefree, bool coordinated_turn);
 
 #endif /* STABILIZATION_ATTITUDE_RC_SETPOINT_H */
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_direct.c
similarity index 54%
rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c
rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_direct.c
index 90cfc96084..9c0a220b76 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_direct.c
@@ -19,7 +19,7 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file stabilization_none.c
+/** @file stabilization_direct.c
  *  Dummy stabilization for rotorcrafts.
  *
  *  Doesn't actually do any stabilization,
@@ -27,42 +27,47 @@
  */
 
 #include "firmwares/rotorcraft/stabilization.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_direct.h"
 
 #include "modules/radio_control/radio_control.h"
 #include "generated/airframe.h"
 #include "generated/modules.h"
 
-struct Int32Rates stabilization_none_rc_cmd;
+struct Int32Rates stabilization_direct_rc_cmd;
 
-void stabilization_none_init(void)
+void stabilization_direct_init(void)
 {
-  INT_RATES_ZERO(stabilization_none_rc_cmd);
+  INT_RATES_ZERO(stabilization_direct_rc_cmd);
 }
 
-void stabilization_none_read_rc(void)
+void stabilization_direct_read_rc(void)
 {
-
-  stabilization_none_rc_cmd.p = (int32_t)radio_control.values[RADIO_ROLL];
-  stabilization_none_rc_cmd.q = (int32_t)radio_control.values[RADIO_PITCH];
-  stabilization_none_rc_cmd.r = (int32_t)radio_control.values[RADIO_YAW];
+#ifdef RADIO_CONTROL
+  stabilization_direct_rc_cmd.p = (int32_t)radio_control_get(RADIO_ROLL);
+  stabilization_direct_rc_cmd.q = (int32_t)radio_control_get(RADIO_PITCH);
+  stabilization_direct_rc_cmd.r = (int32_t)radio_control_get(RADIO_YAW);
+#endif
 }
 
-void stabilization_none_enter(void)
+void stabilization_direct_enter(void)
 {
-  INT_RATES_ZERO(stabilization_none_rc_cmd);
+  INT_RATES_ZERO(stabilization_direct_rc_cmd);
 }
 
-void stabilization_none_run(bool in_flight __attribute__((unused)))
+void stabilization_direct_run(bool in_flight UNUSED, struct StabilizationSetpoint *sp UNUSED,
+    struct ThrustSetpoint *thrust UNUSED, int32_t *cmd UNUSED)
 {
   /* just directly pass rc commands through */
 #ifdef COMMAND_ROLL
-  stabilization_cmd[COMMAND_ROLL]  = stabilization_none_rc_cmd.p;
+  cmd[COMMAND_ROLL]   = stabilization_direct_rc_cmd.p;
 #endif
 #ifdef COMMAND_PITCH
-  stabilization_cmd[COMMAND_PITCH] = stabilization_none_rc_cmd.q;
+  cmd[COMMAND_PITCH]  = stabilization_direct_rc_cmd.q;
 #endif
 #ifdef COMMAND_YAW
-  stabilization_cmd[COMMAND_YAW]   = stabilization_none_rc_cmd.r;
+  cmd[COMMAND_YAW]    = stabilization_direct_rc_cmd.r;
+#endif
+#ifdef COMMAND_THRUST
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
 #endif
 }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_direct.h
similarity index 66%
rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.h
rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_direct.h
index 0b41b971e1..2935fbc8ec 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_direct.h
@@ -19,24 +19,25 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file stabilization_none.h
+/** @file stabilization_direct.h
  *  Dummy stabilization for rotorcrafts.
  *
  *  Doesn't actually do any stabilization,
  *  just directly passes the RC commands along.
  */
 
-#ifndef STABILIZATION_NONE
-#define STABILIZATION_NONE
+#ifndef STABILIZATION_DIRECT
+#define STABILIZATION_DIRECT
 
 #include "math/pprz_algebra_int.h"
+#include "firmwares/rotorcraft/stabilization.h"
 
-extern void stabilization_none_init(void);
-extern void stabilization_none_read_rc(void);
-extern void stabilization_none_run(bool in_flight);
-extern void stabilization_none_enter(void);
+extern void stabilization_direct_init(void);
+extern void stabilization_direct_read_rc(void);
+extern void stabilization_direct_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd);
+extern void stabilization_direct_enter(void);
 
-extern struct Int32Rates stabilization_none_rc_cmd;
+extern struct Int32Rates stabilization_direct_rc_cmd;
 
 
-#endif /* STABILIZATION_NONE */
+#endif /* STABILIZATION_DIRECT */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index 7d906bf89c..d0652e40dc 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -31,7 +31,6 @@
  */
 
 #include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
 
@@ -240,9 +239,8 @@ float act_obs[INDI_NUM_ACT];
 int32_t num_thrusters;
 int32_t num_thrusters_x;
 
-struct Int32Eulers stab_att_sp_euler;
-struct Int32Quat   stab_att_sp_quat;
-struct FloatRates  stab_att_ff_rates;
+static struct Int32Eulers stab_att_sp_euler;
+static struct Int32Quat   stab_att_sp_quat;
 
 // Register actuator feedback if we rely on RPM information
 #if STABILIZATION_INDI_RPM_FEEDBACK
@@ -256,11 +254,6 @@ static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback,
 PRINT_CONFIG_MSG("STABILIZATION_INDI_RPM_FEEDBACK")
 #endif
 
-abi_event thrust_ev;
-static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment);
-struct FloatVect3 indi_thrust_increment;
-bool indi_thrust_increment_set = false;
-
 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
 #ifdef STABILIZATION_INDI_G1
@@ -333,9 +326,17 @@ static void send_att_full_indi(struct transport_tx *trans, struct link_device *d
 {
   float zero = 0.0;
   struct FloatRates *body_rates = stateGetBodyRates_f();
+  struct FloatEulers att_sp;
+  EULERS_FLOAT_OF_BFP(att_sp, stab_att_sp_euler);
+#if GUIDANCE_INDI_HYBRID
+  struct FloatEulers att;
+  float_eulers_of_quat_zxy(&att, stateGetNedToBodyQuat_f());
+#else
+  struct FloatEulers att = *stateGetNedToBodyEulers_f();
+#endif
   pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
-                                      &zero, &zero, &zero,      // att
-                                      &zero, &zero, &zero,      // att.ref
+                                      &att.phi, &att.theta, &att.psi,           // att
+                                      &att_sp.phi, &att_sp.theta, &att_sp.psi,  // att.ref
                                       &body_rates->p,           // rate
                                       &body_rates->q,
                                       &body_rates->r,
@@ -373,7 +374,6 @@ void stabilization_indi_init(void)
 #if STABILIZATION_INDI_RPM_FEEDBACK
   AbiBindMsgACT_FEEDBACK(STABILIZATION_INDI_ACT_FEEDBACK_ID, &act_feedback_ev, act_feedback_cb);
 #endif
-  AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
 
   float_vect_zero(actuator_state_filt_vectd, INDI_NUM_ACT);
   float_vect_zero(actuator_state_filt_vectdd, INDI_NUM_ACT);
@@ -483,86 +483,14 @@ void init_filters(void)
 }
 
 /**
- * Function that calculates the failsafe setpoint
- */
-void stabilization_indi_set_failsafe_setpoint(void)
-{
-  /* set failsafe to zero roll/pitch and current heading */
-  int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
-  PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
-  stab_att_sp_quat.qx = 0;
-  stab_att_sp_quat.qy = 0;
-  PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
-}
-
-/**
- * @param rpy rpy from which to calculate quaternion setpoint
- *
- * Function that calculates the setpoint quaternion from rpy
- */
-void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler = *rpy;
-
-  int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
-  FLOAT_RATES_ZERO(stab_att_ff_rates);
-}
-
-/**
- * @param quat quaternion setpoint
- */
-void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  stab_att_sp_quat = *quat;
-  int32_eulers_of_quat(&stab_att_sp_euler, quat);
-  FLOAT_RATES_ZERO(stab_att_ff_rates);
-}
-
-/**
- * @param cmd 2D command in North East axes
- * @param heading Heading of the setpoint
- *
- * Function that calculates the setpoint quaternion from a command in earth axes
- */
-void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler.psi = heading;
-
-  // compute sp_euler phi/theta for debugging/telemetry
-  /* Rotate horizontal commands to body frame by psi */
-  int32_t psi = stateGetNedToBodyEulers_i()->psi;
-  int32_t s_psi, c_psi;
-  PPRZ_ITRIG_SIN(s_psi, psi);
-  PPRZ_ITRIG_COS(c_psi, psi);
-  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
-
-  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
-  FLOAT_RATES_ZERO(stab_att_ff_rates);
-}
-
-/**
- * @brief Set attitude setpoint from stabilization setpoint struct
- *
- * @param sp Stabilization setpoint structure
- */
-void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
-  stab_att_sp_quat = stab_sp_to_quat_i(sp);
-  stab_att_ff_rates = stab_sp_to_rates_f(sp);
-}
-
-/**
- * @param att_err attitude error
- * @param rate_control boolean that states if we are in rate control or attitude control
  * @param in_flight boolean that states if the UAV is in flight or not
+ * @param sp rate setpoint
+ * @param thrust thrust setpoint
+ * @param cmd output command array
  *
  * Function that calculates the INDI commands
  */
-void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
+void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
 
   // Propagate actuator filters
@@ -612,6 +540,14 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   float angular_acc_disturbance_estimate[INDI_OUTPUTS];
   float_vect_diff(angular_acc_disturbance_estimate, angular_acceleration, angular_acc_prediction_filt, 3);
 
+  if (in_flight) {
+    // Limit the estimated disturbance in yaw for drones that are stable in sideslip
+    BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
+  } else {
+    // Not in flight, so don't estimate disturbance
+    float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
+  }
+
   //The rates used for feedback are by default the measured rates.
   //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
   //Note that due to the delay, the PD controller may need relaxed gains.
@@ -644,62 +580,46 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   rates_filt.r = body_rates->r;
 #endif
 
-  //calculate the virtual control (reference acceleration) based on a PD controller
+  // calculate the virtual control (reference acceleration) based on a PD controller
+  struct FloatRates rate_sp = stab_sp_to_rates_f(sp);
   angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi_gains.rate.p;
   angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
   angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
 
-  struct FloatVect3 v_thrust;
-  v_thrust.x = 0.0;
-  v_thrust.y = 0.0;
-  v_thrust.z = 0.0;
-  if (indi_thrust_increment_set) {
-    //update thrust command such that the current is correctly estimated
-    stabilization_cmd[COMMAND_THRUST] = 0;
-    float current_thrust_filt_z = 0;
+  // compute virtual thrust
+  struct FloatVect3 v_thrust = { 0.f, 0.f, 0.f };
+  if (thrust->type == THRUST_INCR_SP) {
+    v_thrust.x = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_X);
+    v_thrust.y = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_Y);
+    v_thrust.z = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_Z);
+
+    // Compute estimated thrust
+    struct FloatVect3 thrust_filt = { 0.f, 0.f, 0.f };
     for (i = 0; i < INDI_NUM_ACT; i++) {
-      stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
-      current_thrust_filt_z += Bwls[3][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_z[i];
-    }
-    stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
-
-    // Add the increment to the current estimated thrust
-    v_thrust.z = current_thrust_filt_z + indi_thrust_increment.z;
-
+      thrust_filt.z += Bwls[3][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_z[i];
 #if INDI_OUTPUTS == 5
-    stabilization_cmd[COMMAND_THRUST_X] = 0;
-    float current_thrust_filt_x = 0;
-    for (i = 0; i < INDI_NUM_ACT; i++) {
-      stabilization_cmd[COMMAND_THRUST_X] += actuator_state[i] * (int32_t) act_is_thruster_x[i];
-      current_thrust_filt_x += Bwls[4][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_x[i];
-    }
-    stabilization_cmd[COMMAND_THRUST_X] /= num_thrusters_x;
-
-    v_thrust.x = current_thrust_filt_x + indi_thrust_increment.x;
+      thrust_filt.x += Bwls[4][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_x[i];
 #endif
-
+    }
+    // Add the current estimated thrust to the increment
+    VECT3_ADD(v_thrust, thrust_filt);
   } else {
-    // incremental thrust
+    // build incremental thrust
+    float th_cmd_z = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
     for (i = 0; i < INDI_NUM_ACT; i++) {
-      v_thrust.z += stabilization_cmd[COMMAND_THRUST] * Bwls[3][i];
+      v_thrust.z += th_cmd_z * Bwls[3][i];
 #if INDI_OUTPUTS == 5
-      stabilization_cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X];
-      v_thrust.x += stabilization_cmd[COMMAND_THRUST_X] * Bwls[4][i];
+      // TODO set X thrust from RC in the thrust input setpoint
+      cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X];
+      v_thrust.x += cmd[COMMAND_THRUST_X] * Bwls[4][i];
 #endif
     }
+    v_thrust.y = 0.f;
   }
 
   // This term compensates for the spinup torque in the yaw axis
   float g2_times_u = float_vect_dot_product(g2, indi_u, INDI_NUM_ACT)/INDI_G_SCALING;
 
-  if (in_flight) {
-    // Limit the estimated disturbance in yaw for drones that are stable in sideslip
-    BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
-  } else {
-    // Not in flight, so don't estimate disturbance
-    float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
-  }
-
   // The control objective in array format
   indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]);
   indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]);
@@ -748,10 +668,18 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   for (i = 0; i < INDI_NUM_ACT; i++) {
     actuators_pprz[i] = (int16_t) indi_u[i];
   }
+
+  //update thrust command such that the current is correctly estimated
+  cmd[COMMAND_THRUST] = 0;
+  for (i = 0; i < INDI_NUM_ACT; i++) {
+    cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
+  }
+  cmd[COMMAND_THRUST] /= num_thrusters;
+
 }
 
 /**
- * Function that sets the du_min, du_max and du_pref if function not elsewhere defined
+ * Function that sets the u_min, u_max and u_pref if function not elsewhere defined
  */
 void WEAK stabilization_indi_set_wls_settings(void)
 {
@@ -778,23 +706,26 @@ void WEAK stabilization_indi_set_wls_settings(void)
 }
 
 /**
- * @param enable_integrator
- * @param rate_control boolean that determines if we are in rate control or attitude control
+ * @param in_flight enable integrator only in flight
+ * @param att_sp attitude stabilization setpoint
+ * @param thrust thrust setpoint
+ * @param[out] output command vector
  *
  * Function that should be called to run the INDI controller
  */
-void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
+void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
-  /* attitude error                          */
+  stab_att_sp_euler = stab_sp_to_eulers_i(att_sp);  // stab_att_sp_euler.psi still used in ref..
+  stab_att_sp_quat = stab_sp_to_quat_i(att_sp);     // quat attitude setpoint
+
+  /* attitude error in float */
   struct FloatQuat att_err;
   struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
-  struct FloatQuat quat_sp_f;
+  struct FloatQuat quat_sp = stab_sp_to_quat_f(att_sp);
 
-  QUAT_FLOAT_OF_BFP(quat_sp_f, quat_sp);
-  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp_f);
+  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp);
 
   struct FloatVect3 att_fb;
-
 #if TILT_TWIST_CTRL
   struct FloatQuat tilt;
   struct FloatQuat twist;
@@ -810,14 +741,14 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
 
   // local variable to compute rate setpoints based on attitude error
   struct FloatRates rate_sp;
-
   // calculate the virtual control (reference acceleration) based on a PD controller
   rate_sp.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p;
   rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
   rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
 
   // Add feed-forward rates to the attitude feedback part
-  RATES_ADD(rate_sp, stab_att_ff_rates);
+  struct FloatRates ff_rates = stab_sp_to_rates_f(att_sp);
+  RATES_ADD(rate_sp, ff_rates);
 
   // Store for telemetry
   angular_rate_ref.p = rate_sp.p;
@@ -828,23 +759,8 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
   /*BoundAbs(rate_sp.r, 5.0);*/
 
   /* compute the INDI command */
-  stabilization_indi_rate_run(rate_sp, in_flight);
-
-  // Reset thrust increment boolean
-  indi_thrust_increment_set = false;
-}
-
-// This function reads rc commands
-void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  struct FloatQuat q_sp;
-#if USE_EARTH_BOUND_RC_SETPOINT
-  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#else
-  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#endif
-
-  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
+  struct StabilizationSetpoint sp = stab_sp_from_rates_f(&rate_sp);
+  stabilization_indi_rate_run(in_flight, &sp, thrust, cmd);
 }
 
 /**
@@ -1073,15 +989,6 @@ static void act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *fee
 }
 #endif
 
-/**
- * ABI callback that obtains the thrust increment from guidance INDI
- */
-static void thrust_cb(uint8_t UNUSED sender_id, struct FloatVect3 thrust_increment)
-{
-  indi_thrust_increment = thrust_increment;
-  indi_thrust_increment_set = true;
-}
-
 static void bound_g_mat(void)
 {
   int8_t i;
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
index 3b6d02f93e..9abdaf4c6b 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
@@ -24,14 +24,13 @@
 #define STABILIZATION_INDI
 
 #include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
 
 // Scaling for the control effectiveness to make it readible
 #define INDI_G_SCALING 1000.0
 
-extern struct Int32Quat   stab_att_sp_quat;  ///< with #INT32_QUAT_FRAC
-extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
 extern float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
 extern float actuator_state_filt_vect[INDI_NUM_ACT];
 extern bool act_is_servo[INDI_NUM_ACT];
@@ -63,15 +62,9 @@ extern struct Indi_gains indi_gains;
 
 extern void stabilization_indi_init(void);
 extern void stabilization_indi_enter(void);
-extern void stabilization_indi_set_failsafe_setpoint(void);
-extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
-extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
-extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
-extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
-extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
+extern void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd);
+extern void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd);
 extern void stabilization_indi_set_wls_settings(void);
-extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
-extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
 
 #endif /* STABILIZATION_INDI */
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
index 6d59875a6a..3efb46e7d1 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
@@ -31,7 +31,6 @@
  */
 
 #include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
 
@@ -98,7 +97,6 @@
 
 struct Int32Eulers stab_att_sp_euler;
 struct Int32Quat   stab_att_sp_quat;
-struct FloatRates  stab_att_ff_rates;
 
 static struct FirstOrderLowPass rates_filt_fo[3];
 
@@ -268,77 +266,6 @@ void stabilization_indi_enter(void)
   indi_init_filters();
 }
 
-void stabilization_indi_set_failsafe_setpoint(void)
-{
-  /* set failsafe to zero roll/pitch and current heading */
-  int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
-  PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
-  stab_att_sp_quat.qx = 0;
-  stab_att_sp_quat.qy = 0;
-  PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
-  FLOAT_RATES_ZERO(stab_att_ff_rates);
-}
-
-/**
- * @brief Set attitude quaternion setpoint from rpy
- *
- * @param rpy roll pitch yaw input
- */
-void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler = *rpy;
-
-  int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
-  FLOAT_RATES_ZERO(stab_att_ff_rates);
-}
-
-/**
- * @param quat quaternion setpoint
- */
-void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
-{
-  stab_att_sp_quat = *quat;
-  int32_eulers_of_quat(&stab_att_sp_euler, quat);
-  FLOAT_RATES_ZERO(stab_att_ff_rates);
-}
-
-/**
- * @brief Set attitude setpoint from command in earth axes
- *
- * @param cmd The command in earth axes (North East)
- * @param heading The desired heading
- */
-void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
-{
-  // stab_att_sp_euler.psi still used in ref..
-  stab_att_sp_euler.psi = heading;
-
-  // compute sp_euler phi/theta for debugging/telemetry
-  /* Rotate horizontal commands to body frame by psi */
-  int32_t psi = stateGetNedToBodyEulers_i()->psi;
-  int32_t s_psi, c_psi;
-  PPRZ_ITRIG_SIN(s_psi, psi);
-  PPRZ_ITRIG_COS(c_psi, psi);
-  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
-  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
-
-  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
-  FLOAT_RATES_ZERO(stab_att_ff_rates);
-}
-
-/**
- * @brief Set attitude setpoint from stabilization setpoint struct
- *
- * @param sp Stabilization setpoint structure
- */
-void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
-{
-  stab_att_sp_euler = stab_sp_to_eulers_i(sp);
-  stab_att_sp_quat = stab_sp_to_quat_i(sp);
-  stab_att_ff_rates = stab_sp_to_rates_f(sp);
-}
-
 /**
  * @brief Update butterworth filter for p, q and r of a FloatRates struct
  *
@@ -383,11 +310,14 @@ static inline void finite_difference(float output[3], float new[3], float old[3]
 /**
  * @brief Does the INDI calculations
  *
- * @param indi_commands[] Array of commands that the function will write to
- * @param att_err quaternion attitude error
+ * @param in_flight true aircraft is flying
+ * @param sp rate setpoint
+ * @param thrust thrust setpoint
+ * @param cmd output command array
  */
-void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __attribute__((unused)))
+void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
+
   //Propagate input filters
   //first order actuator dynamics
   indi.u_act_dyn.p = indi.u_act_dyn.p + indi.act_dyn.p * (indi.u_in.p - indi.u_act_dyn.p);
@@ -423,6 +353,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
 #endif
 
   //This lets you impose a maximum yaw rate.
+  struct FloatRates rate_sp = stab_sp_to_rates_f(sp);
   BoundAbs(rate_sp.r, indi.attitude_max_yaw_rate);
 
   // Compute reference angular acceleration:
@@ -440,7 +371,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
 
   //Don't increment if thrust is off and on the ground
   //without this the inputs will increment to the maximum before even getting in the air.
-  if (stabilization_cmd[COMMAND_THRUST] < 300 && !in_flight) {
+  if (th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z) < 300 && !in_flight) {
     FLOAT_RATES_ZERO(indi.u_in);
 
     // If on the gournd, no increments, just proportional control
@@ -471,14 +402,16 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
 #endif
 
   /*  INDI feedback */
-  stabilization_cmd[COMMAND_ROLL] = indi.u_in.p;
-  stabilization_cmd[COMMAND_PITCH] = indi.u_in.q;
-  stabilization_cmd[COMMAND_YAW] = indi.u_in.r;
+  cmd[COMMAND_ROLL] = indi.u_in.p;
+  cmd[COMMAND_PITCH] = indi.u_in.q;
+  cmd[COMMAND_YAW] = indi.u_in.r;
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
 
   /* bound the result */
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
 }
 
 /**
@@ -487,18 +420,19 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
  * @param in_flight not used
  * @param rate_control rate control enabled, otherwise attitude control
  */
-void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __attribute__((unused)))
+void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
-  /* attitude error                          */
+  stab_att_sp_euler = stab_sp_to_eulers_i(att_sp);  // stab_att_sp_euler.psi still used in ref..
+  stab_att_sp_quat = stab_sp_to_quat_i(att_sp);     // quat attitude setpoint
+
+  /* attitude error in float */
   struct FloatQuat att_err;
   struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
-  struct FloatQuat quat_sp_f;
+  struct FloatQuat quat_sp = stab_sp_to_quat_f(att_sp);
 
-  QUAT_FLOAT_OF_BFP(quat_sp_f, quat_sp);
-  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp_f);
+  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp);
 
   struct FloatVect3 att_fb;
-
 #if TILT_TWIST_CTRL
   struct FloatQuat tilt;
   struct FloatQuat twist;
@@ -519,26 +453,12 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __
   rate_sp.r = indi.gains.att.r * att_fb.z / indi.gains.rate.r;
 
   // Add feed-forward rates to the attitude feedback part
-  RATES_ADD(rate_sp, stab_att_ff_rates);
+  struct FloatRates ff_rates = stab_sp_to_rates_f(att_sp);
+  RATES_ADD(rate_sp, ff_rates);
 
   /* compute the INDI command */
-  stabilization_indi_rate_run(rate_sp, in_flight);
-}
-
-/**
- * This function reads rc commands
- *
- * @param in_flight boolean that states if the UAV is in flight or not
- */
-void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
-{
-  struct FloatQuat q_sp;
-#if USE_EARTH_BOUND_RC_SETPOINT
-  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#else
-  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
-#endif
-  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
+  struct StabilizationSetpoint sp = stab_sp_from_rates_f(&rate_sp);
+  stabilization_indi_rate_run(in_flight, &sp, thrust, cmd);
 }
 
 /**
@@ -590,3 +510,4 @@ static inline void lms_estimation(void)
     indi.g2   = est->g2 * INDI_EST_SCALE;
   }
 }
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
index 0938e1660c..bbcbce6ac3 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
@@ -35,12 +35,10 @@
 #define STABILIZATION_INDI_SIMPLE_H
 
 #include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
 #include "filters/low_pass_filter.h"
 
-extern struct Int32Quat   stab_att_sp_quat;  ///< with #INT32_QUAT_FRAC
-extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
-
 struct Indi_gains {
   struct FloatRates att;
   struct FloatRates rate;
@@ -84,14 +82,8 @@ struct IndiVariables {
 extern struct IndiVariables indi;
 extern void stabilization_indi_init(void);
 extern void stabilization_indi_enter(void);
-extern void stabilization_indi_set_failsafe_setpoint(void);
-extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
-extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
-extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
-extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
-extern void stabilization_indi_rate_run(struct FloatRates rates_sp, bool in_flight);
-extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
-extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
+extern void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd);
+extern void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd);
 extern void stabilization_indi_simple_reset_r_filter_cutoff(float new_cutoff);
 
 #endif /* STABILIZATION_INDI_SIMPLE_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c
index c19554dcd0..5be72f43c2 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c
@@ -19,76 +19,27 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file stabilization_none.c
- *  Dummy stabilization for rotorcrafts.
- *
- *  Doesn't actually do any stabilization,
- *  just directly passes the RC commands along.
+/** @file stabilization_oneloop.c
  */
 
-//#include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_oneloop.h"
 #include "firmwares/rotorcraft/oneloop/oneloop_andi.h"
-
-#include "modules/radio_control/radio_control.h"
-#include "generated/airframe.h"
 #include "generated/modules.h"
 
 
-struct FloatEulers stab_att_sp_euler;
-struct Int32Quat   stab_att_sp_quat;
-struct FloatRates  stab_att_ff_rates;
-
-
-
-void stabilization_attitude_init(void)
-{
-  // oneloop init is already done through module init
-}
-
 void stabilization_attitude_enter(void)
 {
   oneloop_andi_enter(true);
 }
 
-void stabilization_attitude_set_failsafe_setpoint(void)
-{
-
-}
-
-void stabilization_attitude_set_rpy_setpoint_i(UNUSED struct Int32Eulers *rpy)
-{
-
-}
-
-void stabilization_attitude_set_quat_setpoint_i(UNUSED struct Int32Quat *quat)
-{
-
-}
-
-void stabilization_attitude_set_earth_cmd_i(UNUSED struct Int32Vect2 *cmd, UNUSED int32_t heading)
-{
-
-}
-
-void stabilization_attitude_set_stab_sp(UNUSED struct StabilizationSetpoint *sp)
-{
-
-}
-
-void stabilization_attitude_run(bool in_flight)
+void stabilization_attitude_run(bool in_flight, UNUSED struct StabilizationSetpoint *sp, UNUSED struct ThrustSetpoint *thrust, UNUSED int32_t *cmd)
 {
   struct FloatVect3 PSA_des    = { 0 };
   int    rm_order_h = 3;
   int    rm_order_v = 3;
   // Run the oneloop controller in half-loop mode
-  if (oneloop_andi.half_loop){
-  oneloop_andi_run(in_flight, oneloop_andi.half_loop, PSA_des, rm_order_h, rm_order_v);
+  if (oneloop_andi.half_loop) {
+    oneloop_andi_run(in_flight, oneloop_andi.half_loop, PSA_des, rm_order_h, rm_order_v);
   }
 }
 
-
-void stabilization_attitude_read_rc(UNUSED bool in_flight, UNUSED bool in_carefree, UNUSED bool coordinated_turn)
-{
-
-}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.h
index 8c6e536cc7..87232005cf 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.h
@@ -19,25 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file stabilization_none.h
- *  Dummy stabilization for rotorcrafts.
- *
- *  Doesn't actually do any stabilization,
- *  just directly passes the RC commands along.
+/** @file stabilization_oneloop.h
  */
 
 #ifndef STABILIZATION_ONELOOP
 #define STABILIZATION_ONELOOP
 
-#include "math/pprz_algebra_int.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 
-extern struct FloatEulers stab_att_sp_euler;
-extern struct Int32Quat   stab_att_sp_quat;
-extern struct FloatRates  stab_att_ff_rates;
+#endif /* STABILIZATION_ONELOOP */
 
-
-extern struct Int32Rates stabilization_oneloop_rc_cmd;
-
-
-#endif /* STABILIZATION_NONE */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
index 999cff6e86..b26c3d3e17 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
@@ -64,34 +64,11 @@
 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
 
-struct FloatRates stabilization_rate_sp;
+static struct FloatRates stabilization_rate_sp;
+static struct FloatRates stabilization_rate_sum_err;
+static struct FloatRates stabilization_rate_fb_cmd;
 struct FloatRates stabilization_rate_gain;
 struct FloatRates stabilization_rate_igain;
-struct FloatRates stabilization_rate_sum_err;
-
-struct FloatRates stabilization_rate_fb_cmd;
-
-#ifndef STABILIZATION_RATE_DEADBAND_P
-#define STABILIZATION_RATE_DEADBAND_P 0
-#endif
-#ifndef STABILIZATION_RATE_DEADBAND_Q
-#define STABILIZATION_RATE_DEADBAND_Q 0
-#endif
-#ifndef STABILIZATION_RATE_DEADBAND_R
-#define STABILIZATION_RATE_DEADBAND_R 200
-#endif
-
-#define ROLL_RATE_DEADBAND_EXCEEDED()                                         \
-  (radio_control.values[RADIO_ROLL] >  STABILIZATION_RATE_DEADBAND_P || \
-   radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
-
-#define PITCH_RATE_DEADBAND_EXCEEDED()                                         \
-  (radio_control.values[RADIO_PITCH] >  STABILIZATION_RATE_DEADBAND_Q || \
-   radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
-
-#define YAW_RATE_DEADBAND_EXCEEDED()                                         \
-  (radio_control.values[RADIO_YAW] >  STABILIZATION_RATE_DEADBAND_R || \
-   radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -108,7 +85,7 @@ static void send_rate(struct transport_tx *trans, struct link_device *dev)
                           &stabilization_rate_fb_cmd.p,
                           &stabilization_rate_fb_cmd.q,
                           &stabilization_rate_fb_cmd.r,
-                          &stabilization_cmd[COMMAND_THRUST]);
+                          &stabilization.cmd[COMMAND_THRUST]);
 }
 #endif
 
@@ -133,62 +110,17 @@ void stabilization_rate_init(void)
 #endif
 }
 
-
-void stabilization_rate_read_rc(void)
-{
-
-  if (ROLL_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.p = radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.p = 0;
-  }
-
-  if (PITCH_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.q = radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.q = 0;
-  }
-
-  if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-    stabilization_rate_sp.r = radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.r = 0;
-  }
-}
-
-//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
-void stabilization_rate_read_rc_switched_sticks(void)
-{
-
-  if (ROLL_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.r =  - radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.r = 0;
-  }
-
-  if (PITCH_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.q = radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.q = 0;
-  }
-
-  if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-    stabilization_rate_sp.p = radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.p = 0;
-  }
-}
-
 void stabilization_rate_enter(void)
 {
   FLOAT_RATES_ZERO(stabilization_rate_sum_err);
 }
 
-void stabilization_rate_run(bool in_flight)
+void stabilization_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
   /* compute feed-back command */
   struct FloatRates _error;
   struct FloatRates *body_rate = stateGetBodyRates_f();
+  stabilization_rate_sp = stab_sp_to_rates_f(rate_sp);
   RATES_DIFF(_error, stabilization_rate_sp, (*body_rate));
   if (in_flight) {
     /* update integrator */
@@ -211,13 +143,31 @@ void stabilization_rate_run(bool in_flight)
   stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r +
                                 stabilization_rate_igain.r  * stabilization_rate_sum_err.r;
 
-  stabilization_cmd[COMMAND_ROLL]  = stabilization_rate_fb_cmd.p;
-  stabilization_cmd[COMMAND_PITCH] = stabilization_rate_fb_cmd.q;
-  stabilization_cmd[COMMAND_YAW]   = stabilization_rate_fb_cmd.r;
+  cmd[COMMAND_ROLL]  = stabilization_rate_fb_cmd.p;
+  cmd[COMMAND_PITCH] = stabilization_rate_fb_cmd.q;
+  cmd[COMMAND_YAW]   = stabilization_rate_fb_cmd.r;
+  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(th, 0, THRUST_AXIS_Z);
 
   /* bound the result */
-  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
-  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
-
+  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
+  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
 }
+
+struct StabilizationSetpoint stabilization_rate_read_rc(struct RadioControl *rc)
+{
+  struct FloatRates rate_sp;
+  FLOAT_RATES_ZERO(rate_sp);
+  if (ROLL_RATE_DEADBAND_EXCEEDED(rc)) {
+    rate_sp.p = rc->values[RC_RATE_P] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
+  }
+  if (PITCH_RATE_DEADBAND_EXCEEDED(rc)) {
+    rate_sp.q = rc->values[RC_RATE_Q] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
+  }
+  if (YAW_RATE_DEADBAND_EXCEEDED(rc) && !THROTTLE_STICK_DOWN_FROM_RC(rc)) {
+    rate_sp.r = rc->values[RC_RATE_R] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
+  }
+  return stab_sp_from_rates_f(&rate_sp);
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h
index b0edfe5353..3c03f0acfd 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h
@@ -28,18 +28,50 @@
 #ifndef STABILIZATION_RATE
 #define STABILIZATION_RATE
 
+#include "firmwares/rotorcraft/stabilization.h"
+#include "modules/radio_control/radio_control.h"
 #include "math/pprz_algebra_float.h"
 
-extern void stabilization_rate_init(void);
-extern void stabilization_rate_read_rc(void);
-extern void stabilization_rate_read_rc_switched_sticks(void);
-extern void stabilization_rate_run(bool in_flight);
-extern void stabilization_rate_enter(void);
+#ifndef STABILIZATION_RATE_DEADBAND_P
+#define STABILIZATION_RATE_DEADBAND_P 0
+#endif
+#ifndef STABILIZATION_RATE_DEADBAND_Q
+#define STABILIZATION_RATE_DEADBAND_Q 0
+#endif
+#ifndef STABILIZATION_RATE_DEADBAND_R
+#define STABILIZATION_RATE_DEADBAND_R 200
+#endif
+
+#define ROLL_RATE_DEADBAND_EXCEEDED(_rc)                       \
+  (_rc->values[RADIO_ROLL] >  STABILIZATION_RATE_DEADBAND_P || \
+   _rc->values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
+
+#define PITCH_RATE_DEADBAND_EXCEEDED(_rc)                       \
+  (_rc->values[RADIO_PITCH] >  STABILIZATION_RATE_DEADBAND_Q || \
+   _rc->values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
+
+#define YAW_RATE_DEADBAND_EXCEEDED(_rc)                       \
+  (_rc->values[RADIO_YAW] >  STABILIZATION_RATE_DEADBAND_R || \
+   _rc->values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
+
+#if SWITCH_STICKS_FOR_RATE_CONTROL
+// Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
+#define RC_RATE_P RADIO_YAW
+#define RC_RATE_Q RADIO_PITCH
+#define RC_RATE_R RADIO_ROLL
+#else
+// Normal orientation
+#define RC_RATE_P RADIO_ROLL
+#define RC_RATE_Q RADIO_PITCH
+#define RC_RATE_R RADIO_YAW
+#endif
+
+extern void stabilization_rate_init(void);
+extern void stabilization_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd);
+extern void stabilization_rate_enter(void);
+extern struct StabilizationSetpoint stabilization_rate_read_rc(struct RadioControl *rc);
 
-extern struct FloatRates stabilization_rate_sp;
 extern struct FloatRates stabilization_rate_gain;
 extern struct FloatRates stabilization_rate_igain;
-extern struct FloatRates stabilization_rate_sum_err;
-extern struct FloatRates stabilization_rate_fb_cmd;
 
 #endif /* STABILIZATION_RATE */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c
index 6a5063e450..c42a400e21 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c
@@ -43,39 +43,15 @@ struct FloatRates stabilization_rate_sp;
 #define STABILIZATION_INDI_MAX_RATE 6.0
 #endif
 
-/* -- RC deadbands */
-#ifndef STABILIZATION_RATE_DEADBAND_P
-#define STABILIZATION_RATE_DEADBAND_P 0
-#endif
-#ifndef STABILIZATION_RATE_DEADBAND_Q
-#define STABILIZATION_RATE_DEADBAND_Q 0
-#endif
-#ifndef STABILIZATION_RATE_DEADBAND_R
-#define STABILIZATION_RATE_DEADBAND_R 200
-#endif
-
-#define ROLL_RATE_DEADBAND_EXCEEDED()                                         \
-  (radio_control.values[RADIO_ROLL] >  STABILIZATION_RATE_DEADBAND_P || \
-   radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
-
-#define PITCH_RATE_DEADBAND_EXCEEDED()                                         \
-  (radio_control.values[RADIO_PITCH] >  STABILIZATION_RATE_DEADBAND_Q || \
-   radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
-
-#define YAW_RATE_DEADBAND_EXCEEDED()                                         \
-  (radio_control.values[RADIO_YAW] >  STABILIZATION_RATE_DEADBAND_R || \
-   radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
-
-
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
 
 static void send_rate(struct transport_tx *trans, struct link_device *dev)
 {
   float dummy = 0;
-  float fb_p = stabilization_cmd[COMMAND_ROLL];
-  float fb_q = stabilization_cmd[COMMAND_PITCH];
-  float fb_r = stabilization_cmd[COMMAND_YAW];
+  float fb_p = stabilization.cmd[COMMAND_ROLL];
+  float fb_q = stabilization.cmd[COMMAND_PITCH];
+  float fb_r = stabilization.cmd[COMMAND_YAW];
 
   pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
                           &stabilization_rate_sp.p,
@@ -85,7 +61,7 @@ static void send_rate(struct transport_tx *trans, struct link_device *dev)
                           &fb_p,
                           &fb_q,
                           &fb_r,
-                          &stabilization_cmd[COMMAND_THRUST]);
+                          &stabilization.cmd[COMMAND_THRUST]);
 }
 #endif
 
@@ -94,6 +70,7 @@ static void send_rate(struct transport_tx *trans, struct link_device *dev)
  */
 void stabilization_rate_init(void)
 {
+  FLOAT_RATES_ZERO(stabilization_rate_sp);
   // indi init is already done through module init
 
 #if PERIODIC_TELEMETRY
@@ -109,60 +86,30 @@ void stabilization_rate_enter(void)
   stabilization_indi_enter();
 }
 
-/**
- * @brief Read RC comands with roll and yaw sticks
- */
-void stabilization_rate_read_rc(void)
-{
-  if (ROLL_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.p = (float) radio_control.values[RADIO_ROLL] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.p = 0;
-  }
-
-  if (PITCH_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.q = (float) radio_control.values[RADIO_PITCH] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.q = 0;
-  }
-
-  if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-    stabilization_rate_sp.r = (float) radio_control.values[RADIO_YAW] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.r = 0;
-  }
-}
-
-/**
- * @brief Read rc with roll and yaw sitcks switched if the default orientation is vertical
- * but airplane sticks are desired
- */
-void stabilization_rate_read_rc_switched_sticks(void)
-{
-  if (ROLL_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.r =  - (float) radio_control.values[RADIO_ROLL] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.r = 0;
-  }
-
-  if (PITCH_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.q = (float) radio_control.values[RADIO_PITCH] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.q = 0;
-  }
-
-  if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-    stabilization_rate_sp.p = (float) radio_control.values[RADIO_YAW] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
-  } else {
-    stabilization_rate_sp.p = 0;
-  }
-}
-
 /**
  * @brief Run indi rate interface from the "stabilization_rate_run" function
  */
-void stabilization_rate_run(bool in_flight)
+void stabilization_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 {
+  stabilization_rate_sp = stab_sp_to_rates_f(rate_sp);
+
   /* compute the INDI rate command */
-  stabilization_indi_rate_run(stabilization_rate_sp, in_flight);
+  stabilization_indi_rate_run(in_flight, rate_sp, thrust, cmd);
 }
+
+struct StabilizationSetpoint stabilization_rate_read_rc(struct RadioControl *rc)
+{
+  struct FloatRates rate_sp;
+  FLOAT_RATES_ZERO(rate_sp);
+  if (ROLL_RATE_DEADBAND_EXCEEDED(rc)) {
+    rate_sp.p = rc->values[RC_RATE_P] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  }
+  if (PITCH_RATE_DEADBAND_EXCEEDED(rc)) {
+    rate_sp.q = rc->values[RC_RATE_Q] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  }
+  if (YAW_RATE_DEADBAND_EXCEEDED(rc) && !THROTTLE_STICK_DOWN_FROM_RC(rc)) {
+    rate_sp.r = rc->values[RC_RATE_R] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  }
+  return stab_sp_from_rates_f(&rate_sp);
+}
+
diff --git a/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.c b/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.c
index fe4f59898f..104c9c2e65 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.c
+++ b/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.c
@@ -27,6 +27,7 @@
 #include "modules/ctrl/ctrl_module_innerloop_demo.h"
 #include "state.h"
 #include "modules/radio_control/radio_control.h"
+#include "generated/radio.h"
 #include "firmwares/rotorcraft/stabilization.h"
 
 struct ctrl_module_demo_struct {
@@ -42,9 +43,6 @@ float ctrl_module_demo_pr_d_gain = 0.1f;
 float ctrl_module_demo_y_ff_gain = 0.4f;   // Yaw
 float ctrl_module_demo_y_d_gain = 0.05f;
 
-void ctrl_module_init(void);
-void ctrl_module_run(bool in_flight);
-
 void ctrl_module_init(void)
 {
   ctrl_module_demo.rc_x = 0;
@@ -54,66 +52,41 @@ void ctrl_module_init(void)
 }
 
 // simple rate control without reference model nor attitude
-void ctrl_module_run(bool in_flight)
+static void ctrl_module_run(bool in_flight)
 {
   if (!in_flight) {
     // Reset integrators
-    stabilization_cmd[COMMAND_ROLL] = 0;
-    stabilization_cmd[COMMAND_PITCH] = 0;
-    stabilization_cmd[COMMAND_YAW] = 0;
-    stabilization_cmd[COMMAND_THRUST] = 0;
+    stabilization.cmd[COMMAND_ROLL] = 0;
+    stabilization.cmd[COMMAND_PITCH] = 0;
+    stabilization.cmd[COMMAND_YAW] = 0;
+    stabilization.cmd[COMMAND_THRUST] = 0;
   } else {
-    stabilization_cmd[COMMAND_ROLL]   = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain -
+    stabilization.cmd[COMMAND_ROLL]   = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain -
       stateGetBodyRates_i()->p * ctrl_module_demo_pr_d_gain;
-    stabilization_cmd[COMMAND_PITCH]  = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain -
+    stabilization.cmd[COMMAND_PITCH]  = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain -
       stateGetBodyRates_i()->q * ctrl_module_demo_pr_d_gain;
-    stabilization_cmd[COMMAND_YAW]    = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain -
+    stabilization.cmd[COMMAND_YAW]    = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain -
       stateGetBodyRates_i()->r * ctrl_module_demo_y_d_gain;
-    stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t;
+    stabilization.cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t;
   }
 }
 
 
 ////////////////////////////////////////////////////////////////////
 // Call our controller
-// Implement own Horizontal loops
-void guidance_h_module_init(void)
+void guidance_module_enter(void)
 {
   ctrl_module_init();
 }
 
-void guidance_h_module_enter(void)
-{
-  ctrl_module_init();
-}
-
-void guidance_h_module_read_rc(void)
+void guidance_module_run(bool in_flight)
 {
   // -MAX_PPRZ to MAX_PPRZ
   ctrl_module_demo.rc_t = radio_control.values[RADIO_THROTTLE];
   ctrl_module_demo.rc_x = radio_control.values[RADIO_ROLL];
   ctrl_module_demo.rc_y = radio_control.values[RADIO_PITCH];
   ctrl_module_demo.rc_z = radio_control.values[RADIO_YAW];
-}
-
-void guidance_h_module_run(bool in_flight)
-{
   // Call full inner-/outerloop / horizontal-/vertical controller:
   ctrl_module_run(in_flight);
 }
 
-void guidance_v_module_init(void)
-{
-  // initialization of your custom vertical controller goes here
-}
-
-// Implement own Vertical loops
-void guidance_v_module_enter(void)
-{
-  // your code that should be executed when entering this vertical mode goes here
-}
-
-void guidance_v_module_run(UNUSED bool in_flight)
-{
-  // your vertical controller goes here
-}
diff --git a/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.h b/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.h
index 2b474aea15..7bdb0d245d 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.h
+++ b/sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.h
@@ -36,22 +36,10 @@ extern float ctrl_module_demo_pr_d_gain;
 extern float ctrl_module_demo_y_ff_gain;   // Yaw
 extern float ctrl_module_demo_y_d_gain;
 
+extern void ctrl_module_init(void);
 
-// Demo with own guidance_h
-#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
-
-// and own guidance_v
-#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
-
-// Implement own Horizontal loops
-extern void guidance_h_module_init(void);
-extern void guidance_h_module_enter(void);
-extern void guidance_h_module_read_rc(void);
-extern void guidance_h_module_run(bool in_flight);
-
-// Implement own Vertical loops
-extern void guidance_v_module_init(void);
-extern void guidance_v_module_enter(void);
-extern void guidance_v_module_run(bool in_flight);
+// Implement own loops
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 #endif /* CTRL_MODULE_INNERLOOP_DEMO_H_ */
diff --git a/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.c b/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.c
index 6a35ed6948..efe1167d84 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.c
+++ b/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.c
@@ -27,16 +27,18 @@
 #include "modules/ctrl/ctrl_module_outerloop_demo.h"
 #include "state.h"
 #include "modules/radio_control/radio_control.h"
+#include "firmwares/rotorcraft/guidance/guidance_v.h"
 #include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
+#include "modules/radio_control/radio_control.h"
 #include "autopilot.h"
 
 // Own Variables
 
 struct ctrl_module_demo_struct {
 // RC Inputs
-  struct Int32Eulers rc_sp;
+  struct AttitudeRCInput rc_sp;
 
 // Output command
   struct Int32Eulers cmd;
@@ -50,28 +52,27 @@ float comode_time = 0;
 
 ////////////////////////////////////////////////////////////////////
 // Call our controller
-// Implement own Horizontal loops
-void guidance_h_module_init(void)
+void ctrl_module_init(void)
 {
+  stabilization_attitude_rc_setpoint_init(&ctrl.rc_sp);
 }
 
-void guidance_h_module_enter(void)
+void guidance_module_enter(void)
 {
   // Store current heading
   ctrl.cmd.psi = stateGetNedToBodyEulers_i()->psi;
 
   // Convert RC to setpoint
-  stabilization_attitude_read_rc_setpoint_eulers(&ctrl.rc_sp, autopilot.in_flight, false, false);
+  stabilization_attitude_read_rc_setpoint_eulers(&ctrl.rc_sp, autopilot_in_flight(), false, false, &radio_control);
+
+  // vertical mode in hover
+  guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
 }
 
-void guidance_h_module_read_rc(void)
+void guidance_module_run(bool in_flight)
 {
-  stabilization_attitude_read_rc_setpoint_eulers(&ctrl.rc_sp, autopilot.in_flight, false, false);
-}
+  stabilization_attitude_read_rc_setpoint_eulers(&ctrl.rc_sp, autopilot_in_flight(), false, false, &radio_control);
 
-
-void guidance_h_module_run(bool in_flight)
-{
   // YOUR NEW HORIZONTAL OUTERLOOP CONTROLLER GOES HERE
   // ctrl.cmd = CallMyNewHorizontalOuterloopControl(ctrl);
   float roll = 0.0;
@@ -80,9 +81,9 @@ void guidance_h_module_run(bool in_flight)
   ctrl.cmd.phi = ANGLE_BFP_OF_REAL(roll);
   ctrl.cmd.theta = ANGLE_BFP_OF_REAL(pitch);
 
-  stabilization_attitude_set_rpy_setpoint_i(&(ctrl.cmd));
-  stabilization_attitude_run(in_flight);
-
-  // Alternatively, use the indi_guidance and send AbiMsgACCEL_SP to it instead of setting pitch and roll
+  struct StabilizationSetpoint sp = stab_sp_from_eulers_i(&(ctrl.cmd));
+  struct ThrustSetpoint th = guidance_v_run(in_flight);
+  // execute attitude stabilization:
+  stabilization_attitude_run(in_flight, &sp, &th, stabilization.cmd);
 }
 
diff --git a/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.h b/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.h
index 0f480dc512..c900a31b6e 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.h
+++ b/sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.h
@@ -33,16 +33,10 @@
 // Settings
 extern float comode_time;
 
-// Demo with own guidance_h
-#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
-
-// But re-using an existing altitude-hold controller
-#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER
+extern void ctrl_module_init(void);
 
 // Implement own Horizontal loops
-extern void guidance_h_module_init(void);
-extern void guidance_h_module_enter(void);
-extern void guidance_h_module_read_rc(void);
-extern void guidance_h_module_run(bool in_flight);
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 #endif /* CTRL_MODULE_OUTERLOOP_DEMO_H_ */
diff --git a/sw/airborne/modules/ctrl/ctrl_windtunnel.c b/sw/airborne/modules/ctrl/ctrl_windtunnel.c
index 9203f40e30..22d8b22017 100644
--- a/sw/airborne/modules/ctrl/ctrl_windtunnel.c
+++ b/sw/airborne/modules/ctrl/ctrl_windtunnel.c
@@ -55,8 +55,6 @@ struct min_max_ctrl_t ctrl_windtunnel_throttle = {.min = CTRL_WINDTUNNEL_THR_MIN
 struct min_max_ctrl_t ctrl_windtunnel_flaps = {.min = CTRL_WINDTUNNEL_FLAP_MIN, .max = CTRL_WINDTUNNEL_FLAP_MAX, .step = CTRL_WINDTUNNEL_FLAP_STEP};
 static float last_time = 0;
 
-void ctrl_module_init(void);
-void ctrl_module_run(bool in_flight);
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -72,11 +70,11 @@ static void send_windtunnel_meas(struct transport_tx *trans, struct link_device
   float aoa = DegOfRad(windtunnel_to_body_e.theta);
   float power = electrical.vsupply * electrical.current;
   pprz_msg_send_WINDTUNNEL_MEAS(trans, dev, AC_ID, &aoa, &air_data.airspeed, &electrical.vsupply, &electrical.current,
-                                &power, COMMANDS_NB, stabilization_cmd);
+                                &power, COMMANDS_NB, stabilization.cmd);
 }
 #endif
 
-void ctrl_module_init(void)
+void ctrl_windtunnel_init(void)
 {
   ctrl_windtunnel.rc_throttle = 0;
   ctrl_windtunnel.rc_roll = 0;
@@ -93,7 +91,7 @@ void ctrl_module_init(void)
 #endif
 }
 
-void ctrl_module_run(bool in_flight __attribute__((unused)))
+static void ctrl_module_run(bool in_flight __attribute__((unused)))
 {
   bool done = false;
   // Increase step in steptime
@@ -127,54 +125,33 @@ void ctrl_module_run(bool in_flight __attribute__((unused)))
     last_time = get_sys_time_float();
   }
 
-  stabilization_cmd[COMMAND_ROLL] = 0;
-  stabilization_cmd[COMMAND_PITCH] = 0;
-  stabilization_cmd[COMMAND_YAW] = 0;
-  stabilization_cmd[COMMAND_THRUST] = (done) ? 0 : ctrl_windtunnel_throttle.current;
-  stabilization_cmd[COMMAND_FLAPS] = (done) ? 0 : ctrl_windtunnel_flaps.current;
+  stabilization.cmd[COMMAND_ROLL] = 0;
+  stabilization.cmd[COMMAND_PITCH] = 0;
+  stabilization.cmd[COMMAND_YAW] = 0;
+  stabilization.cmd[COMMAND_THRUST] = (done) ? 0 : ctrl_windtunnel_throttle.current;
+  stabilization.cmd[COMMAND_FLAPS] = (done) ? 0 : ctrl_windtunnel_flaps.current;
 }
 
 
 ////////////////////////////////////////////////////////////////////
 // Call our controller
-// Implement own Horizontal loops
-void guidance_h_module_init(void)
+// Implement own loops
+void guidance_module_enter(void)
 {
-  ctrl_module_init();
+  ctrl_windtunnel.rc_throttle = 0;
+  ctrl_windtunnel.rc_roll = 0;
+  ctrl_windtunnel.rc_pitch = 0;
+  ctrl_windtunnel.rc_yaw = 0;
 }
 
-void guidance_h_module_enter(void)
-{
-  ctrl_module_init();
-}
-
-void guidance_h_module_read_rc(void)
+void guidance_module_run(bool in_flight)
 {
   // -MAX_PPRZ to MAX_PPRZ
   ctrl_windtunnel.rc_throttle = radio_control.values[RADIO_THROTTLE];
   ctrl_windtunnel.rc_roll = radio_control.values[RADIO_ROLL];
   ctrl_windtunnel.rc_pitch = radio_control.values[RADIO_PITCH];
   ctrl_windtunnel.rc_yaw = radio_control.values[RADIO_YAW];
-}
-
-void guidance_h_module_run(bool in_flight)
-{
   // Call full inner-/outerloop / horizontal-/vertical controller:
   ctrl_module_run(in_flight);
 }
 
-void guidance_v_module_init(void)
-{
-  // initialization of your custom vertical controller goes here
-}
-
-// Implement own Vertical loops
-void guidance_v_module_enter(void)
-{
-  // your code that should be executed when entering this vertical mode goes here
-}
-
-void guidance_v_module_run(UNUSED bool in_flight)
-{
-  // your vertical controller goes here
-}
diff --git a/sw/airborne/modules/ctrl/ctrl_windtunnel.h b/sw/airborne/modules/ctrl/ctrl_windtunnel.h
index c5ad600998..7965458445 100644
--- a/sw/airborne/modules/ctrl/ctrl_windtunnel.h
+++ b/sw/airborne/modules/ctrl/ctrl_windtunnel.h
@@ -42,15 +42,11 @@ extern float ctrl_windtunnel_steptime;
 extern struct min_max_ctrl_t ctrl_windtunnel_throttle;
 extern struct min_max_ctrl_t ctrl_windtunnel_flaps;
 
-// Implement own Horizontal loops
-extern void guidance_h_module_init(void);
-extern void guidance_h_module_enter(void);
-extern void guidance_h_module_read_rc(void);
-extern void guidance_h_module_run(bool in_flight);
+extern void ctrl_windtunnel_init(void);
 
-// Implement own Vertical loops
-extern void guidance_v_module_init(void);
-extern void guidance_v_module_enter(void);
-extern void guidance_v_module_run(bool in_flight);
+// Implement own loops
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 #endif /* CTRL_MODULE_WINDTUNNEL_H_ */
+
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c b/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c
index 4b8487ef3e..6b0566e555 100644
--- a/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c
+++ b/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c
@@ -81,11 +81,9 @@ void eff_scheduling_cyfoam_init(void)
 
 static void eff_scheduling_periodic_a(void)
 {
-  // Go from transition percentage to ratio
-  float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;
-
   int8_t i;
   int8_t j;
+  const float ratio = stabilization.transition_ratio;
   for (i = 0; i < INDI_OUTPUTS; i++) {
     for (j = 0; j < INDI_NUM_ACT; j++) {
       g1g2[i][j] = g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio;
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_generic.c b/sw/airborne/modules/ctrl/eff_scheduling_generic.c
index b50250a0a3..dfda3bfd44 100644
--- a/sw/airborne/modules/ctrl/eff_scheduling_generic.c
+++ b/sw/airborne/modules/ctrl/eff_scheduling_generic.c
@@ -81,10 +81,9 @@ void eff_scheduling_generic_init(void)
 
 void eff_scheduling_generic_periodic(void)
 {
-  // Go from transition percentage to ratio
-  float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;
   int8_t i;
   int8_t j;
+  const float ratio = stabilization.transition_ratio;
   for (i = 0; i < INDI_OUTPUTS; i++) {
     for (j = 0; j < INDI_NUM_ACT; j++) {
       g1g2[i][j] = g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio;
diff --git a/sw/airborne/modules/ctrl/optical_flow_functions.c b/sw/airborne/modules/ctrl/optical_flow_functions.c
index 526b9c36f3..f4c577e2e4 100644
--- a/sw/airborne/modules/ctrl/optical_flow_functions.c
+++ b/sw/airborne/modules/ctrl/optical_flow_functions.c
@@ -43,6 +43,7 @@ uint8_t cov_array_filledXY;
 uint32_t ind_histZ;
 uint8_t cov_array_filledZ;
 
+struct OpticalFlowHover of_hover;
 
 /**
  * Set the covariance of the divergence and the thrust / past divergence
diff --git a/sw/airborne/modules/ctrl/optical_flow_functions.h b/sw/airborne/modules/ctrl/optical_flow_functions.h
index 8c63f6530d..0781d18ab0 100644
--- a/sw/airborne/modules/ctrl/optical_flow_functions.h
+++ b/sw/airborne/modules/ctrl/optical_flow_functions.h
@@ -70,7 +70,7 @@ extern uint8_t cov_array_filledXY;
 extern uint32_t ind_histZ;
 extern uint8_t cov_array_filledZ;
 
-struct OpticalFlowHover of_hover;
+extern struct OpticalFlowHover of_hover;
 
 extern float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs);
 extern void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY,
diff --git a/sw/airborne/modules/ctrl/optical_flow_hover.c b/sw/airborne/modules/ctrl/optical_flow_hover.c
index 737955f596..5d28ec2142 100644
--- a/sw/airborne/modules/ctrl/optical_flow_hover.c
+++ b/sw/airborne/modules/ctrl/optical_flow_hover.c
@@ -22,10 +22,12 @@
 #include "optical_flow_functions.h"
 
 #include "generated/airframe.h"
+#include "autopilot.h"
 #include "paparazzi.h"
 #include "modules/core/abi.h"
 #include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "firmwares/rotorcraft/guidance/guidance_v.h"
 #include "modules/energy/electrical.h"
 #include <stdio.h>
 
@@ -222,10 +224,8 @@ void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t sta
 // resetting all variables to be called for instance when starting up / re-entering module
 static void reset_horizontal_vars(void);
 static void reset_vertical_vars(void);
-void vertical_ctrl_module_init(void);
-void vertical_ctrl_module_run(bool in_flight);
-void horizontal_ctrl_module_init(void);
-void horizontal_ctrl_module_run(bool in_flight);
+void vertical_ctrl_module_run(void);
+void horizontal_ctrl_module_run(void);
 
 // Compute OptiTrack stabilization for 1/2 axes
 void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu);
@@ -298,45 +298,27 @@ void optical_flow_hover_init()
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTICAL_FLOW_HOVER, send_optical_flow_hover);
 }
 
-/**
- * Initialize the vertical optical flow hover module
- */
-void vertical_ctrl_module_init(void)
-{
-  // filling the of_hover_ctrl struct with default values:
-  reset_vertical_vars();
-}
-
-/**
- * Initialize the horizontal optical flow hover module
- */
-void horizontal_ctrl_module_init(void)
-{
-  // filling the of_hover_ctrl struct with default values:
-  reset_horizontal_vars();
-}
-
 /**
  * Reset all horizontal variables:
  */
 static void reset_horizontal_vars(void)
 {
   // Take the current angles as neutral
-  struct Int32Eulers tempangle;
-  int32_eulers_of_quat(&tempangle, &stab_att_sp_quat);
+  struct Int32Eulers tempangle = stab_sp_to_eulers_i(&stabilization.sp);
   of_hover_ctrl_X.nominal_value = DegOfRad(FLOAT_OF_BFP(tempangle.phi, INT32_ANGLE_FRAC));
   of_hover_ctrl_Y.nominal_value = DegOfRad(FLOAT_OF_BFP(tempangle.theta, INT32_ANGLE_FRAC));
 
   des_inputs.phi = 0;
   des_inputs.theta = 0;
 
-  if ((hover_method == 0) && (GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE)) {
+  // FIXME module is always used for vertical control
+  if ((hover_method == 0)) {
     // Z - X - Y Order
     oscillatingX = 1;
     oscillatingY = 1;
     of_hover_ctrl_X.PID.P = OFH_PGAINX;
     of_hover_ctrl_Y.PID.P = OFH_PGAINX;
-  } else if ((hover_method == 2) && (GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE)) {
+  } else if ((hover_method == 2)) {
     // Z Set XY
     oscillatingX = 1;
     oscillatingY = 1;
@@ -345,7 +327,7 @@ static void reset_horizontal_vars(void)
     of_hover_ctrl_X.PID.I = OFH_IGAINX / 4; // Have a slighly lower I gain during Z
     of_hover_ctrl_Y.PID.I = OFH_IGAINY / 4; // Have a slighly lower I gain during Z
   } else {
-    // if V is in NAV or hover_method = 1
+    // if V is in NAV or hover_method = 1 FIXME NAV is not called for vertical
     //All axes
     oscillatingX = 0;
     oscillatingY = 0;
@@ -420,13 +402,11 @@ static void reset_vertical_vars(void)
   prev_vision_timeZ = vision_time;
 }
 
-// Read H RC
-void guidance_h_module_read_rc(void) {}
 
 /**
  * Run the horizontal optical flow hover module
  */
-void horizontal_ctrl_module_run(bool in_flight)
+void horizontal_ctrl_module_run(void)
 {
   /***********
    * TIME
@@ -509,8 +489,6 @@ void horizontal_ctrl_module_run(bool in_flight)
 
   // Compute 0, 1 or 2 horizontal axes with optitrack
   computeOptiTrack(!oscphi, !osctheta, &ofh_sp_eu);
-  // Run the stabilization mode
-  stabilization_attitude_set_rpy_setpoint_i(&ofh_sp_eu);
 
   prev_vision_timeXY = vision_time;
 }
@@ -518,7 +496,7 @@ void horizontal_ctrl_module_run(bool in_flight)
 /**
  * Run the vertical optical flow hover module
  */
-void vertical_ctrl_module_run(bool in_flight)
+void vertical_ctrl_module_run(void)
 {
   /***********
    * TIME
@@ -595,11 +573,10 @@ void vertical_ctrl_module_run(bool in_flight)
     }
   }
 
-  stabilization_cmd[COMMAND_THRUST] = des_inputs.thrust;
 }
 
-void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int32_t flow_x, int32_t flow_y,
-                         int32_t flow_der_x, int32_t flow_der_y, float quality, float size_div)
+void ofh_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp, int32_t flow_x, int32_t flow_y,
+                         int32_t flow_der_x, int32_t flow_der_y, float quality UNUSED, float size_div)
 {
   if (!derotated) {
     flowX = flow_x;
@@ -615,63 +592,38 @@ void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t sta
 }
 
 ////////////////////////////////////////////////////////////////////
-// Call our vertical controller
-void guidance_v_module_init(void)
-{
-  vertical_ctrl_module_init();
-}
-
-// Call our horizontal controller
-void guidance_h_module_init(void)
-{
-  horizontal_ctrl_module_init();
-}
+// Call our controller
 
 /**
- * Entering the vertical module (user switched to module)
+ * Entering the module (user switched to module)
  */
-void guidance_v_module_enter(void)
+void guidance_module_enter(void)
 {
   reset_vertical_vars();
 
   // adaptive estimation - assume hover condition when entering the module
-  of_hover_ctrl_Z.nominal_value = (float) stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ;
+  of_hover_ctrl_Z.nominal_value = (float) stabilization.cmd[COMMAND_THRUST] / MAX_PPRZ;
   des_inputs.thrust = (int32_t) of_hover_ctrl_Z.nominal_value * MAX_PPRZ;
-}
 
-/**
- * Entering the horizontal module (user switched to module)
- */
-void guidance_h_module_enter(void)
-{
   // Set current psi as heading
   ofh_sp_eu.psi = stateGetNedToBodyEulers_i()->psi;
 
   VECT2_COPY(of_hover_ref_pos, *stateGetPositionNed_i());
   reset_horizontal_vars();
-
 }
 
-// Run the veritcal controller
-void guidance_v_module_run(bool in_flight)
+// Run the controller
+void guidance_module_run(bool in_flight)
 {
   if (electrical.bat_low) {
-    autopilot_static_set_mode(AP_MODE_NAV);
+    autopilot_set_mode(AP_MODE_NAV);
   } else {
-    // your vertical controller goes here
-    vertical_ctrl_module_run(in_flight);
-  }
-}
-
-// Run the horizontal controller
-void guidance_h_module_run(bool in_flight)
-{
-
-  if (electrical.bat_low) {
-    autopilot_static_set_mode(AP_MODE_NAV);
-  } else {
-    horizontal_ctrl_module_run(in_flight);
-    stabilization_attitude_run(in_flight);
+    // your controller goes here
+    vertical_ctrl_module_run();
+    horizontal_ctrl_module_run();
+    struct StabilizationSetpoint sp = stab_sp_from_eulers_i(&ofh_sp_eu);
+    struct ThrustSetpoint th = th_sp_from_thrust_i(des_inputs.thrust, THRUST_AXIS_Z);
+    stabilization_attitude_run(in_flight, &sp, &th, stabilization.cmd);
   }
 }
 
@@ -754,3 +706,4 @@ void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
     opti_sp_eu->theta = -(c_psi * of_hover_cmd_earth.x + s_psi * of_hover_cmd_earth.y) >> INT32_TRIG_FRAC;
   }
 }
+
diff --git a/sw/airborne/modules/ctrl/optical_flow_hover.h b/sw/airborne/modules/ctrl/optical_flow_hover.h
index e11c601908..2072195345 100644
--- a/sw/airborne/modules/ctrl/optical_flow_hover.h
+++ b/sw/airborne/modules/ctrl/optical_flow_hover.h
@@ -22,15 +22,6 @@
 #define OPTICAL_FLOW_HOVER_H_
 
 #include "std.h"
-// Without optitrack set to: GUIDANCE_V/H_MODE_ATTITUDE
-// With optitrack set to: GUIDANCE_V/H_MODE_NAV
-// To use the Optical Flow Hover module use GUIDANCE_V/H_MODE_MODULE
-
-#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
-// #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_NAV
-
-#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
-//#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_NAV
 
 extern bool oscphi;
 extern bool osctheta;
@@ -45,15 +36,8 @@ extern struct OpticalFlowHoverControl of_hover_ctrl_Z;
 // The module functions
 extern void optical_flow_hover_init(void);
 
-// Vertical loops
-extern void guidance_v_module_init(void);
-extern void guidance_v_module_enter(void);
-extern void guidance_v_module_run(bool in_flight);
-
-// Horizontal loops
-extern void guidance_h_module_init(void);
-extern void guidance_h_module_enter(void);
-extern void guidance_h_module_run(bool in_flight);
-extern void guidance_h_module_read_rc(void);
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 #endif /* OPTICAL_FLOW_LANDING_H_ */
+
diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.c b/sw/airborne/modules/ctrl/optical_flow_landing.c
index 0f0a73da74..e595741b65 100644
--- a/sw/airborne/modules/ctrl/optical_flow_landing.c
+++ b/sw/airborne/modules/ctrl/optical_flow_landing.c
@@ -81,6 +81,7 @@ float divergence_vision;
 #include "modules/core/abi.h"
 #include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "firmwares/rotorcraft/guidance/guidance_v.h"
 #include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
 
 // used for automated landing:
@@ -325,13 +326,12 @@ float past_divergence_history[OFL_COV_WINDOW_SIZE];
 uint32_t ind_hist;
 uint8_t cov_array_filled;
 
-void vertical_ctrl_module_init(void);
 void vertical_ctrl_module_run(bool in_flight);
 
 /**
  * Initialize the optical flow landing module
  */
-void vertical_ctrl_module_init(void)
+void optical_flow_landing_init(void)
 {
   // filling the of_landing_ctrl struct with default values:
   of_landing_ctrl.use_bias = true;
@@ -617,7 +617,7 @@ void vertical_ctrl_module_run(bool in_flight)
     if (module_active_time_sec < 2.5f) {
       int32_t nominal_throttle = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
       thrust_set = nominal_throttle;
-      stabilization_cmd[COMMAND_THRUST] = thrust;
+      stabilization.cmd[COMMAND_THRUST] = thrust;
       // keep track of histories and set the covariance
       set_cov_div(thrust_set);
       cov_div = 0.0f; // however, cov div is set to 0 to prevent strange issues at module startup.
@@ -902,7 +902,6 @@ void vertical_ctrl_module_run(bool in_flight)
 
     if (in_flight) {
       Bound(thrust_set, 0.25 * of_landing_ctrl.nominal_thrust * MAX_PPRZ, MAX_PPRZ);
-      stabilization_cmd[COMMAND_THRUST] = thrust_set;
     }
   }
 
@@ -987,9 +986,10 @@ void vertical_ctrl_module_run(bool in_flight)
   };
 
   // set the desired roll pitch and yaw:
-  stabilization_indi_set_rpy_setpoint_i(&rpy);
+  struct StabilizationSetpoint sp = stab_sp_from_eulers_i(&rpy);
+  struct ThrustSetpoint th = th_sp_from_thrust_i(thrust_set, THRUST_AXIS_Z);
   // execute attitude stabilization:
-  stabilization_attitude_run(in_flight);
+  stabilization_attitude_run(in_flight, &sp, &th, stabilization.cmd);
 }
 
 /**
@@ -1163,46 +1163,23 @@ void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp,
 ////////////////////////////////////////////////////////////////////
 // Call our controller
 
-void guidance_h_module_init(void)
-{
-
-}
-
-void guidance_h_module_enter(void)
-{
-
-}
-
-void guidance_h_module_run(bool UNUSED in_flight)
-{
-
-}
-
-void guidance_h_module_read_rc(void)
-{
-
-}
-
-void guidance_v_module_init(void)
-{
-  vertical_ctrl_module_init();
-}
-
 /**
  * Entering the module (user switched to module)
  */
-void guidance_v_module_enter(void)
+void guidance_module_enter(void)
 {
   reset_all_vars();
 
   // adaptive estimation - assume hover condition when entering the module
-  of_landing_ctrl.nominal_thrust = (float) stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ;
+  of_landing_ctrl.nominal_thrust = (float) stabilization.cmd[COMMAND_THRUST] / MAX_PPRZ;
   thrust_set = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
 }
 
-void guidance_v_module_run(bool in_flight)
+void guidance_module_run(bool in_flight)
 {
   vertical_ctrl_module_run(in_flight);
+
+  // FIXME horizontal guidance ?
 }
 
 // SSL:
diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.h b/sw/airborne/modules/ctrl/optical_flow_landing.h
index dc1986dd11..d3a19b4dcc 100644
--- a/sw/airborne/modules/ctrl/optical_flow_landing.h
+++ b/sw/airborne/modules/ctrl/optical_flow_landing.h
@@ -99,25 +99,15 @@ struct OpticalFlowLanding {
 
 extern struct OpticalFlowLanding of_landing_ctrl;
 
+extern void optical_flow_landing_init(void);
 
+// FIXME Horizontal control have to be fixed before use
 // Without optitrack set to: GUIDANCE_H_MODE_ATTITUDE
 // With optitrack set to: GUIDANCE_H_MODE_HOVER / NAV (NAV is the common option in the experiments.)
 
-#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
-
-// Own guidance_v
-#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
-
-// Implement own horizontal loop:
-extern void guidance_h_module_init(void);
-extern void guidance_h_module_enter(void);
-extern void guidance_h_module_run(bool in_flight);
-extern void guidance_h_module_read_rc(void);
-
-// Implement own Vertical loops
-extern void guidance_v_module_init(void);
-extern void guidance_v_module_enter(void);
-extern void guidance_v_module_run(bool in_flight);
+// Implement own loop:
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 // SSL functions:
 void save_texton_distribution(void);
diff --git a/sw/airborne/modules/ctrl/scheduling_indi_simple.c b/sw/airborne/modules/ctrl/scheduling_indi_simple.c
index 70e6f2adc2..0872471f39 100644
--- a/sw/airborne/modules/ctrl/scheduling_indi_simple.c
+++ b/sw/airborne/modules/ctrl/scheduling_indi_simple.c
@@ -45,7 +45,6 @@ void ctrl_eff_scheduling_init(void)
 void ctrl_eff_scheduling_periodic(void)
 {
   // Go from transition percentage to ratio
-  /*float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;*/
   float ratio = 0.0;
 
   // Ratio is only based on pitch now, as the pitot tube is often not mounted.
@@ -55,12 +54,6 @@ void ctrl_eff_scheduling_periodic(void)
     ratio = 0.0;
   }
 
-  //g_forward[0] = STABILIZATION_INDI_FORWARD_G1_P;
-  // When using the pitch slider to take the props out of the mix, adjust the pitch effectiveness
-  //g_forward[1] = STABILIZATION_INDI_FORWARD_G1_Q - (1.0 - pitch_slider) * STABILIZATION_INDI_MOT_PITCH_EFF;
-  // When using the yaw slider to take the props out of the mix, adjust the yaw effectiveness
-  //g_forward[2] = STABILIZATION_INDI_FORWARD_G1_R - (1.0 - yaw_slider) * STABILIZATION_INDI_MOT_YAW_EFF;
-
   indi.g1.p = g_hover[0] * (1.0 - ratio) + g_forward[0] * ratio;
   indi.g1.q = g_hover[1] * (1.0 - ratio) + g_forward[1] * ratio;
   indi.g1.r = g_hover[2] * (1.0 - ratio) + g_forward[2] * ratio;
@@ -83,3 +76,4 @@ void ctrl_eff_scheduling_periodic(void)
   guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN * (1.0 - ratio_spec_force)
                                   + GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD * ratio_spec_force;
 }
+
diff --git a/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c
index f5414ec4d2..2e654682b8 100644
--- a/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c
+++ b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c
@@ -27,8 +27,10 @@
 #include "modules/ctrl/vertical_ctrl_module_demo.h"
 
 #include "generated/airframe.h"
+#include "autopilot.h"
 #include "paparazzi.h"
 #include "modules/core/abi.h"
+#include "firmwares/rotorcraft/guidance/guidance_h.h"
 #include "firmwares/rotorcraft/stabilization.h"
 
 /* Default sonar/agl to use */
@@ -53,9 +55,6 @@ static void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distan
 struct VerticalCtrlDemo v_ctrl;
 
 
-void vertical_ctrl_module_init(void);
-void vertical_ctrl_module_run(bool in_flight);
-
 void vertical_ctrl_module_init(void)
 {
   v_ctrl.agl = 0.0f;
@@ -69,20 +68,22 @@ void vertical_ctrl_module_init(void)
 }
 
 
-void vertical_ctrl_module_run(bool in_flight)
+static struct ThrustSetpoint vertical_ctrl_module_run(bool in_flight)
 {
+  struct ThrustSetpoint th;
   if (!in_flight) {
     // Reset integrators
     v_ctrl.sum_err = 0;
-    stabilization_cmd[COMMAND_THRUST] = 0;
+    th = th_sp_from_thrust_i(0, THRUST_AXIS_Z);
   } else {
     int32_t nominal_throttle = 0.5 * MAX_PPRZ;
     float err = v_ctrl.setpoint - v_ctrl.agl;
     int32_t thrust = nominal_throttle + v_ctrl.pgain * err + v_ctrl.igain * v_ctrl.sum_err;
     Bound(thrust, 0, MAX_PPRZ);
-    stabilization_cmd[COMMAND_THRUST] = thrust;
+    th = th_sp_from_thrust_i(thrust, THRUST_AXIS_Z);
     v_ctrl.sum_err += err;
   }
+  return th;
 }
 
 static void vertical_ctrl_agl_cb(__attribute__((unused)) uint8_t sender_id, __attribute__((unused)) uint32_t stamp, float distance)
@@ -93,18 +94,17 @@ static void vertical_ctrl_agl_cb(__attribute__((unused)) uint8_t sender_id, __at
 
 ////////////////////////////////////////////////////////////////////
 // Call our controller
-void guidance_v_module_init(void)
-{
-  vertical_ctrl_module_init();
-}
-
-void guidance_v_module_enter(void)
+void guidance_module_enter(void)
 {
   // reset integrator
   v_ctrl.sum_err = 0.0f;
+
+  guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
 }
 
-void guidance_v_module_run(bool in_flight)
+void guidance_module_run(bool in_flight)
 {
-  vertical_ctrl_module_run(in_flight);
+  struct ThrustSetpoint th = vertical_ctrl_module_run(in_flight);
+  struct StabilizationSetpoint stab = guidance_h_run(in_flight);
+  stabilization_run(in_flight, &stab, &th, stabilization.cmd);
 }
diff --git a/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h
index d5f834c3c7..37a20fcdc0 100644
--- a/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h
+++ b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h
@@ -46,9 +46,10 @@ extern struct VerticalCtrlDemo v_ctrl;
 // and own guidance_v
 #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
 
+extern void vertical_ctrl_module_init(void);
+
 // Implement own Vertical loops
-extern void guidance_v_module_init(void);
-extern void guidance_v_module_enter(void);
-extern void guidance_v_module_run(bool in_flight);
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 #endif /* VERTICAL_CTRL_MODULE_DEMO_H_ */
diff --git a/sw/airborne/modules/display/max7456.c b/sw/airborne/modules/display/max7456.c
index c5466cd76f..9e5cb41d82 100644
--- a/sw/airborne/modules/display/max7456.c
+++ b/sw/airborne/modules/display/max7456.c
@@ -767,7 +767,7 @@ void draw_osd(void)
 #if defined(FIXEDWING_FIRMWARE)
       osd_sprintf(osd_string, "THR%.0f", (((float)command_get(COMMAND_THROTTLE) / (float)MAX_PPRZ) * 100.));
 #else
-      osd_sprintf(osd_string, "THR%.0fTHR", (((float)stabilization_cmd[COMMAND_THRUST] / (float)MAX_PPRZ) * 100.));
+      osd_sprintf(osd_string, "THR%.0fTHR", (((float)stabilization.cmd[COMMAND_THRUST] / (float)MAX_PPRZ) * 100.));
 #endif
       osd_put_s(osd_string, L_JUST, 6, 3, 1);
       step = 60;
diff --git a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
index 57e2d55e22..afbf890682 100644
--- a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
+++ b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
@@ -102,17 +102,17 @@ static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unus
 /**
  * Initialization of horizontal guidance module.
  */
-void guidance_h_module_init(void)
+void guidance_opticflow_hover_init(void)
 {
   // Subscribe to the VELOCITY_ESTIMATE ABI message
   AbiBindMsgVELOCITY_ESTIMATE(VISION_VELOCITY_ESTIMATE_ID, &velocity_est_ev, stabilization_opticflow_vel_cb);
 }
 
 /**
- * Horizontal guidance mode enter resets the errors
+ * guidance mode enter resets the errors
  * and starts the controller.
  */
-void guidance_h_module_enter(void)
+void guidance_module_enter(void)
 {
   /* Reset the integrated errors */
   opticflow_stab.err_vx_int = 0;
@@ -122,40 +122,29 @@ void guidance_h_module_enter(void)
   opticflow_stab.cmd.phi = 0;
   opticflow_stab.cmd.theta = 0;
   opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi;
-}
 
-/**
- * Read the RC commands
- */
-void guidance_h_module_read_rc(void)
-{
-  // TODO: change the desired vx/vy
+  guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
 }
 
 /**
  * Main guidance loop
  * @param[in] in_flight Whether we are in flight or not
  */
-void guidance_h_module_run(bool in_flight)
+void guidance_module_run(bool in_flight)
 {
-  /* Update the setpoint */
-  stabilization_attitude_set_rpy_setpoint_i(&opticflow_stab.cmd);
-
+  struct StabilizationSetpoint sp = stab_sp_from_eulers_i(&opticflow_stab.cmd);
+  struct ThrustSetpoint th = guidance_v_run(in_flight);
   /* Run the default attitude stabilization */
-  stabilization_attitude_run(in_flight);
+  stabilization_attitude_run(in_flight, &sp, &th, stabilization.cmd);
 }
 
 /**
  * Update the controls on a new VELOCITY_ESTIMATE ABI message.
  */
 static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
-    uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise_x, float noise_y, float noise_z)
+    uint32_t stamp UNUSED, float vel_x, float vel_y, float vel_z UNUSED,
+    float noise_x, float noise_y, float noise_z UNUSED)
 {
-  /* Check if we are in the correct AP_MODE before setting commands */
-  if (autopilot_get_mode() != AP_MODE_MODULE) {
-    return;
-  }
-
   if (noise_x >= 0.f)
   {
     /* Calculate the error */
diff --git a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.h b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.h
index 08a1e954e1..397814c500 100644
--- a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.h
+++ b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.h
@@ -48,10 +48,10 @@ struct opticflow_stab_t {
 };
 extern struct opticflow_stab_t opticflow_stab;
 
-// Implement own Horizontal loops
-extern void guidance_h_module_init(void);
-extern void guidance_h_module_enter(void);
-extern void guidance_h_module_read_rc(void);
-extern void guidance_h_module_run(bool in_flight);
+extern void guidance_opticflow_hover_init(void);
+
+// Implement own module loops
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 #endif /* GUIDANCE_OPTICFLOW_HOVER_H_ */
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 7267686a26..5f57af5440 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -789,18 +789,13 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
     ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
 
 #ifndef NO_RESET_UPDATE_SETPOINT_HEADING
-
+    // FIXME is this hard reset of control setpoint really needed ? is it the right place ?
     if (ekf2.quat_reset_counter < quat_reset_counter) {
       float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
-#if defined STABILIZATION_ATTITUDE_TYPE_INT
-      stab_att_sp_euler.psi += ANGLE_BFP_OF_REAL(psi);
-#else
-      stab_att_sp_euler.psi += psi;
-#endif
       guidance_h.sp.heading += psi;
-      guidance_h.rc_sp.psi += psi;
+      guidance_h.rc_sp.heading += psi;
       nav.heading += psi;
-      guidance_h_read_rc(autopilot_in_flight());
+      //guidance_h_read_rc(autopilot_in_flight());
       stabilization_attitude_enter();
       ekf2.quat_reset_counter = quat_reset_counter;
     }
diff --git a/sw/airborne/modules/ins/ins_flow.c b/sw/airborne/modules/ins/ins_flow.c
index e3523db314..ea2b792f3a 100644
--- a/sw/airborne/modules/ins/ins_flow.c
+++ b/sw/airborne/modules/ins/ins_flow.c
@@ -456,13 +456,13 @@ static void send_ins_flow(struct transport_tx *trans, struct link_device *dev)
     // normally:
     p = phi_dot;
     // when estimating the gyros:
-    // // p = -1.8457e-04 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
-    // p = -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
+    // // p = -1.8457e-04 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
+    // p = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
     // TODO: expand the full filter later as well, to include q:
     q = q_GT;
   } else {
     p = ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll;
-    // p = -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
+    // p = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
     q = ins_flow.lp_gyro_pitch - ins_flow.lp_gyro_bias_pitch;
   }
 
@@ -762,7 +762,7 @@ void ins_flow_update(void)
   }
   // same assumption for the roll command: assuming a close-to-hover situation and roll trim for staying in place:
   ins_flow.lp_roll_command = lp_factor_strong * ins_flow.lp_roll_command + (1 - lp_factor_strong) *
-                             stabilization_cmd[COMMAND_ROLL];
+                             stabilization.cmd[COMMAND_ROLL];
 
   // only start estimation when flying (and above 1 meter: || position->z > -1.0f )
   // I removed the condition on height, since (1) we need to start the filter anyway explicitly now, and (2) it created a dependence on GPS fix.
@@ -842,7 +842,7 @@ void ins_flow_update(void)
 #else
 
   /*
-  moments[moment_ind] = Ix *(-0.000553060716181365 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
+  moments[moment_ind] = Ix *(-0.000553060716181365 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
 
   int select_ind = moment_ind - MOMENT_DELAY;
   if(select_ind < 0) {
@@ -858,7 +858,7 @@ void ins_flow_update(void)
   moment_ind = 0;
   }
   */
-  // moment = Ix *(-0.000553060716181365 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
+  // moment = Ix *(-0.000553060716181365 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
   moment = 0;
 #endif
 
@@ -884,7 +884,7 @@ void ins_flow_update(void)
     } */
 
     // temporary insertion of gyro estimate here, for quicker effect:
-    // OF_X[OF_ANGLE_IND] += dt * -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
+    // OF_X[OF_ANGLE_IND] += dt * -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
 
     OF_X[OF_ANGLE_IND] += dt * OF_X[OF_ANGLE_DOT_IND];
     OF_X[OF_ANGLE_DOT_IND] += dt * (moment / Ix);
@@ -1241,10 +1241,10 @@ void ins_flow_update(void)
       } else {
         // TODO: You can fake gyros here by estimating them as follows:
         // rate_p_filt_est = -1.8457e-04 * cmd_roll;
-        // gyro_meas_roll = -1.8457e-04 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
-        // gyro_meas_roll = -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
+        // gyro_meas_roll = -1.8457e-04 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
+        // gyro_meas_roll = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
 
-        // gyro_meas_roll = 1e-04 * parameters[PAR_PRED_ROLL_1] * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
+        // gyro_meas_roll = 1e-04 * parameters[PAR_PRED_ROLL_1] * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
         // gyro_meas_roll = parameters[PAR_PRED_ROLL_2] * gyro_meas_roll + 1E-3 * parameters[PAR_PRED_ROLL_3] * ins_flow.optical_flow_x;
 
         // only flow:
diff --git a/sw/airborne/modules/ins/ins_int.c b/sw/airborne/modules/ins/ins_int.c
index 3569ca8ed8..567ee47e81 100644
--- a/sw/airborne/modules/ins/ins_int.c
+++ b/sw/airborne/modules/ins/ins_int.c
@@ -492,7 +492,7 @@ static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unu
   }
 #endif
 #ifdef INS_AGL_THROTTLE_THRESHOLD
-   if(stabilization_cmd[COMMAND_THRUST] < INS_AGL_THROTTLE_THRESHOLD){
+   if(stabilization.cmd[COMMAND_THRUST] < INS_AGL_THROTTLE_THRESHOLD){
      return;
    }
 #endif
diff --git a/sw/airborne/modules/loggers/direct_memory_logger.c b/sw/airborne/modules/loggers/direct_memory_logger.c
index 9968648a2c..96311691e7 100644
--- a/sw/airborne/modules/loggers/direct_memory_logger.c
+++ b/sw/airborne/modules/loggers/direct_memory_logger.c
@@ -135,7 +135,7 @@ void direct_memory_logger_periodic(void)
       log_struct.gyro_p    = imu.gyro.p;
       log_struct.gyro_q    = imu.gyro.q;
       log_struct.gyro_r    = imu.gyro.r;
-      log_struct.thrust    = stabilization_cmd[COMMAND_THRUST];
+      log_struct.thrust    = stabilization.cmd[COMMAND_THRUST];
 
       sst25vfxxxx_write(&dml.sst, (uint8_t *) &log_struct, sizeof(struct LogStruct));
       break;
diff --git a/sw/airborne/modules/loggers/logger_control_effectiveness.c b/sw/airborne/modules/loggers/logger_control_effectiveness.c
index 0bccc7eac6..550326b969 100644
--- a/sw/airborne/modules/loggers/logger_control_effectiveness.c
+++ b/sw/airborne/modules/loggers/logger_control_effectiveness.c
@@ -118,7 +118,7 @@ void logger_control_effectiveness_periodic(void)
 #if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
   for (unsigned int i = 0; i < COMMANDS_NB; i++) {
 #ifdef ROTORCRAFT_FIRMWARE
-    sdLogWriteLog(pprzLogFile, ",%ld", stabilization_cmd[i]);
+    sdLogWriteLog(pprzLogFile, ",%ld", stabilization.cmd[i]);
 #endif
 #ifdef FIXEDWING_FIRMWARE
     sdLogWriteLog(pprzLogFile, ",%d", commands[i]);
diff --git a/sw/airborne/modules/loggers/logger_file.c b/sw/airborne/modules/loggers/logger_file.c
index 1abbaf2cde..55732e1f2c 100644
--- a/sw/airborne/modules/loggers/logger_file.c
+++ b/sw/airborne/modules/loggers/logger_file.c
@@ -108,8 +108,8 @@ static void logger_file_write_row(FILE *file) {
 #endif
 #ifdef COMMAND_THRUST
   fprintf(file, "%d,%d,%d,%d\n",
-      stabilization_cmd[COMMAND_THRUST], stabilization_cmd[COMMAND_ROLL],
-      stabilization_cmd[COMMAND_PITCH], stabilization_cmd[COMMAND_YAW]);
+      stabilization.cmd[COMMAND_THRUST], stabilization.cmd[COMMAND_ROLL],
+      stabilization.cmd[COMMAND_PITCH], stabilization.cmd[COMMAND_YAW]);
 #else
   fprintf(file, "%d,%d\n", h_ctl_aileron_setpoint, h_ctl_elevator_setpoint);
 #endif
diff --git a/sw/airborne/modules/nav/nav_heli_spinup.c b/sw/airborne/modules/nav/nav_heli_spinup.c
index 7fe96a567a..0b048cfccf 100644
--- a/sw/airborne/modules/nav/nav_heli_spinup.c
+++ b/sw/airborne/modules/nav/nav_heli_spinup.c
@@ -43,13 +43,13 @@ void nav_heli_spinup_setup(uint16_t duration, float throttle)
   nav_heli_spinup.throttle = throttle * MAX_PPRZ;
 
 #ifdef COMMAND_ROLL
-  stabilization_cmd[COMMAND_ROLL]  = 0;
+  stabilization.cmd[COMMAND_ROLL]  = 0;
 #endif
 #ifdef COMMAND_PITCH
-  stabilization_cmd[COMMAND_PITCH] = 0;
+  stabilization.cmd[COMMAND_PITCH] = 0;
 #endif
 #ifdef COMMAND_YAW
-  stabilization_cmd[COMMAND_YAW]   = 0;
+  stabilization.cmd[COMMAND_YAW]   = 0;
 #endif
   nav.throttle = 0;
 
@@ -68,13 +68,13 @@ bool nav_heli_spinup_run(void)
   }
 
 #ifdef COMMAND_ROLL
-  stabilization_cmd[COMMAND_ROLL]  = 0;
+  stabilization.cmd[COMMAND_ROLL]  = 0;
 #endif
 #ifdef COMMAND_PITCH
-  stabilization_cmd[COMMAND_PITCH] = 0;
+  stabilization.cmd[COMMAND_PITCH] = 0;
 #endif
 #ifdef COMMAND_YAW
-  stabilization_cmd[COMMAND_YAW]   = 0;
+  stabilization.cmd[COMMAND_YAW]   = 0;
 #endif
   nav.throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration;
 
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
index 1c82552cf3..82070836ee 100644
--- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
+++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
@@ -71,10 +71,10 @@ float nav_hybrid_line_gain = 1.0f;
 #define NAV_HYBRID_NAV_CIRCLE_DIST 40.f
 #endif
 
-#ifdef NAV_HYBRID_POS_GAIN 
-float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN; 
+#ifdef NAV_HYBRID_POS_GAIN
+float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN;
 #else
-float nav_hybrid_pos_gain = 1.0; 
+float nav_hybrid_pos_gain = 1.0;
 #endif
 
 #ifndef GUIDANCE_INDI_HYBRID
diff --git a/sw/airborne/modules/obstacle_avoidance/guidance_OA.c b/sw/airborne/modules/obstacle_avoidance/guidance_OA.c
index a0d9c23316..6fcc11a540 100644
--- a/sw/airborne/modules/obstacle_avoidance/guidance_OA.c
+++ b/sw/airborne/modules/obstacle_avoidance/guidance_OA.c
@@ -135,16 +135,11 @@ struct FloatVect3 Total_force = {0, 0, 0};
 //    //                          &opticflow_stab.desired_vy, &opti_speed_read.x, &opti_speed_read.y);
 //}
 
-void guidance_h_module_init(void)
-{
-
-}
-
 /**
  * Horizontal guidance mode enter resets the errors
  * and starts the controller.
  */
-void guidance_h_module_enter(void)
+void guidance_module_enter(void)
 {
   /* Reset the integrated errors */
   opticflow_stab.err_vx_int = 0;
@@ -158,28 +153,21 @@ void guidance_h_module_enter(void)
   new_heading = 0;
 
 //  register_periodic_telemetry(DefaultPeriodic, "INPUT_CONTROL", send_INPUT_CONTROL);
-}
 
-/**
- * Read the RC commands
- */
-void guidance_h_module_read_rc(void)
-{
-  // TODO: change the desired vx/vy
+  guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
 }
 
 /**
  * Main guidance loop
  * @param[in] in_flight Whether we are in flight or not
  */
-void guidance_h_module_run(bool in_flight)
+void guidance_module_run(bool in_flight)
 {
   OA_update();
-  /* Update the setpoint */
-  stabilization_attitude_set_rpy_setpoint_i(&opticflow_stab.cmd);
-
+  struct StabilizationSetpoint sp = stab_sp_from_eulers_i(&opticflow_stab.cmd);
+  struct ThrustSetpoint th = guidance_v_run(in_flight);
   /* Run the default attitude stabilization */
-  stabilization_attitude_run(in_flight);
+  stabilization_attitude_run(in_flight, &sp, &th, stabilization.cmd);
 }
 
 /**
diff --git a/sw/airborne/modules/obstacle_avoidance/guidance_OA.h b/sw/airborne/modules/obstacle_avoidance/guidance_OA.h
index 94aeeceb21..b0dbcc5c60 100644
--- a/sw/airborne/modules/obstacle_avoidance/guidance_OA.h
+++ b/sw/airborne/modules/obstacle_avoidance/guidance_OA.h
@@ -62,10 +62,8 @@ extern float r_dot_new;
 extern float speed_pot;
 
 // Implement own Horizontal loops
-extern void guidance_h_module_init(void);
-extern void guidance_h_module_enter(void);
-extern void guidance_h_module_read_rc(void);
-extern void guidance_h_module_run(bool in_flight);
+extern void guidance_module_enter(void);
+extern void guidance_module_run(bool in_flight);
 
 // Update the stabilization commands based on a vision result
 extern void OA_update(void);
diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.c b/sw/airborne/modules/rot_wing_drone/rotwing_state.c
index 97a2ae2b4a..6df0fb6132 100644
--- a/sw/airborne/modules/rot_wing_drone/rotwing_state.c
+++ b/sw/airborne/modules/rot_wing_drone/rotwing_state.c
@@ -304,7 +304,7 @@ void rotwing_check_set_current_state(void)
       }
 
       // Check if state needs to be set to fixed wing with hover motors idle (If hover thrust below threshold)
-      if (stabilization_cmd[COMMAND_THRUST] < ROTWING_MIN_THRUST_IDLE && rotwing_state.desired_state > ROTWING_STATE_FW) {
+      if (stabilization.cmd[COMMAND_THRUST] < ROTWING_MIN_THRUST_IDLE && rotwing_state.desired_state > ROTWING_STATE_FW) {
         rotwing_state_fw_idle_counter++;
       } else {
         rotwing_state_fw_idle_counter = 0;
@@ -325,7 +325,7 @@ void rotwing_check_set_current_state(void)
 
     case ROTWING_STATE_FW_HOV_MOT_IDLE:
       // Check if state needs to be set to fixed wing with hover motors activated
-      if (stabilization_cmd[COMMAND_THRUST] > ROTWING_MIN_THRUST_IDLE
+      if (stabilization.cmd[COMMAND_THRUST] > ROTWING_MIN_THRUST_IDLE
           || rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) {
         rotwing_state_fw_counter++;
       } else {
diff --git a/sw/tools/generators/gen_autopilot.ml b/sw/tools/generators/gen_autopilot.ml
index a7e3fbfa94..7a858ed54a 100644
--- a/sw/tools/generators/gen_autopilot.ml
+++ b/sw/tools/generators/gen_autopilot.ml
@@ -251,10 +251,17 @@ let print_ap_periodic = fun modes ctrl_block main_freq name out_h ->
 
   (** Print function *)
   let print_call = fun call ->
+    let store =
+      try
+        let s = Xml.attrib call "store" in
+        let s = List.hd (List.rev (Str.split (Str.regexp " ") s)) in (* extract last word *)
+        sprintf "%s = " s
+      with _ -> ""
+    in
     try
       let f = Xml.attrib call "fun" in
-      let cond = try String.concat "" ["if ("; (Xml.attrib call "cond"); ") { "; f; "; }\n"]
-        with _ -> String.concat "" [f; ";\n"] in
+      let cond = try String.concat "" ["if ("; (Xml.attrib call "cond"); ") { "; store; f; "; }\n"]
+        with _ -> String.concat "" [store; f; ";\n"] in
       lprintf out_h "%s" cond
     with _ -> ()
   in
@@ -293,9 +300,25 @@ let print_ap_periodic = fun modes ctrl_block main_freq name out_h ->
     List.filter (fun m -> (Xml.tag m) = "control") (Xml.children mode)
   in
 
+  (** Find store attributes *)
+  let rec get_stores = fun acc x ->
+    match x with
+    | [] -> acc
+    | h::xs -> begin
+      try
+        let s = Xml.attrib h "store" in
+        get_stores (s::acc) xs
+      with _ ->
+        get_stores (get_stores acc (Xml.children h)) xs
+    end
+  in
+  let stores = Gen_common.singletonize (get_stores [] (modes @ ctrl_block)) in
+
+
   (** Start printing the main periodic task *)
   lprintf out_h "\nstatic inline void autopilot_core_%s_periodic_task(void) {\n\n" name;
   right ();
+  List.iter (fun s -> lprintf out_h "%s;\n" s) stores;
   lprintf out_h "uint8_t mode = autopilot_core_%s_mode_select();\n" name; (* get selected mode *)
   lprintf out_h "mode = autopilot_core_%s_mode_exceptions(mode);\n" name; (* change mode according to exceptions *)
   lprintf out_h "mode = autopilot_core_%s_global_exceptions(mode);\n" name; (* change mode according to global exceptions *)
diff --git a/tests/modules/generated/airframe.h b/tests/modules/generated/airframe.h
index 2b11ce1b88..8034925a74 100644
--- a/tests/modules/generated/airframe.h
+++ b/tests/modules/generated/airframe.h
@@ -17,6 +17,9 @@
 #define COMMAND_YAW 2
 #define COMMAND_THRUST 3
 
+// extra commands
+#define COMMAND_FLAPS 4
+
 #define SetCommandsFromRC(_commands_array, _rc_array) { }
 #define AllActuatorsInit() { }
 #define AllActuatorsCommit() { }
@@ -53,4 +56,10 @@
 #define INS_ROLL_NEUTRAL_DEFAULT 0.
 #define INS_PITCH_NEUTRAL_DEFAULT 0.
 
+#define SECTION_STABILIZATION_ATTITUDE 1
+#define STABILIZATION_ATTITUDE_SP_MAX_PHI 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_R 1.04719755
+#define STABILIZATION_ATTITUDE_DEADBAND_R 250
+
 #endif // AIRFRAME_H
diff --git a/tests/modules/generated/modules.h b/tests/modules/generated/modules.h
index 70e0efdb24..53afce9180 100644
--- a/tests/modules/generated/modules.h
+++ b/tests/modules/generated/modules.h
@@ -9,6 +9,7 @@
 #include "std.h"
 #include "core/settings.h"
 #include "radio_control/radio_control.h"
+#include "generated/radio.h"
 #include "./mcu.h"
 #include "mcu_periph/gpio.h"
 #include "math/pprz_algebra_int.h"