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

FlightTaskManual: Smooth flight integration: refactored and checked second half of get_acceleration_xy executing the intention

Browse Source
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 42708ea456
commit 6fb4c79234
1 changed files with 53 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+53 -53
View File
@@ -61,14 +61,18 @@ public:
_jerk_hor_max(parent, "MPC_JERK_MAX", false),
_jerk_hor_min(parent, "MPC_JERK_MIN", false),
_deceleration_hor_slow(parent, "MPC_DEC_HOR_SLOW", false),
_acceleration_hor_max(this, "ACC_HOR_MAX", true),
_acceleration_hor_manual(this, "ACC_HOR_MAN", true),
_acceleration_z_max_up(this, "ACC_UP_MAX", true),
_acceleration_z_max_down(this, "ACC_DOWN_MAX", true),
_acceleration_hor_max(this, "MPC_ACC_HOR_MAX", false),
_acceleration_hor_manual(this, "MPC_ACC_HOR_MAN", false),
_acceleration_z_max_up(this, "MPC_ACC_UP_MAX", false),
_acceleration_z_max_down(this, "MPC_ACC_DOWN_MAX", false),
_rc_flt_smp_rate(parent, "RC_FLT_SMP_RATE", false),
_rc_flt_cutoff(parent, "RC_FLT_CUTOFF", false),
_manual_direction_change_hysteresis(false),
_filter_manual_pitch(50.0f, 10.0f),
_filter_manual_roll(50.0f, 10.0f)
{};
_filter_roll_stick(50.0f, 10.0f),
_filter_pitch_stick(50.0f, 10.0f)
{
_manual_direction_change_hysteresis.set_hysteresis_time_from(false, DIRECTION_CHANGE_TIME_US);
};
virtual ~FlightTaskManual() {};
/**
@@ -78,6 +82,8 @@ public:
virtual int activate()
{
FlightTask::activate();
_filter_roll_stick.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get());
_filter_pitch_stick.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get());
_hold_position = matrix::Vector3f(NAN, NAN, NAN);
return 0;
};
@@ -178,14 +184,18 @@ private:
/* --- Acceleration Smoothing --- */
static constexpr uint64_t DIRECTION_CHANGE_TIME_US = 1e5; /** Time in us to switch into direction change state */
control::BlockParamFloat _jerk_hor_max; /**< maximum jerk only applied when breaking to zero */
control::BlockParamFloat _jerk_hor_min; /**< minimum jerk only applied when breaking to zero */
control::BlockParamFloat _jerk_hor_max; /**< maximum jerk only applied when braking to zero */
control::BlockParamFloat _jerk_hor_min; /**< minimum jerk only applied when braking to zero */
control::BlockParamFloat _deceleration_hor_slow; /**< slow velocity setpoint slewrate for manual deceleration*/
control::BlockParamFloat _acceleration_hor_max; /**< maximum velocity setpoint slewrate for auto & fast manual brake */
control::BlockParamFloat _acceleration_hor_manual; /**< maximum velocity setpoint slewrate for manual acceleration */
control::BlockParamFloat _acceleration_z_max_up; /**< max acceleration up */
control::BlockParamFloat _acceleration_z_max_down; /**< max acceleration down */
control::BlockParamFloat _rc_flt_smp_rate; /**< sample rate for stick lowpass filter */
control::BlockParamFloat _rc_flt_cutoff; /**< cutoff frequency for stick lowpass filter */
matrix::Vector2f _stick_input_xy_prev;
matrix::Vector3f _vel_sp_prev; /* velocity setpoint of last loop to calculate setpoint slewrate - acceleration */
enum stick_user_intention {
@@ -199,8 +209,8 @@ private:
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 */
systemlib::Hysteresis _manual_direction_change_hysteresis;
math::LowPassFilter2p _filter_manual_pitch;
math::LowPassFilter2p _filter_manual_roll;
math::LowPassFilter2p _filter_roll_stick;
math::LowPassFilter2p _filter_pitch_stick;
void vel_sp_slewrate(matrix::Vector3f &vel_sp, const matrix::Vector2f &stick_xy, const float &stick_z)
{
@@ -229,16 +239,23 @@ private:
_vel_sp_prev = vel_sp;
}
void reset_slewrate_xy()
{
_vel_sp_prev(0) = _velocity(0);
_vel_sp_prev(1) = _velocity(1);
}
float get_acceleration_xy(const matrix::Vector2f &stick_xy)
{
/*
* In manual mode we consider four states with different acceleration handling:
* 1. user wants to stop
* 2. user wants to quickly change direction
* 1. user wants to stop/brake (lets go of sticks)
* 2. user wants to quickly change direction (> 90 degree)
* 3. user wants to accelerate
* 4. user wants to decelerate
*/
float acceleration_state_dependent_xy = 0.f;
const matrix::Vector2f vel_xy(_velocity(0), _velocity(1));
/* check input stick for zero or direction */
const float stick_xy_norm = stick_xy.norm();
@@ -257,7 +274,7 @@ private:
stick_xy_prev_unit.normalize();
}
/* check if stick direction and current velocity are within 60 angle cos(60) = 0.5 */
/* check if stick direction changed more than 60 degree angle cos(60) = 0.5 */
const bool previous_stick_aligned = (stick_xy_unit * stick_xy_prev_unit) > 0.5f;
/* check acceleration */
@@ -295,37 +312,35 @@ private:
* execute the user intention
*/
/* we always want to break starting with slow deceleration */
/* pilot starts to have brake intention */
if ((_intention_xy_prev != brake) && (intention_xy == brake)) {
if (_jerk_hor_max.get() > _jerk_hor_min.get()) {
_manual_jerk_limit_xy = (_jerk_hor_max.get() - _jerk_hor_min.get()) / _velocity_hor_manual.get() *
sqrtf(_velocity(0) * _velocity(0) + _velocity(1) * _velocity(1)) + _jerk_hor_min.get();
/* allow jerk proportional to velocity */
const float jerk_range = _jerk_hor_max.get() - _jerk_hor_min.get();
const float velocity_ratio = vel_xy.norm() / _velocity_hor_manual.get();
_manual_jerk_limit_xy = _jerk_hor_min.get() + jerk_range * velocity_ratio;
/* we start braking with lowest accleration */
/* start braking with lowest deceleration */
acceleration_state_dependent_xy = _deceleration_hor_slow.get();
} else {
/* set the jerk limit large since we don't know it better*/
_manual_jerk_limit_xy = 1000000.f;
/* at brake we use max acceleration */
/* set the jerk limit large because it's disabled*/
_manual_jerk_limit_xy = 1e6f;
/* to brake we use max acceleration */
acceleration_state_dependent_xy = _acceleration_hor_max.get();
}
/* reset slew rate */
_vel_sp_prev(0) = _velocity(0);
_vel_sp_prev(1) = _velocity(1);
reset_slewrate_xy();
}
/* allowed state transitions */
switch (_intention_xy_prev) {
case brake: {
/* pilot does not want to brake anymore, initialize with lowest acceleration */
if (intention_xy != brake) {
_intention_xy_prev = acceleration;
/* we initialize with lowest acceleration */
acceleration_state_dependent_xy = _deceleration_hor_slow.get();
}
@@ -333,16 +348,10 @@ private:
}
case direction_change: {
/* only exit direction change if brake or aligned */
matrix::Vector2f vel_xy(_velocity(0), _velocity(1));
matrix::Vector2f vel_xy_norm = (vel_xy.length() > 0.0f) ? vel_xy.normalized() : vel_xy;
bool stick_vel_aligned = (vel_xy_norm * stick_xy_unit > 0.0f);
/* update manual direction change hysteresis */
/* only exit direction change if brake or desired heading aligned */
const bool stick_vel_aligned = (vel_xy * stick_xy_unit > 0.0f);
_manual_direction_change_hysteresis.set_state_and_update(!stick_vel_aligned);
/* exit direction change if one of the condition is met */
if (intention_xy == brake) {
_intention_xy_prev = intention_xy;
@@ -365,8 +374,7 @@ private:
_intention_xy_prev = intention_xy;
if (_intention_xy_prev == direction_change) {
_vel_sp_prev(0) = _velocity(0);
_vel_sp_prev(1) = _velocity(1);
reset_slewrate_xy();
}
break;
@@ -376,8 +384,7 @@ private:
_intention_xy_prev = intention_xy;
if (_intention_xy_prev == direction_change) {
_vel_sp_prev(0) = _velocity(0);
_vel_sp_prev(1) = _velocity(1);
reset_slewrate_xy();
}
break;
@@ -385,11 +392,11 @@ private:
}
/*
* apply acceleration based on state
* calculate dynamic acceleration based on state
*/
switch (_intention_xy_prev) {
case brake: {
/* limit jerk when braking to zero */
float jerk = (_acceleration_hor_max.get() - acceleration_state_dependent_xy) / _deltatime;
@@ -404,10 +411,9 @@ private:
}
case direction_change: {
/* limit acceleration linearly on stick input*/
acceleration_state_dependent_xy = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy_norm +
_deceleration_hor_slow.get();
const float acceleration_range = _acceleration_hor_manual.get() - _deceleration_hor_slow.get();
acceleration_state_dependent_xy = _deceleration_hor_slow.get() + acceleration_range * stick_xy_norm;
break;
}
@@ -429,16 +435,10 @@ private:
break;
}
default :
printf("User intention not recognized"); // TODO: take this out, can never happen
acceleration_state_dependent_xy = _acceleration_hor_max.get();
}
/* update previous stick input */
_stick_input_xy_prev = matrix::Vector2f(_filter_manual_pitch.apply(stick_xy(0)),
_filter_manual_roll.apply(stick_xy(1)));
_stick_input_xy_prev = matrix::Vector2f(_filter_pitch_stick.apply(stick_xy(0)), _filter_roll_stick.apply(stick_xy(1)));
if (_stick_input_xy_prev.length() > 1.0f) {
_stick_input_xy_prev = _stick_input_xy_prev.normalized();