fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

GotoControl: remove dependency on PositionControl

Browse Source 
It was there only to have the empty trajectory setpoint defined.
I rather redefine it for the single use.
This commit is contained in:
Matthias Grob
2023-11-30 17:16:02 +01:00
parent c853acc2ff
commit 5a2efc1cb2
2 changed files with 6 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/GotoControl/CMakeLists.txt
-1
View File
@@ -36,4 +36,3 @@ px4_add_library(GotoControl
GotoControl.hpp GotoControl.hpp
) )
target_include_directories(GotoControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(GotoControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(GotoControl PUBLIC PositionControl)
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
+6 -10
View File
@@ -36,7 +36,6 @@
*/ */
#include "GotoControl.hpp" #include "GotoControl.hpp"
#include "PositionControl.hpp"
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <float.h> #include <float.h>
@@ -94,7 +93,8 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
_position_smoothing.generateSetpoints(position, position_setpoint, feedforward_velocity, dt, _position_smoothing.generateSetpoints(position, position_setpoint, feedforward_velocity, dt,
force_zero_velocity_setpoint, out_setpoints); force_zero_velocity_setpoint, out_setpoints);
trajectory_setpoint_s trajectory_setpoint = PositionControl::empty_trajectory_setpoint; const trajectory_setpoint_s empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
trajectory_setpoint_s trajectory_setpoint = empty_trajectory_setpoint;
_position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position); _position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position);
_position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity); _position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
@@ -115,8 +115,6 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
_controlling_heading = true; _controlling_heading = true;
} else { } else {
// TODO: error messaging for non-finite headings
trajectory_setpoint.yaw = NAN; trajectory_setpoint.yaw = NAN;
trajectory_setpoint.yawspeed = NAN; trajectory_setpoint.yawspeed = NAN;
@@ -224,19 +222,17 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint
void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint) void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint)
{ {
float max_heading_rate = _param_mpc_yawrauto_max.get(); float max_heading_rate = _param_mpc_yawrauto_max.get();
float max_heading_accel = _param_mpc_yawrauto_acc.get(); float max_heading_accel = _param_mpc_yawrauto_acc.get();
if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(_param_mpc_yawrauto_max.get())) { if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(goto_setpoint.max_heading_rate)) {
max_heading_rate = math::constrain(_param_mpc_yawrauto_max.get(), 0.f, max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, 0.f, _param_mpc_yawrauto_max.get());
_param_mpc_yawrauto_max.get());
// linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic // linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints // only limit acceleration once within velocity constraints
if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) { if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) {
const float rate_scale = max_heading_rate / _param_mpc_yawrauto_max.get(); const float rate_scale = max_heading_rate / _param_mpc_yawrauto_max.get();
max_heading_accel = math::constrain(_param_mpc_yawrauto_acc.get() * rate_scale, max_heading_accel = math::constrain(_param_mpc_yawrauto_acc.get() * rate_scale, 0.f, _param_mpc_yawrauto_acc.get());
0.f, _param_mpc_yawrauto_acc.get());
} }
} }