dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

GotoControl: remove dependency on PositionControl 

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-27 15:01:27 +01:00
parent c853acc2ff
commit 5a2efc1cb2
2 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/mc_pos_control/GotoControl/CMakeLists.txt
View File

@ -36,4 +36,3 @@ px4_add_library(GotoControl
GotoControl.hpp
)
target_include_directories(GotoControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(GotoControl PUBLIC PositionControl)

16
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
View File

@ -36,7 +36,6 @@
*/
#include "GotoControl.hpp"
#include "PositionControl.hpp"
#include <drivers/drv_hrt.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,
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.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
@ -115,8 +115,6 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
_controlling_heading = true;
} else {
// TODO: error messaging for non-finite headings
trajectory_setpoint.yaw = 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)
{
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();
if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(_param_mpc_yawrauto_max.get())) {
max_heading_rate = math::constrain(_param_mpc_yawrauto_max.get(), 0.f,
_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(goto_setpoint.max_heading_rate, 0.f, _param_mpc_yawrauto_max.get());
// linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints
if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) {
const float rate_scale = max_heading_rate / _param_mpc_yawrauto_max.get();
max_heading_accel = math::constrain(_param_mpc_yawrauto_acc.get() * rate_scale,
0.f, _param_mpc_yawrauto_acc.get());
max_heading_accel = math::constrain(_param_mpc_yawrauto_acc.get() * rate_scale, 0.f, _param_mpc_yawrauto_acc.get());
}
}