[fix] Correct dimensions g1g2 matrix

paparazzi · Apr 18, 2017 · d9c6931 · d9c6931
1 parent 1e9bbc5
commit d9c6931
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -638,13 +638,13 @@ void calc_g1g2_pseudo_inv(void) {
   }
 
   //there are numerical errors if the scaling is not right.
-  float_vect_scale(g1g2_trans_mult[0], 100.0, INDI_OUTPUTS*INDI_NUM_ACT);
+  float_vect_scale(g1g2_trans_mult[0], 100.0, INDI_OUTPUTS*INDI_OUTPUTS);
 
   //inverse of 4x4 matrix
   float_mat_inv_4d(g1g2inv[0], g1g2_trans_mult[0]);
 
   //scale back
-  float_vect_scale(g1g2inv[0], 100.0, INDI_OUTPUTS*INDI_NUM_ACT);
+  float_vect_scale(g1g2inv[0], 100.0, INDI_OUTPUTS*INDI_OUTPUTS);
 
   //G1G2'*G1G2inv
   //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS