From 5dc74f07ccb247e4bb07c32f920ed3e73d2ec702 Mon Sep 17 00:00:00 2001 From: Federico Usai Date: Thu, 2 May 2019 18:57:02 +0200 Subject: [PATCH 1/6] adds a parameter that scales the velocity in _getCurrentState in FlightTaskAuto for offtrack and previous_in_front --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 4 ++-- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 1 + src/modules/mc_pos_control/mc_pos_control_params.c | 14 ++++++++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 8e08a5199018..a9f614ddeac6 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -428,11 +428,11 @@ State FlightTaskAuto::_getCurrentState() // Target is behind. return_state = State::target_behind; - } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) { + } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { // Current position is more than cruise speed in front of previous setpoint. return_state = State::previous_infront; - } else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) { + } else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { // Vehicle is more than cruise speed off track. return_state = State::offtrack; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index d68c26571a09..56dd849cd3e5 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -107,6 +107,7 @@ class FlightTaskAuto : public FlightTask ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_wp_nav_acc, (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line (ParamFloat) 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 746f930964fb..5192fc4729d9 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,20 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); */ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); +/** + * Navigator waypoint acceptance threshold + * + * Threshold for the internal waypoint update in + * FLightTaskAuto state machine. + * + * @min 1.0 + * @max 1000.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_WP_NAV_ACC, 1.0f); + /** * Proportional gain for horizontal trajectory position error * From 77999ef31eac7732621aea215ed66d6967e2f042 Mon Sep 17 00:00:00 2001 From: Federico Usai Date: Sun, 5 May 2019 12:00:33 +0200 Subject: [PATCH 2/6] fixes lines length requirements --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index a9f614ddeac6..f83049a179de 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -428,7 +428,8 @@ State FlightTaskAuto::_getCurrentState() // Target is behind. return_state = State::target_behind; - } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { + } else if (u_prev_to_target * prev_to_pos < 0.0f + && prev_to_pos.length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { // Current position is more than cruise speed in front of previous setpoint. return_state = State::previous_infront; From 1123edbabd5dbcb24cdc9bf93e80e20f23c1927f Mon Sep 17 00:00:00 2001 From: Federico Usai Date: Sun, 5 May 2019 22:12:59 +0200 Subject: [PATCH 3/6] resolves style errors --- src/modules/mc_pos_control/mc_pos_control_params.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 5192fc4729d9..f06a997013b1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -252,9 +252,8 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); /** * Navigator waypoint acceptance threshold * - * Threshold for the internal waypoint update in - * FLightTaskAuto state machine. - * + * Threshold for the internal waypoint update in FLightTaskAuto state machine. + * * @min 1.0 * @max 1000.0 * @increment 1 From 629c94061bc1170236132649729cf895dcea77e6 Mon Sep 17 00:00:00 2001 From: Federico Usai Date: Thu, 2 May 2019 18:57:02 +0200 Subject: [PATCH 4/6] adds a parameter that scales the velocity in _getCurrentState in FlightTaskAuto for offtrack and previous_in_front --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 4 ++-- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 1 + src/modules/mc_pos_control/mc_pos_control_params.c | 14 ++++++++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 8e08a5199018..a9f614ddeac6 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -428,11 +428,11 @@ State FlightTaskAuto::_getCurrentState() // Target is behind. return_state = State::target_behind; - } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) { + } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { // Current position is more than cruise speed in front of previous setpoint. return_state = State::previous_infront; - } else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) { + } else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { // Vehicle is more than cruise speed off track. return_state = State::offtrack; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index d68c26571a09..56dd849cd3e5 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -107,6 +107,7 @@ class FlightTaskAuto : public FlightTask ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_wp_nav_acc, (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line (ParamFloat) 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 746f930964fb..5192fc4729d9 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,20 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); */ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); +/** + * Navigator waypoint acceptance threshold + * + * Threshold for the internal waypoint update in + * FLightTaskAuto state machine. + * + * @min 1.0 + * @max 1000.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_WP_NAV_ACC, 1.0f); + /** * Proportional gain for horizontal trajectory position error * From 773cb1997060602ef830e964602ec762297b2a6f Mon Sep 17 00:00:00 2001 From: Federico Usai Date: Sun, 5 May 2019 12:00:33 +0200 Subject: [PATCH 5/6] fixes lines length requirements --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index a9f614ddeac6..f83049a179de 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -428,7 +428,8 @@ State FlightTaskAuto::_getCurrentState() // Target is behind. return_state = State::target_behind; - } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { + } else if (u_prev_to_target * prev_to_pos < 0.0f + && prev_to_pos.length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) { // Current position is more than cruise speed in front of previous setpoint. return_state = State::previous_infront; From 2e47fccdfbb677c9684193bc36891478c915916b Mon Sep 17 00:00:00 2001 From: Federico Usai Date: Sun, 5 May 2019 22:12:59 +0200 Subject: [PATCH 6/6] resolves style errors --- src/modules/mc_pos_control/mc_pos_control_params.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 5192fc4729d9..f06a997013b1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -252,9 +252,8 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); /** * Navigator waypoint acceptance threshold * - * Threshold for the internal waypoint update in - * FLightTaskAuto state machine. - * + * Threshold for the internal waypoint update in FLightTaskAuto state machine. + * * @min 1.0 * @max 1000.0 * @increment 1