diff --git a/src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp b/src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp index a39bd1fb0c..4ee48f9107 100644 --- a/src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp +++ b/src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp @@ -282,17 +282,6 @@ VehicleModelEstimator::Run() } } - // Compute required buffer size - if (!_armed || (now - _task_start) < 3300000 || _thrust_sp_buffer.get_length() < 2) { - int _thrust_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_thrust_sp) + 1; - - if (_thrust_sp_buffer.get_length() != _thrust_sp_buffer_length) { - if(!_thrust_sp_buffer.allocate(_thrust_sp_buffer_length)) { - PX4_ERR("Thrust setpoint buffer allocation failed!"); - } - } - } - // Apply low pass filter Vector3f lin_accel_filtered_new(_lpf_lin_accel.apply(Vector3f(acceleration.xyz))); @@ -332,6 +321,17 @@ VehicleModelEstimator::Run() } } + // Compute required buffer size + if (!_armed || (now - _task_start) < 3300000 || _thrust_sp_buffer.get_length() < 2) { + int _thrust_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_thrust_sp) + 1; + + if (_thrust_sp_buffer.get_length() != _thrust_sp_buffer_length) { + if(!_thrust_sp_buffer.allocate(_thrust_sp_buffer_length)) { + PX4_ERR("Thrust setpoint buffer allocation failed!"); + } + } + } + // Apply low pass filter Vector3f thrust_sp_filtered_new(_lpf_thrust_sp.apply(Vector3f(thrust_setpoint.xyz)));