fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: immediately switch out of goto setpoint when receiving trajectory_setpoint

Browse Source 
Previously, when switching from a goto setpoint into a mode that publishes
trajectory_setpoint, the previous goto setpoint was still used for 500ms,
which then caused a setpoint jump.
This change makes sure that when a trajectory_setpoint is received, any
existing goto setpoint is marked as invalid immediately.
This commit is contained in:
Beat Küng 2025-05-21 15:12:05 +02:00 committed by Silvan Fuhrer
parent f0ecd9e757
commit 80febaf0f0
3 changed files with 25 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -45,9 +45,16 @@ using namespace time_literals;
bool GotoControl::checkForSetpoint(const hrt_abstime &now, const bool enabled)
{
_goto_setpoint_sub.update();
const bool timestamp_initialized = _goto_setpoint_sub.get().timestamp != 0;
const bool no_timeout = now < (_goto_setpoint_sub.get().timestamp + 500_ms);
_goto_setpoint_sub.update(&_goto_setpoint);
if (!enabled) {
// Flag the setpoint as invalid if disabled, so if it is enabled in near future,
// we don't use an old setpoint
_goto_setpoint.timestamp = 0;
}
const bool timestamp_initialized = _goto_setpoint.timestamp != 0;
const bool no_timeout = now < (_goto_setpoint.timestamp + 500_ms);
const bool need_to_run = timestamp_initialized && no_timeout && enabled;
if (!need_to_run) {
@ -65,9 +72,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
_is_initialized = true;
}
const goto_setpoint_s &goto_setpoint = _goto_setpoint_sub.get();
const Vector3f position_setpoint(_goto_setpoint_sub.get().position);
const Vector3f position_setpoint(_goto_setpoint.position);
if (!position_setpoint.isAllFinite()) {
// TODO: error messaging
@ -85,7 +90,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
resetPositionSmoother(position);
}
setPositionSmootherLimits(goto_setpoint);
setPositionSmootherLimits(_goto_setpoint);
const Vector3f feedforward_velocity{};
const bool force_zero_velocity_setpoint = false;
@ -99,13 +104,13 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
out_setpoints.acceleration.copyTo(trajectory_setpoint.acceleration);
out_setpoints.jerk.copyTo(trajectory_setpoint.jerk);
if (goto_setpoint.flag_control_heading && PX4_ISFINITE(goto_setpoint.heading) && PX4_ISFINITE(heading)) {
if (_goto_setpoint.flag_control_heading && PX4_ISFINITE(_goto_setpoint.heading) && PX4_ISFINITE(heading)) {
if (!_controlling_heading || _need_smoother_reset) {
resetHeadingSmoother(heading);
}
setHeadingSmootherLimits(goto_setpoint);
_heading_smoothing.update(goto_setpoint.heading, dt);
setHeadingSmootherLimits(_goto_setpoint);
_heading_smoothing.update(_goto_setpoint.heading, dt);
trajectory_setpoint.yaw = _heading_smoothing.getSmoothedHeading();
trajectory_setpoint.yawspeed = _heading_smoothing.getSmoothedHeadingRate();
@ -121,11 +126,11 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
_need_smoother_reset = false;
trajectory_setpoint.timestamp = goto_setpoint.timestamp;
trajectory_setpoint.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(trajectory_setpoint);
vehicle_constraints_s vehicle_constraints{
.timestamp = goto_setpoint.timestamp,
.timestamp = trajectory_setpoint.timestamp,
.speed_up = NAN,
.speed_down = NAN,
.want_takeoff = false

3
src/modules/mc_pos_control/GotoControl/GotoControl.hpp
View File

@ -115,10 +115,11 @@ private:
*/
void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint);
uORB::SubscriptionData<goto_setpoint_s> _goto_setpoint_sub{ORB_ID(goto_setpoint)};
uORB::Subscription _goto_setpoint_sub{ORB_ID(goto_setpoint)};
uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
goto_setpoint_s _goto_setpoint{};
PositionSmoothing _position_smoothing;
HeadingSmoothing _heading_smoothing;

9
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -426,9 +426,12 @@ void MulticopterPositionControl::Run()
PositionControlStates states{set_vehicle_states(vehicle_local_position, dt)};
// if a goto setpoint available this publishes a trajectory setpoint to go there
if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample,
_vehicle_control_mode.flag_multicopter_position_control_enabled)) {
// If a goto setpoint is available this publishes a trajectory setpoint to go there
// If trajectory_setpoint is published elsewhere, do not use the goto setpoint
const bool goto_setpoint_enable = _vehicle_control_mode.flag_multicopter_position_control_enabled
&& !_trajectory_setpoint_sub.updated();
if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample, goto_setpoint_enable)) {
_goto_control.update(dt, states.position, states.yaw);
}