fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 10:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManual: Smooth flight integration: Copy over vel_sp_slewrate without any changes

Browse Source
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 5854fa06e9
commit 1b858f5e56
1 changed files with 31 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+31
View File
@@ -202,6 +202,37 @@ private:
math::LowPassFilter2p _filter_manual_pitch;
math::LowPassFilter2p _filter_manual_roll;
void vel_sp_slewrate(float dt)
{
matrix::Vector2f vel_sp_xy(_vel_sp(0), _vel_sp(1));
matrix::Vector2f vel_sp_prev_xy(_vel_sp_prev(0), _vel_sp_prev(1));
matrix::Vector2f vel_xy(_vel(0), _vel(1));
matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / dt;
/* limit total horizontal acceleration */
if (acc_xy.length() > _acceleration_state_dependent_xy) {
vel_sp_xy = _acceleration_state_dependent_xy * acc_xy.normalized() * dt + vel_sp_prev_xy;
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
}
/* limit vertical acceleration */
float acc_z = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
float max_acc_z;
if (_control_mode.flag_control_manual_enabled) {
max_acc_z = (acc_z < 0.0f) ? -_acceleration_state_dependent_z : _acceleration_state_dependent_z;
} else {
max_acc_z = (acc_z < 0.0f) ? -_acceleration_z_max_up.get() : _acceleration_z_max_down.get();
}
if (fabsf(acc_z) > fabsf(max_acc_z)) {
_vel_sp(2) = max_acc_z * dt + _vel_sp_prev(2);
}
}
void set_manual_acceleration_xy(matrix::Vector2f &stick_xy, const float dt)
{