dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixup vehicle_model

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp
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)));