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

MC auto - Modify yaw_sp_aligned to take mis_yaw_error tolerance.

Browse Source 
Add check for this flag in AutoLine
This commit is contained in:
bresch 2019-07-19 14:31:19 +02:00 committed by Mathieu Bresciani
parent 9e9b2ab9e8
commit de10f1e04d
5 changed files with 14 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -125,12 +125,14 @@ void FlightTaskAuto::_limitYawRate()
const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
const float dyaw_max = yawrate_max * _deltatime;
const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max);
_yaw_setpoint = _yaw_sp_prev + dyaw;
_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint);
_yaw_sp_prev = _yaw_setpoint;
float yaw_setpoint_sat = _yaw_sp_prev + dyaw;
yaw_setpoint_sat = matrix::wrap_pi(yaw_setpoint_sat);
// The yaw setpoint is aligned when its rate is not saturated
_yaw_sp_aligned = fabsf(dyaw_desired) < fabsf(dyaw_max);
// The yaw setpoint is aligned when it is within tolerance
_yaw_sp_aligned = fabsf(_yaw_setpoint - yaw_setpoint_sat) < math::radians(_param_mis_yaw_err.get());
_yaw_setpoint = yaw_setpoint_sat;
_yaw_sp_prev = _yaw_setpoint;
}
if (PX4_ISFINITE(_yawspeed_setpoint)) {

3
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
View File

@ -119,7 +119,8 @@ protected:
_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err // yaw-error threshold
);
private:

8
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
View File

@ -175,14 +175,8 @@ void FlightTaskAutoLine::_generateXYsetpoints()
// Vehicle is still far from destination. Accelerate or keep maximum target speed.
float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime;
float yaw_diff = 0.0f;
if (PX4_ISFINITE(_yaw_setpoint)) {
yaw_diff = wrap_pi(_yaw_setpoint - _yaw);
}
// If yaw offset is large, only accelerate with 0.5 m/s^2.
float acc_max = (fabsf(yaw_diff) > math::radians(_param_mis_yaw_err.get())) ? 0.5f : _param_mpc_acc_hor.get();
float acc_max = (_yaw_sp_aligned) ? _param_mpc_acc_hor.get() : 0.5f;
if (acc_track > acc_max) {
// accelerate towards target

1
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
View File

@ -51,7 +51,6 @@ public:
protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max

6
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -191,10 +191,12 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
float speed_at_target = 0.0f;
const float distance_current_next = Vector2f(&(_target - _next_wp)(0)).length();
const bool waypoint_overlap = Vector2f(&(_target - _prev_wp)(0)).length() < _target_acceptance_radius;
const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned;
if (distance_current_next > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > _target_acceptance_radius) &&
_param_mpc_yaw_mode.get() != 4) {
!waypoint_overlap &&
yaw_align_check_pass) {
// Max speed between current and next
const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next);
const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() * Vector2f(&(_target - _next_wp)(