mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
FlightTaskManual: Smooth flight integration: Copy over set_manual_acceleration_xy without any changes
This commit is contained in:
Matthias Grob
Beat Küng
parent
6c0e7654ed
commit
e6442c7a7c
@ -162,4 +162,214 @@ private:
|
||||
uORB::Subscription<control_state_s> _sub_control_state;
|
||||
|
||||
matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */
|
||||
|
||||
void set_manual_acceleration_xy(matrix::Vector2f &stick_xy, const float dt)
|
||||
{
|
||||
|
||||
/*
|
||||
* In manual mode we consider four states with different acceleration handling:
|
||||
* 1. user wants to stop
|
||||
* 2. user wants to quickly change direction
|
||||
* 3. user wants to accelerate
|
||||
* 4. user wants to decelerate
|
||||
*/
|
||||
|
||||
/* get normalized stick input vector */
|
||||
matrix::Vector2f stick_xy_norm = (stick_xy.length() > 0.0f) ? stick_xy.normalized() : stick_xy;
|
||||
matrix::Vector2f stick_xy_prev_norm = (_stick_input_xy_prev.length() > 0.0f) ? _stick_input_xy_prev.normalized() :
|
||||
_stick_input_xy_prev;
|
||||
|
||||
/* check if stick direction and current velocity are within 60angle */
|
||||
const bool is_aligned = (stick_xy_norm * stick_xy_prev_norm) > 0.5f;
|
||||
|
||||
/* check if zero input stick */
|
||||
const bool is_prev_zero = (fabsf(_stick_input_xy_prev.length()) <= FLT_EPSILON);
|
||||
const bool is_current_zero = (fabsf(stick_xy.length()) <= FLT_EPSILON);
|
||||
|
||||
/* check acceleration */
|
||||
const bool do_acceleration = is_prev_zero || (is_aligned &&
|
||||
((stick_xy.length() > _stick_input_xy_prev.length()) || (fabsf(stick_xy.length() - 1.0f) < FLT_EPSILON)));
|
||||
|
||||
const bool do_deceleration = (is_aligned && (stick_xy.length() <= _stick_input_xy_prev.length()));
|
||||
|
||||
const bool do_direction_change = !is_aligned;
|
||||
|
||||
manual_stick_input intention;
|
||||
|
||||
if (is_current_zero) {
|
||||
/* we want to stop */
|
||||
intention = brake;
|
||||
|
||||
} else if (do_acceleration) {
|
||||
/* we do manual acceleration */
|
||||
intention = acceleration;
|
||||
|
||||
} else if (do_deceleration) {
|
||||
/* we do manual deceleration */
|
||||
intention = deceleration;
|
||||
|
||||
} else if (do_direction_change) {
|
||||
/* we have a direction change */
|
||||
intention = direction_change;
|
||||
|
||||
} else {
|
||||
/* catchall: acceleration */
|
||||
intention = acceleration;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* update user intention
|
||||
*/
|
||||
|
||||
/* we always want to break starting with slow deceleration */
|
||||
if ((_user_intention_xy != brake) && (intention == 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(_vel(0) * _vel(0) + _vel(1) * _vel(1)) + _jerk_hor_min.get();
|
||||
|
||||
/* we start braking with lowest accleration */
|
||||
_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 */
|
||||
_acceleration_state_dependent_xy = _acceleration_hor_max.get();
|
||||
|
||||
}
|
||||
|
||||
/* reset slew rate */
|
||||
_vel_sp_prev(0) = _vel(0);
|
||||
_vel_sp_prev(1) = _vel(1);
|
||||
|
||||
}
|
||||
|
||||
switch (_user_intention_xy) {
|
||||
case brake: {
|
||||
if (intention != brake) {
|
||||
_user_intention_xy = acceleration;
|
||||
/* we initialize with lowest acceleration */
|
||||
_acceleration_state_dependent_xy = _deceleration_hor_slow.get();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case direction_change: {
|
||||
/* only exit direction change if brake or aligned */
|
||||
matrix::Vector2f vel_xy(_vel(0), _vel(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_norm > 0.0f);
|
||||
|
||||
/* update manual direction change hysteresis */
|
||||
_manual_direction_change_hysteresis.set_state_and_update(!stick_vel_aligned);
|
||||
|
||||
|
||||
/* exit direction change if one of the condition is met */
|
||||
if (intention == brake) {
|
||||
_user_intention_xy = intention;
|
||||
|
||||
} else if (stick_vel_aligned) {
|
||||
_user_intention_xy = acceleration;
|
||||
|
||||
} else if (_manual_direction_change_hysteresis.get_state()) {
|
||||
|
||||
/* TODO: find conditions which are always continuous
|
||||
* only if stick input is large*/
|
||||
if (stick_xy.length() > 0.6f) {
|
||||
_acceleration_state_dependent_xy = _acceleration_hor_max.get();
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case acceleration: {
|
||||
_user_intention_xy = intention;
|
||||
|
||||
if (_user_intention_xy == direction_change) {
|
||||
_vel_sp_prev(0) = _vel(0);
|
||||
_vel_sp_prev(1) = _vel(1);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case deceleration: {
|
||||
_user_intention_xy = intention;
|
||||
|
||||
if (_user_intention_xy == direction_change) {
|
||||
_vel_sp_prev(0) = _vel(0);
|
||||
_vel_sp_prev(1) = _vel(1);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* apply acceleration based on state
|
||||
*/
|
||||
switch (_user_intention_xy) {
|
||||
case brake: {
|
||||
|
||||
/* limit jerk when braking to zero */
|
||||
float jerk = (_acceleration_hor_max.get() - _acceleration_state_dependent_xy) / dt;
|
||||
|
||||
if (jerk > _manual_jerk_limit_xy) {
|
||||
_acceleration_state_dependent_xy = _manual_jerk_limit_xy * dt + _acceleration_state_dependent_xy;
|
||||
|
||||
} else {
|
||||
_acceleration_state_dependent_xy = _acceleration_hor_max.get();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case direction_change: {
|
||||
|
||||
/* limit acceleration linearly on stick input*/
|
||||
_acceleration_state_dependent_xy = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length() +
|
||||
_deceleration_hor_slow.get();
|
||||
break;
|
||||
}
|
||||
|
||||
case acceleration: {
|
||||
/* limit acceleration linearly on stick input*/
|
||||
float acc_limit = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length()
|
||||
+ _deceleration_hor_slow.get();
|
||||
|
||||
if (_acceleration_state_dependent_xy > acc_limit) {
|
||||
acc_limit = _acceleration_state_dependent_xy;
|
||||
}
|
||||
|
||||
_acceleration_state_dependent_xy = acc_limit;
|
||||
break;
|
||||
}
|
||||
|
||||
case deceleration: {
|
||||
_acceleration_state_dependent_xy = _deceleration_hor_slow.get();
|
||||
break;
|
||||
}
|
||||
|
||||
default :
|
||||
warn_rate_limited("User intention not recognized");
|
||||
_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)));
|
||||
|
||||
|
||||
if (_stick_input_xy_prev.length() > 1.0f) {
|
||||
_stick_input_xy_prev = _stick_input_xy_prev.normalized();
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user