From d9c6931ebe42c2e2bb069bf703c3733c7b137061 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Fri, 14 Apr 2017 18:02:22 +0200 Subject: [PATCH] [fix] Correct dimensions g1g2 matrix --- .../firmwares/rotorcraft/stabilization/stabilization_indi.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 3ca275850ea..9a28c09e4d6 100644 --- 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