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

GotoControl: make interface over uORB

Browse Source
This commit is contained in:
Matthias Grob 2023-11-23 14:53:02 +01:00
parent 96a81c22e3
commit 3de6fee07f
5 changed files with 65 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/flight_mode_manager/FlightModeManager.hpp
View File

@ -50,6 +50,7 @@
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_constraints.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>

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

@ -38,15 +38,37 @@
#include "GotoControl.hpp"
#include "PositionControl.hpp"
#include <drivers/drv_hrt.h>
#include <float.h>
#include <lib/mathlib/mathlib.h>
void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading,
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint)
{
trajectory_setpoint = PositionControl::empty_trajectory_setpoint;
using namespace time_literals;
const Vector3f position_setpoint(goto_setpoint.position);
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);
const bool need_to_run = timestamp_initialized && no_timeout && enabled;
if (!need_to_run) {
_is_initialized = false;
}
return need_to_run;
}
void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading)
{
if (!_is_initialized) {
resetPositionSmoother(position);
resetHeadingSmoother(heading);
_is_initialized = true;
}
const goto_setpoint_s &goto_setpoint = _goto_setpoint_sub.get();
const Vector3f position_setpoint(_goto_setpoint_sub.get().position);
if (!position_setpoint.isAllFinite()) {
// TODO: error messaging
@ -72,6 +94,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;
_position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position);
_position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
_position_smoothing.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration);
@ -99,9 +123,18 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
_controlling_heading = false;
}
trajectory_setpoint.timestamp = goto_setpoint.timestamp;
_need_smoother_reset = false;
trajectory_setpoint.timestamp = goto_setpoint.timestamp;
_trajectory_setpoint_pub.publish(trajectory_setpoint);
vehicle_constraints_s vehicle_constraints{
.timestamp = goto_setpoint.timestamp,
.speed_up = NAN,
.speed_down = NAN,
.want_takeoff = false
};
_vehicle_constraints_pub.publish(vehicle_constraints);
}
void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)

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

@ -47,8 +47,12 @@
#include <mathlib/math/Limits.hpp>
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/goto_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
class GotoControl : public ModuleParams
{
@ -59,6 +63,8 @@ public:
/** @param error [m] position smoother's maximum allowed horizontal position error at which trajectory integration halts */
void setMaxAllowedHorizontalPositionError(const float error) { _position_smoothing.setMaxAllowedHorizontalError(error); }
bool checkForSetpoint(const hrt_abstime &now, const bool enabled);
/**
* @brief resets the position smoother at the current position with zero velocity and acceleration.
*
@ -83,10 +89,7 @@ public:
* @param[in] goto_setpoint struct containing current go-to setpoints
* @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
*/
void update(const float dt, const matrix::Vector3f &position, const float heading,
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint);
bool is_initialized{false};
void update(const float dt, const matrix::Vector3f &position, const float heading);
private:
void updateParams() override;
@ -105,9 +108,15 @@ private:
*/
void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint);
uORB::SubscriptionData<goto_setpoint_s> _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)};
PositionSmoothing _position_smoothing;
HeadingSmoothing _heading_smoothing;
bool _is_initialized{false}; ///< true if smoothers were reset to current state
// flags that the next update() requires a valid current vehicle position to reset the smoothers
bool _need_smoother_reset{true};

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

@ -370,37 +370,14 @@ void MulticopterPositionControl::Run()
PositionControlStates states{set_vehicle_states(vehicle_local_position)};
_trajectory_setpoint_sub.update(&_setpoint);
const hrt_abstime last_goto_timestamp = _goto_setpoint.timestamp;
_goto_setpoint_sub.update(&_goto_setpoint);
if ((_goto_setpoint.timestamp != 0)
&& (_goto_setpoint.timestamp >= _time_position_control_enabled)
&& (hrt_elapsed_time(&last_goto_timestamp) < 200_ms)
&& _vehicle_control_mode.flag_multicopter_position_control_enabled) {
// take goto setpoint as priority over trajectory setpoint
if (!_goto_control.is_initialized) {
_goto_control.resetPositionSmoother(states.position);
_goto_control.resetHeadingSmoother(states.yaw);
}
_goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint);
// for logging
_trajectory_setpoint_pub.publish(_setpoint);
_vehicle_constraints.timestamp = hrt_absolute_time();
_vehicle_constraints.want_takeoff = false;
_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
} else {
_goto_control.is_initialized = false;
_vehicle_constraints_sub.update(&_vehicle_constraints);
// 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)) {
_goto_control.update(dt, states.position, states.yaw);
}
_trajectory_setpoint_sub.update(&_setpoint);
adjustSetpointForEKFResets(vehicle_local_position, _setpoint);
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
@ -416,6 +393,9 @@ void MulticopterPositionControl::Run()
if (_vehicle_control_mode.flag_multicopter_position_control_enabled
&& (_setpoint.timestamp >= _time_position_control_enabled)) {
// update vehicle constraints and handle smooth takeoff
_vehicle_constraints_sub.update(&_vehicle_constraints);
// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) {

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

@ -58,7 +58,6 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/goto_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
@ -97,14 +96,12 @@ private:
uORB::PublicationData<takeoff_status_s> _takeoff_status_pub{ORB_ID(takeoff_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::Subscription _goto_setpoint_sub{ORB_ID(goto_setpoint)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
@ -113,7 +110,6 @@ private:
hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */
hrt_abstime _time_position_control_enabled{0};
goto_setpoint_s _goto_setpoint{};
trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint};
vehicle_control_mode_s _vehicle_control_mode{};
@ -132,8 +128,6 @@ private:
.landed = true,
};
GotoControl _goto_control{this};
DEFINE_PARAMETERS(
// Position Control
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
@ -190,7 +184,8 @@ private:
control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */
control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */
PositionControl _control; /**< class for core PID position control */
GotoControl _goto_control{this}; ///< class for handling smooth goto position setpoints
PositionControl _control; ///< class for core PID position control
hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */