fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 02:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManual: Smooth flight integration: prepare interface of acceleartion methods for xy and z

Browse Source
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 634a67d058
commit 56b54d7b85
1 changed files with 61 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+61 -51
View File
@@ -101,20 +101,20 @@ public:
FlightTask::update(); FlightTask::update();
/* prepare stick input */ /* prepare stick input */
matrix::Vector2f sick_xy; matrix::Vector2f stick_xy; /**< horizontal two dimensional stick input within a unit circle */
sick_xy(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get()); stick_xy(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get());
sick_xy(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get()); stick_xy(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get());
float stick_z = -math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get()); float stick_z = -math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get());
const float stick_xy_norm = sick_xy.norm(); const float stick_xy_norm = stick_xy.norm();
/* saturate such that magnitude in xy is never larger than 1 */ /* saturate such that magnitude in xy is never larger than 1 */
if (stick_xy_norm > 1.0f) { if (stick_xy_norm > 1.0f) {
sick_xy /= stick_xy_norm; stick_xy /= stick_xy_norm;
} }
/* rotate stick input to produce velocity setpoint in NED frame */ /* rotate stick input to produce velocity setpoint in NED frame */
matrix::Vector3f velocity_setpoint(sick_xy(0), sick_xy(1), stick_z); matrix::Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), stick_z);
velocity_setpoint = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, get_input_frame_yaw())) * velocity_setpoint; velocity_setpoint = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, get_input_frame_yaw())) * velocity_setpoint;
/* scale [0,1] length velocity vector to maximal manual speed (in m/s) */ /* scale [0,1] length velocity vector to maximal manual speed (in m/s) */
@@ -124,7 +124,7 @@ public:
velocity_setpoint = velocity_setpoint.emult(vel_scale); velocity_setpoint = velocity_setpoint.emult(vel_scale);
/* smooth out velocity setpoint by slewrate and return it */ /* smooth out velocity setpoint by slewrate and return it */
vel_sp_slewrate(velocity_setpoint); vel_sp_slewrate(velocity_setpoint, stick_xy, stick_z);
_set_velocity_setpoint(velocity_setpoint); _set_velocity_setpoint(velocity_setpoint);
/* handle position and altitude hold */ /* handle position and altitude hold */
@@ -187,7 +187,7 @@ private:
control::BlockParamFloat _acceleration_z_max_up; /**< max acceleration up */ control::BlockParamFloat _acceleration_z_max_up; /**< max acceleration up */
control::BlockParamFloat _acceleration_z_max_down; /**< max acceleration down */ control::BlockParamFloat _acceleration_z_max_down; /**< max acceleration down */
matrix::Vector2f _stick_input_xy_prev; matrix::Vector2f _stick_input_xy_prev;
matrix::Vector3f _vel_sp_prev; matrix::Vector3f _vel_sp_prev; /* velocity setpoint of last loop to calculate setpoint slewrate - acceleration */
enum manual_stick_input { enum manual_stick_input {
brake, brake,
direction_change, direction_change,
@@ -198,42 +198,39 @@ private:
manual_stick_input _user_intention_z = brake; /**< what the user wants to do derived from stick input in z direction */ manual_stick_input _user_intention_z = brake; /**< what the user wants to do derived from stick input in z direction */
float _manual_jerk_limit_xy = 1.f; /**< jerk limit in manual mode dependent on stick input */ float _manual_jerk_limit_xy = 1.f; /**< jerk limit in manual mode dependent on stick input */
float _manual_jerk_limit_z = 1.f; /**< jerk limit in manual mode in z */ float _manual_jerk_limit_z = 1.f; /**< jerk limit in manual mode in z */
float _acceleration_state_dependent_xy = 0.2f; /* acceleration limit applied in manual mode */
float _acceleration_state_dependent_z = 0.2f; /* acceleration limit applied in manual mode in z */
systemlib::Hysteresis _manual_direction_change_hysteresis; systemlib::Hysteresis _manual_direction_change_hysteresis;
math::LowPassFilter2p _filter_manual_pitch; math::LowPassFilter2p _filter_manual_pitch;
math::LowPassFilter2p _filter_manual_roll; math::LowPassFilter2p _filter_manual_roll;
void vel_sp_slewrate(matrix::Vector3f &vel_sp) void vel_sp_slewrate(matrix::Vector3f &vel_sp, const matrix::Vector2f &stick_xy, const float &stick_z)
{ {
matrix::Vector2f vel_sp_xy(vel_sp(0), vel_sp(1)); 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)); const matrix::Vector2f vel_sp_prev_xy(_vel_sp_prev(0), _vel_sp_prev(1));
matrix::Vector2f vel_xy(_velocity(0), _velocity(1)); const matrix::Vector2f vel_xy(_velocity(0), _velocity(1));
matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / _deltatime; const matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / _deltatime;
const float acc_xy_max = get_acceleration_xy(stick_xy);
/* limit total horizontal acceleration */ /* limit total horizontal acceleration */
if (acc_xy.length() > _acceleration_state_dependent_xy) { if (acc_xy.length() > acc_xy_max) {
vel_sp_xy = _acceleration_state_dependent_xy * acc_xy.normalized() * _deltatime + vel_sp_prev_xy; vel_sp_xy = acc_xy_max * acc_xy.normalized() * _deltatime + vel_sp_prev_xy;
vel_sp(0) = vel_sp_xy(0); vel_sp(0) = vel_sp_xy(0);
vel_sp(1) = vel_sp_xy(1); vel_sp(1) = vel_sp_xy(1);
} }
/* limit vertical acceleration */ /* limit vertical acceleration */
float acc_z = (vel_sp(2) - _vel_sp_prev(2)) / _deltatime; const float acc_z = (vel_sp(2) - _vel_sp_prev(2)) / _deltatime;
float max_acc_z; const float acc_z_max = math::sign(acc_z) * get_acceleration_z(stick_z);
max_acc_z = math::sign(acc_z) * _acceleration_state_dependent_z; if (fabsf(acc_z) > fabsf(acc_z_max)) {
vel_sp(2) = acc_z_max * _deltatime + _vel_sp_prev(2);
if (fabsf(acc_z) > fabsf(max_acc_z)) {
vel_sp(2) = max_acc_z * _deltatime + _vel_sp_prev(2);
} }
_vel_sp_prev = vel_sp; _vel_sp_prev = vel_sp;
} }
void set_manual_acceleration_xy(matrix::Vector2f &stick_xy) float get_acceleration_xy(const matrix::Vector2f &stick_xy)
{ {
/* /*
* In manual mode we consider four states with different acceleration handling: * In manual mode we consider four states with different acceleration handling:
* 1. user wants to stop * 1. user wants to stop
@@ -241,14 +238,23 @@ private:
* 3. user wants to accelerate * 3. user wants to accelerate
* 4. user wants to decelerate * 4. user wants to decelerate
*/ */
float acceleration_state_dependent_xy = 0.f;
/* get normalized stick input vector */ /* get normalized stick input vector */
matrix::Vector2f stick_xy_norm = (stick_xy.length() > 0.0f) ? stick_xy.normalized() : stick_xy; matrix::Vector2f stick_xy_unit = stick_xy;
matrix::Vector2f stick_xy_prev_norm = (_stick_input_xy_prev.length() > 0.0f) ? _stick_input_xy_prev.normalized() :
_stick_input_xy_prev; if (stick_xy_unit.length() > FLT_EPSILON) {
stick_xy_unit.normalize();
}
matrix::Vector2f stick_xy_prev_unit = _stick_input_xy_prev;
if (stick_xy_prev_unit.length() > FLT_EPSILON) {
stick_xy_prev_unit.normalize();
}
/* check if stick direction and current velocity are within 60angle */ /* check if stick direction and current velocity are within 60angle */
const bool is_aligned = (stick_xy_norm * stick_xy_prev_norm) > 0.5f; const bool is_aligned = (stick_xy_unit * stick_xy_prev_unit) > 0.5f;
/* check if zero input stick */ /* check if zero input stick */
const bool is_prev_zero = (fabsf(_stick_input_xy_prev.length()) <= FLT_EPSILON); const bool is_prev_zero = (fabsf(_stick_input_xy_prev.length()) <= FLT_EPSILON);
@@ -298,7 +304,7 @@ private:
sqrtf(_velocity(0) * _velocity(0) + _velocity(1) * _velocity(1)) + _jerk_hor_min.get(); sqrtf(_velocity(0) * _velocity(0) + _velocity(1) * _velocity(1)) + _jerk_hor_min.get();
/* we start braking with lowest accleration */ /* we start braking with lowest accleration */
_acceleration_state_dependent_xy = _deceleration_hor_slow.get(); acceleration_state_dependent_xy = _deceleration_hor_slow.get();
} else { } else {
@@ -306,7 +312,7 @@ private:
_manual_jerk_limit_xy = 1000000.f; _manual_jerk_limit_xy = 1000000.f;
/* at brake we use max acceleration */ /* at brake we use max acceleration */
_acceleration_state_dependent_xy = _acceleration_hor_max.get(); acceleration_state_dependent_xy = _acceleration_hor_max.get();
} }
@@ -321,7 +327,7 @@ private:
if (intention != brake) { if (intention != brake) {
_user_intention_xy = acceleration; _user_intention_xy = acceleration;
/* we initialize with lowest acceleration */ /* we initialize with lowest acceleration */
_acceleration_state_dependent_xy = _deceleration_hor_slow.get(); acceleration_state_dependent_xy = _deceleration_hor_slow.get();
} }
break; break;
@@ -331,7 +337,7 @@ private:
/* only exit direction change if brake or aligned */ /* only exit direction change if brake or aligned */
matrix::Vector2f vel_xy(_velocity(0), _velocity(1)); matrix::Vector2f vel_xy(_velocity(0), _velocity(1));
matrix::Vector2f vel_xy_norm = (vel_xy.length() > 0.0f) ? vel_xy.normalized() : vel_xy; matrix::Vector2f vel_xy_norm = (vel_xy.length() > 0.0f) ? vel_xy.normalized() : vel_xy;
bool stick_vel_aligned = (vel_xy_norm * stick_xy_norm > 0.0f); bool stick_vel_aligned = (vel_xy_norm * stick_xy_unit > 0.0f);
/* update manual direction change hysteresis */ /* update manual direction change hysteresis */
_manual_direction_change_hysteresis.set_state_and_update(!stick_vel_aligned); _manual_direction_change_hysteresis.set_state_and_update(!stick_vel_aligned);
@@ -349,7 +355,7 @@ private:
/* TODO: find conditions which are always continuous /* TODO: find conditions which are always continuous
* only if stick input is large*/ * only if stick input is large*/
if (stick_xy.length() > 0.6f) { if (stick_xy.length() > 0.6f) {
_acceleration_state_dependent_xy = _acceleration_hor_max.get(); acceleration_state_dependent_xy = _acceleration_hor_max.get();
} }
} }
@@ -386,13 +392,13 @@ private:
case brake: { case brake: {
/* limit jerk when braking to zero */ /* limit jerk when braking to zero */
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 + acceleration_state_dependent_xy;
} else { } else {
_acceleration_state_dependent_xy = _acceleration_hor_max.get(); acceleration_state_dependent_xy = _acceleration_hor_max.get();
} }
break; break;
@@ -401,8 +407,8 @@ private:
case direction_change: { case direction_change: {
/* limit acceleration linearly on stick input*/ /* limit acceleration linearly on stick input*/
_acceleration_state_dependent_xy = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length() + acceleration_state_dependent_xy = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length() +
_deceleration_hor_slow.get(); _deceleration_hor_slow.get();
break; break;
} }
@@ -411,22 +417,22 @@ private:
float acc_limit = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length() float acc_limit = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length()
+ _deceleration_hor_slow.get(); + _deceleration_hor_slow.get();
if (_acceleration_state_dependent_xy > acc_limit) { if (acceleration_state_dependent_xy > acc_limit) {
acc_limit = _acceleration_state_dependent_xy; acc_limit = acceleration_state_dependent_xy;
} }
_acceleration_state_dependent_xy = acc_limit; acceleration_state_dependent_xy = acc_limit;
break; break;
} }
case deceleration: { case deceleration: {
_acceleration_state_dependent_xy = _deceleration_hor_slow.get(); acceleration_state_dependent_xy = _deceleration_hor_slow.get();
break; break;
} }
default : default :
printf("User intention not recognized"); // TODO: take this out, can never happen printf("User intention not recognized"); // TODO: take this out, can never happen
_acceleration_state_dependent_xy = _acceleration_hor_max.get(); acceleration_state_dependent_xy = _acceleration_hor_max.get();
} }
@@ -438,15 +444,17 @@ private:
if (_stick_input_xy_prev.length() > 1.0f) { if (_stick_input_xy_prev.length() > 1.0f) {
_stick_input_xy_prev = _stick_input_xy_prev.normalized(); _stick_input_xy_prev = _stick_input_xy_prev.normalized();
} }
return acceleration_state_dependent_xy;
} }
void set_manual_acceleration_z(float &max_acceleration, const float stick_z) float get_acceleration_z(const float &stick_z)
{ {
/* in manual altitude control apply acceleration limit based on stick input /* in manual altitude control apply acceleration limit based on stick input
* we consider two states * we consider two states
* 1.) brake * 1.) brake
* 2.) accelerate */ * 2.) accelerate */
float acceleration_state_dependent_z = 0.f;
/* check if zero input stick */ /* check if zero input stick */
const bool is_current_zero = (fabsf(stick_z) <= FLT_EPSILON); const bool is_current_zero = (fabsf(stick_z) <= FLT_EPSILON);
@@ -460,7 +468,7 @@ private:
} }
/* get max and min acceleration where min acceleration is just 1/5 of max acceleration */ /* get max and min acceleration where min acceleration is just 1/5 of max acceleration */
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
@@ -468,7 +476,7 @@ private:
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();
/* reset slew rate */ /* reset slew rate */
_vel_sp_prev(2) = _velocity(2); _vel_sp_prev(2) = _velocity(2);
@@ -483,20 +491,22 @@ private:
if (_user_intention_z == brake) { if (_user_intention_z == brake) {
/* limit jerk when braking to zero */ /* limit jerk when braking to zero */
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 + acceleration_state_dependent_z;
} else { } else {
_acceleration_state_dependent_z = _acceleration_z_max_up.get(); acceleration_state_dependent_z = _acceleration_z_max_up.get();
} }
} }
if (_user_intention_z == acceleration) { if (_user_intention_z == acceleration) {
_acceleration_state_dependent_z = (max_acceleration - _acceleration_z_max_down.get()) * fabsf( acceleration_state_dependent_z = (max_acceleration - _acceleration_z_max_down.get()) * fabsf(
stick_z) + _acceleration_z_max_down.get(); stick_z) + _acceleration_z_max_down.get();
} }
return acceleration_state_dependent_z;
} }
}; };