From f1f43d47f9f2d34a5cf6208a8c444e5d4f207efc Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Wed, 10 May 2017 21:46:50 +0200 Subject: [PATCH] better initialization guidance_indi (#2063) --- .../firmwares/rotorcraft/guidance/guidance_indi.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c index 051c3e5ff4..698d8a83a1 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c @@ -107,17 +107,17 @@ static void guidance_indi_calcG(struct FloatMat33 *Gmat); * Call upon entering indi guidance */ void guidance_indi_enter(void) { - thrust_in = 0.0; - thrust_act = 0; + thrust_in = stabilization_cmd[COMMAND_THRUST]; + thrust_act = thrust_in; float tau = 1.0/(2.0*M_PI*filter_cutoff); float sample_time = 1.0/PERIODIC_FREQUENCY; for(int8_t i=0; i<3; i++) { init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0); } - init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0); - init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0); - init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0); + init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi); + init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, stateGetNedToBodyEulers_f()->theta); + init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in); } /**