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

FlightTaskManual: Smooth flight integration: refactored and checked get_acceleration_z

Browse Source
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 6fb4c79234
commit 76a0b9f736
1 changed files with 7 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+7 -11
View File
@@ -401,7 +401,7 @@ private:
float jerk = (_acceleration_hor_max.get() - acceleration_state_dependent_xy) / _deltatime; float jerk = (_acceleration_hor_max.get() - acceleration_state_dependent_xy) / _deltatime;
if (jerk > _manual_jerk_limit_xy) { if (jerk > _manual_jerk_limit_xy) {
acceleration_state_dependent_xy = _manual_jerk_limit_xy * _deltatime + acceleration_state_dependent_xy; acceleration_state_dependent_xy += _manual_jerk_limit_xy * _deltatime;
} else { } else {
acceleration_state_dependent_xy = _acceleration_hor_max.get(); acceleration_state_dependent_xy = _acceleration_hor_max.get();
@@ -455,24 +455,20 @@ private:
* 2.) accelerate */ * 2.) accelerate */
float acceleration_state_dependent_z = 0.f; float acceleration_state_dependent_z = 0.f;
/* check if zero input stick */
const bool is_current_zero = (fabsf(stick_z) <= FLT_EPSILON);
/* default is acceleration */ /* default is acceleration */
stick_user_intention intention = acceleration; stick_user_intention intention = acceleration;
/* check zero input stick */ if (fabsf(stick_z) <= FLT_EPSILON) {
if (is_current_zero) {
intention = brake; intention = brake;
} }
/* get max and min acceleration where min acceleration is just 1/5 of max acceleration */ /* set maximum acceleration depending on upwards or downwards flight */
float max_acceleration = (stick_z <= 0.0f) ? _acceleration_z_max_up.get() : _acceleration_z_max_down.get(); float max_acceleration = (stick_z <= 0.0f) ? _acceleration_z_max_up.get() : _acceleration_z_max_down.get();
/* /*
* update user input * update user input
*/ */
if ((_user_intention_z != brake) && (intention == brake)) { if ((_user_intention_z != brake) && (intention == brake)) {
/* we start with lowest acceleration */ /* we start with lowest acceleration */
acceleration_state_dependent_z = _acceleration_z_max_down.get(); acceleration_state_dependent_z = _acceleration_z_max_down.get();
@@ -493,7 +489,7 @@ private:
float jerk = (_acceleration_z_max_up.get() - acceleration_state_dependent_z) / _deltatime; float jerk = (_acceleration_z_max_up.get() - acceleration_state_dependent_z) / _deltatime;
if (jerk > _manual_jerk_limit_z) { if (jerk > _manual_jerk_limit_z) {
acceleration_state_dependent_z = _manual_jerk_limit_z * _deltatime + acceleration_state_dependent_z; acceleration_state_dependent_z += _manual_jerk_limit_z * _deltatime;
} else { } else {
acceleration_state_dependent_z = _acceleration_z_max_up.get(); acceleration_state_dependent_z = _acceleration_z_max_up.get();
@@ -501,8 +497,8 @@ private:
} }
if (_user_intention_z == acceleration) { if (_user_intention_z == acceleration) {
acceleration_state_dependent_z = (max_acceleration - _acceleration_z_max_down.get()) * fabsf( const float acceleration_range = max_acceleration - _acceleration_z_max_down.get();
stick_z) + _acceleration_z_max_down.get(); acceleration_state_dependent_z = _acceleration_z_max_down.get() + acceleration_range * fabsf(stick_z);
} }
return acceleration_state_dependent_z; return acceleration_state_dependent_z;