dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MPC auto - Add MPC_YAW_MODE: towards waypoint (yaw first) mode. This mode ensures that 

the vehicle yaws towards the next waypoint before accelerating. This is
required for drones with front vision and aerodynamic multicopters such
as standard vtol planes or highspeed multirotors.
This commit is contained in:
bresch 2019-06-27 14:07:48 +02:00 committed by Mathieu Bresciani
parent 2ed00c9cb6
commit 15ec73629b
6 changed files with 74 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -118,6 +118,8 @@ void FlightTaskAuto::_limitYawRate()
{
const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get());
_yaw_sp_aligned = true;
if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) {
// Limit the rate of change of the yaw setpoint
const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
@ -126,10 +128,16 @@ void FlightTaskAuto::_limitYawRate()
_yaw_setpoint = _yaw_sp_prev + dyaw;
_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint);
_yaw_sp_prev = _yaw_setpoint;
// The yaw setpoint is aligned when its rate is not saturated
_yaw_sp_aligned = fabsf(dyaw_desired) < fabsf(dyaw_max);
}
if (PX4_ISFINITE(_yawspeed_setpoint)) {
_yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
// The yaw setpoint is aligned when its rate is not saturated
_yaw_sp_aligned = fabsf(_yawspeed_setpoint) < yawrate_max;
}
}
@ -300,6 +308,7 @@ void FlightTaskAuto::_set_heading_from_mode()
switch (_param_mpc_yaw_mode.get()) {
case 0: // Heading points towards the current waypoint.
case 4: // Same as 0 but yaw fisrt and then go
v = Vector2f(_target) - Vector2f(_position);
break;

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

@ -108,6 +108,7 @@ protected:
int _mission_gear = landing_gear_s::GEAR_KEEP;
float _yaw_sp_prev = NAN;
bool _yaw_sp_aligned{false};
ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */

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

@ -49,8 +49,19 @@ void FlightTaskAutoLine::_generateSetpoints()
_generateHeadingAlongTrack();
}
_generateAltitudeSetpoints();
_generateXYsetpoints();
if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
// Wait for the yaw setpoint to be aligned
if (!_position_locked) {
_velocity_setpoint.setAll(0.f);
_position_setpoint = _position;
_position_locked = true;
}
} else {
_position_locked = false;
_generateAltitudeSetpoints();
_generateXYsetpoints();
}
}
void FlightTaskAutoLine::_setSpeedAtTarget()

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

@ -66,4 +66,5 @@ protected:
private:
void _setSpeedAtTarget(); /**< Sets desiered speed at target */
float _speed_at_target = 0.0f;
bool _position_locked{false};
};

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

@ -145,61 +145,67 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
_checkEkfResetCounters();
_want_takeoff = false;
if (PX4_ISFINITE(_position_setpoint(0)) &&
PX4_ISFINITE(_position_setpoint(1))) {
// Use position setpoints to generate velocity setpoints
if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
// Wait for the yaw setpoint to be aligned
_velocity_setpoint.setAll(0.f);
// Get various path specific vectors. */
Vector2f pos_traj;
pos_traj(0) = _trajectory[0].getCurrentPosition();
pos_traj(1) = _trajectory[1].getCurrentPosition();
Vector2f pos_sp_xy(_position_setpoint);
Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
} else {
if (PX4_ISFINITE(_position_setpoint(0)) &&
PX4_ISFINITE(_position_setpoint(1))) {
// Use position setpoints to generate velocity setpoints
// Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk.
// We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration)
// Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
// To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter
float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get();
float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length();
float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c));
float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get());
speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
// Get various path specific vectors. */
Vector2f pos_traj;
pos_traj(0) = _trajectory[0].getCurrentPosition();
pos_traj(1) = _trajectory[1].getCurrentPosition();
Vector2f pos_sp_xy(_position_setpoint);
Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
// Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk.
// We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration)
// Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
// To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter
float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get();
float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length();
float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c));
float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get());
speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
for (int i = 0; i < 2; i++) {
// If available, constrain the velocity using _velocity_setpoint(.)
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i));
Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
} else {
_velocity_setpoint(i) = vel_sp_xy(i);
for (int i = 0; i < 2; i++) {
// If available, constrain the velocity using _velocity_setpoint(.)
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i));
} else {
_velocity_setpoint(i) = vel_sp_xy(i);
}
_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
_param_mpc_xy_traj_p.get(); // Along-track setpoint + cross-track P controller
}
_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
_param_mpc_xy_traj_p.get(); // Along-track setpoint + cross-track P controller
}
}
if (PX4_ISFINITE(_position_setpoint(2))) {
const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
_param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
if (PX4_ISFINITE(_position_setpoint(2))) {
const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
_param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
// If available, constrain the velocity using _velocity_setpoint(.)
if (PX4_ISFINITE(_velocity_setpoint(2))) {
_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
// If available, constrain the velocity using _velocity_setpoint(.)
if (PX4_ISFINITE(_velocity_setpoint(2))) {
_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
} else {
_velocity_setpoint(2) = vel_sp_z;
}
} else {
_velocity_setpoint(2) = vel_sp_z;
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
}

3
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -762,11 +762,12 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
* Specifies the heading in Auto.
*
* @min 0
* @max 3
* @max 4
* @value 0 towards waypoint
* @value 1 towards home
* @value 2 away from home
* @value 3 along trajectory
* @value 4 towards waypoint (yaw first)
* @group Mission
*/
PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);