Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Auto traj - Add parameter for gain of trajectory controller

Browse Source
This commit is contained in:
bresch
2018-10-24 13:42:03 +02:00
committed by Roman Bapst
parent 2c63388fb7
commit 2847ce20b8
3 changed files with 26 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+3 -5
View File
@@ -119,9 +119,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
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());
float speed_sp_track = _mc_cruise_speed;
speed_sp_track = Vector2f(pos_traj_to_dest).length() * 0.3f;
float speed_sp_track = Vector2f(pos_traj_to_dest).length() * MPC_XY_TRAJ_P.get();
speed_sp_track = math::constrain(speed_sp_track, 0.0f, MPC_XY_CRUISE.get());
Vector2f velocity_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
@@ -135,7 +133,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
}
_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
0.3f; // Along-track setpoint + cross-track P controller
MPC_XY_TRAJ_P.get(); // Along-track setpoint + cross-track P controller
}
} else if (!PX4_ISFINITE(_velocity_setpoint(0)) &&
@@ -148,7 +146,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
if (PX4_ISFINITE(_position_setpoint(2))) {
const float velocity_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
0.3f; // Generate a velocity target for the trajectory using a simple P loop
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))) {
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+3 -1
View File
@@ -60,7 +60,9 @@ protected:
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
(ParamFloat<px4::params::MPC_JERK_MIN>) MPC_JERK_MIN
(ParamFloat<px4::params::MPC_JERK_MIN>) MPC_JERK_MIN,
(ParamFloat<px4::params::MPC_XY_TRAJ_P>) MPC_XY_TRAJ_P,
(ParamFloat<px4::params::MPC_Z_TRAJ_P>) MPC_Z_TRAJ_P
);
void _generateSetpoints() override; /**< Generate setpoints along line. */
src/modules/mc_pos_control/mc_pos_control_params.c
+20
View File
@@ -249,6 +249,26 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
*/
PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
/**
* Proportional gain for horizontal trajectory position error
*
* @min 0.1
* @max 5.0
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.3f);
/**
* Proportional gain for vertical trajectory position error
*
* @min 0.1
* @max 5.0
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_TRAJ_P, 0.3f);
/**
* Cruise speed when angle prev-current/current-next setpoint
* is 90 degrees. It should be lower than MPC_XY_CRUISE.