From 2847ce20b8817511246bc5b88465448367df1ef4 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 24 Oct 2018 13:42:03 +0200
Subject: [PATCH] Auto traj - Add parameter for gain of trajectory controller
---
.../FlightTaskAutoLineSmoothVel.cpp | 8 +++-----
.../FlightTaskAutoLineSmoothVel.hpp | 4 +++-
.../mc_pos_control/mc_pos_control_params.c | 20 +++++++++++++++++++
3 files changed, 26 insertions(+), 6 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index 026ef7155b..3bb4ab4719 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -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))) {
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
index bd88398c8c..cea5b5560c 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
@@ -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. */
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 60cefbd8d0..599895ab46 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -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.
--
GitLab