Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

Fixup vehicle_model

Browse Source
This commit is contained in:
Julien Lecoeur
2019-11-25 19:19:09 +01:00
parent f8c2b0d661
commit 1bb0bf95d3
1 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp
+11 -11
View File
@@ -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 // Apply low pass filter
Vector3f lin_accel_filtered_new(_lpf_lin_accel.apply(Vector3f(acceleration.xyz))); 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 // Apply low pass filter
Vector3f thrust_sp_filtered_new(_lpf_thrust_sp.apply(Vector3f(thrust_setpoint.xyz))); Vector3f thrust_sp_filtered_new(_lpf_thrust_sp.apply(Vector3f(thrust_setpoint.xyz)));