Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding parameter to scale velocity in flight task get current state #11969

Closed
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
Original file line number Diff line number Diff line change
@@ -428,11 +428,12 @@ 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;

1 change: 1 addition & 0 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
Original file line number Diff line number Diff line change
@@ -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<px4::params::MPC_WP_NAV_ACC>) _param_mpc_wp_nav_acc,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_CRUISE_90>) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
13 changes: 13 additions & 0 deletions src/modules/mc_pos_control/mc_pos_control_params.c
Original file line number Diff line number Diff line change
@@ -249,6 +249,19 @@ 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
*