diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
index 3f05c841bd..bbb19e635f 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
@@ -114,6 +114,16 @@ static float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU];
 #if GUIDANCE_INDI_USE_WLS
 #include "math/wls/wls_alloc.h"
 
+// priorities on control inputs (ax, ay, az)
+#ifndef GUIDANCE_INDI_WLS_PRIORITIES
+#define GUIDANCE_INDI_WLS_PRIORITIES {100.f, 100.f, 100.f}
+#endif
+
+// weighting of prefered control outputs (phi, theta, psi, Tx, Ty, Tz)
+#ifndef GUIDANCE_INDI_WLS_WU
+#define GUIDANCE_INDI_WLS_WU {1000.f,1000.f,10.f,1.f,1.f,1.f}
+#endif
+
 #define NU_MAX 6    // Example constant value // [dtheta, dphi, dthrust, dtx , dty]
 #define NV_MAX 3    // Example constant value (ax,ay,az)
 static float du_guidance[GUIDANCE_INDI_NU];
@@ -122,10 +132,10 @@ static float *Bwls_gi[GUIDANCE_INDI_NV];
 static struct WLS_t wls_guid_p = {
   .nu        = GUIDANCE_INDI_NU,
   .nv        = GUIDANCE_INDI_NV,
-  .gamma_sq  = 1000.0,
+  .gamma_sq  = 100.0,
   .v         = {0.0},
-  .Wv        = {100.f, 100.f, 100.f},         // x,y,z
-  .Wu        = {10.f,10.f,0.f,0.f,0.f,10.f}, // minimize the control input (thetq,phi,Tx,Ty,Tz,psi)
+  .Wv        = GUIDANCE_INDI_WLS_PRIORITIES,
+  .Wu        = GUIDANCE_INDI_WLS_WU,
   .u_pref    = {0.0},
   .u_min     = {0.0},
   .u_max     = {0.0},
@@ -266,7 +276,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
 #endif
 
 #if GUIDANCE_INDI_USE_WLS
-  float m = GUIDANCE_INDI_MASS;
+  const float m = GUIDANCE_INDI_MASS;
   wls_guid_p.v[0] = m*a_diff.x;
   wls_guid_p.v[1] = m*a_diff.y;
   wls_guid_p.v[2] = m*a_diff.z;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c
index 0ef4f70de1..f8b7fecdf7 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c
@@ -35,7 +35,7 @@
 // FIXME should be a function of the mass
 
 #ifndef GUIDANCE_INDI_MAX_H_THRUST
-#define GUIDANCE_INDI_MAX_H_THRUST 1.f
+#define GUIDANCE_INDI_MAX_H_THRUST 2.f
 #endif
 
 #ifndef GUIDANCE_INDI_MAX_V_THRUST
@@ -55,27 +55,27 @@ float guidance_indi_max_v_thrust = GUIDANCE_INDI_MAX_V_THRUST;
 static void guidance_indi_calcG_yxz(float Gmat[3][6], struct FloatEulers euler_yxz)
 {
   // Euler Angles
-  float sphi = sinf(euler_yxz.phi);
-  float cphi = cosf(euler_yxz.phi);
-  float stheta = sinf(euler_yxz.theta);
-  float ctheta = cosf(euler_yxz.theta);
-  float cpsi  = cosf(euler_yxz.psi);
-  float spsi  = sinf(euler_yxz.psi);
+  const float sphi = sinf(euler_yxz.phi);
+  const float cphi = cosf(euler_yxz.phi);
+  const float stheta = sinf(euler_yxz.theta);
+  const float ctheta = cosf(euler_yxz.theta);
+  const float cpsi  = cosf(euler_yxz.psi);
+  const float spsi  = sinf(euler_yxz.psi);
 
   // Estimated Thrust
-  float Tx = stab_thrust_filt.x;
-  float Ty = stab_thrust_filt.y;
-  float Tz = stab_thrust_filt.z;
+  const float Tx = stab_thrust_filt.x;
+  const float Ty = stab_thrust_filt.y;
+  const float Tz = stab_thrust_filt.z;
 
   // dPhi (Roll)
-  Gmat[0][0] = Tx * (-spsi * cphi * stheta) + Ty * (spsi * sphi) + Tz * (spsi * cphi * ctheta);
-  Gmat[1][0] = Tx * (cpsi * cphi * stheta) + Ty * (-cpsi * sphi) + Tz * (-cpsi * cphi * ctheta);
-  Gmat[2][0] = Tx * (sphi * stheta) + Ty * (cphi) + Tz * (-sphi * ctheta);
+  Gmat[0][0] = Tx * (stheta * cphi * spsi) + Ty * (stheta * cphi * cpsi) + Tz * (-stheta * sphi);
+  Gmat[1][0] = Tx * (-sphi * spsi) + Ty * (-sphi * cpsi) + Tz * (-cphi);
+  Gmat[2][0] = Tx * (ctheta * cphi * spsi) + Ty * (ctheta * cphi * cpsi) + Tz * (-ctheta * sphi);
 
   // dTheta (Pitch)
-  Gmat[0][1] = Tx * (-cpsi * stheta - spsi * sphi * ctheta) + Tz * (cpsi * ctheta - spsi * sphi * stheta);
-  Gmat[1][1] = Tx * (-spsi * stheta + cpsi * sphi * ctheta) + Tz * (spsi * ctheta + cpsi * sphi * stheta);
-  Gmat[2][1] = Tx * (-cphi * ctheta)                        + Tz * (-cphi * stheta);
+  Gmat[0][1] = Tx * (-stheta * cpsi + ctheta * sphi * spsi) + Ty * (stheta * spsi + ctheta * sphi * cpsi) + Tz * (ctheta * cphi);
+  Gmat[1][1] = 0.f;
+  Gmat[2][1] = Tx * (-ctheta * cpsi - stheta * sphi * spsi) +Ty * (ctheta * spsi - stheta * sphi * cpsi) + Tz * (-stheta * cphi);
 
   // dPsi added to add constraints on psi
   Gmat[0][2] = 0.f;
@@ -83,19 +83,19 @@ static void guidance_indi_calcG_yxz(float Gmat[3][6], struct FloatEulers euler_y
   Gmat[2][2] = 0.f;
 
   // dTx
-  Gmat[0][3] =  cpsi * ctheta - spsi * sphi * stheta;
-  Gmat[1][3] =  spsi * ctheta + cpsi * sphi * stheta;
-  Gmat[2][3] = -stheta * cphi;
+  Gmat[0][3] =  ctheta * cpsi - stheta * sphi * spsi;
+  Gmat[1][3] =  ctheta * spsi;
+  Gmat[2][3] = -stheta * cpsi + ctheta * sphi * spsi;
 
   // dTy
-  Gmat[0][4] = -spsi * cphi;
-  Gmat[1][4] =  cpsi * cphi;
-  Gmat[2][4] =  sphi;
+  Gmat[0][4] = -ctheta * spsi + stheta * sphi * cpsi;
+  Gmat[1][4] =  cphi * cpsi;
+  Gmat[2][4] =  stheta * spsi + ctheta * sphi * cpsi;
 
   // dTz
-  Gmat[0][5] = cpsi * stheta + spsi * sphi * ctheta;
-  Gmat[1][5] = spsi * stheta - cpsi * sphi * ctheta;
-  Gmat[2][5] = cphi * ctheta;
+  Gmat[0][5] =  stheta * cphi;
+  Gmat[1][5] = -sphi;
+  Gmat[2][5] =  ctheta * cphi;
 }
 
 /** Compute effectiveness matrix for guidance
@@ -106,37 +106,37 @@ void guidance_indi_calcG(float Gmat[3][6], struct FloatEulers euler) {
   guidance_indi_calcG_yxz(Gmat, euler);
 }
 
-void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp UNUSED)
+void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp)
 {
   struct FloatEulers euler_yxz_ref = { 0 };
 #if GUIDANCE_INDI_RC_SWITCH_EULER
   euler_yxz_ref.phi = (radio_control_get(RADIO_PITCH) / MAX_PPRZ) * guidance_indi_max_bank;
   euler_yxz_ref.theta = (radio_control_get(RADIO_ROLL) / MAX_PPRZ) * guidance_indi_max_bank;
-  euler_yxz_ref.psi = heading_sp // TODO check this one
 #endif
+  euler_yxz_ref.psi = heading_sp;
 
   // Set lower limits
-  wls->u_min[0] = -guidance_indi_max_bank - euler_yxz->phi;          //phi
-  wls->u_min[1] = -guidance_indi_max_bank - euler_yxz->theta;        //theta
-  wls->u_min[2] = -M_PI - euler_yxz->psi;                            //psi FIXME M_PI or a lower bound ? (was 1 in initial code)
-  wls->u_min[3] = -guidance_indi_max_h_thrust - stab_thrust_filt.x;  //Tx
-  wls->u_min[4] = -guidance_indi_max_h_thrust - stab_thrust_filt.y;  //Ty
-  wls->u_min[5] = -guidance_indi_max_v_thrust - stab_thrust_filt.z;  //Tz (MAX_PPRZ  - stabilization.cmd[COMMAND_THRUST])
+  wls->u_min[0] = -guidance_indi_max_bank - euler_yxz->phi;          // phi
+  wls->u_min[1] = -guidance_indi_max_bank - euler_yxz->theta;        // theta
+  wls->u_min[2] = -M_PI - euler_yxz->psi;                            // psi
+  wls->u_min[3] = -guidance_indi_max_h_thrust - stab_thrust_filt.x;  // Tx
+  wls->u_min[4] = -guidance_indi_max_h_thrust - stab_thrust_filt.y;  // Ty
+  wls->u_min[5] = -guidance_indi_max_v_thrust - stab_thrust_filt.z;  // Tz
 
-  // ->r limits limits
-  wls->u_max[0] = guidance_indi_max_bank - euler_yxz->phi;          //phi
-  wls->u_max[1] = guidance_indi_max_bank - euler_yxz->theta;        //theta
-  wls->u_max[2] = M_PI - euler_yxz->psi;                            //psi
-  wls->u_max[3] = guidance_indi_max_h_thrust - stab_thrust_filt.x;  //Tx
-  wls->u_max[4] = guidance_indi_max_h_thrust - stab_thrust_filt.y;  //Ty
-  wls->u_max[5] = 9.81f * GUIDANCE_INDI_MASS - stab_thrust_filt.z;  //Tz
+  // Set lower limits limits
+  wls->u_max[0] = guidance_indi_max_bank - euler_yxz->phi;          // phi
+  wls->u_max[1] = guidance_indi_max_bank - euler_yxz->theta;        // theta
+  wls->u_max[2] = M_PI - euler_yxz->psi;                            // psi
+  wls->u_max[3] = guidance_indi_max_h_thrust - stab_thrust_filt.x;  // Tx
+  wls->u_max[4] = guidance_indi_max_h_thrust - stab_thrust_filt.y;  // Ty
+  wls->u_max[5] = 9.81f * GUIDANCE_INDI_MASS - stab_thrust_filt.z;  // Tz
 
-  // ->ered states
+  // Set prefered states
   wls->u_pref[0] = euler_yxz_ref.phi - euler_yxz->phi;     // prefered delta phi
   wls->u_pref[1] = euler_yxz_ref.theta - euler_yxz->theta; // prefered delta theta
-  wls->u_pref[2] = euler_yxz_ref.psi - euler_yxz->psi;     // prefered Ty
-  wls->u_pref[3] = stab_thrust_filt.x;                      // prefred Tx
-  wls->u_pref[4] = stab_thrust_filt.y;                      // prefered Ty
-  wls->u_pref[5] = stab_thrust_filt.z;                      // prefered Tz
+  wls->u_pref[2] = euler_yxz_ref.psi - euler_yxz->psi;     // prefered delta psi
+  wls->u_pref[3] = stab_thrust_filt.x;                     // prefered delta Tx
+  wls->u_pref[4] = stab_thrust_filt.y;                     // prefered delta Ty
+  wls->u_pref[5] = stab_thrust_filt.z;                     // prefered delta Tz
 }