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

Revise GotoControl

This commit is contained in:
Matthias Grob 2023-11-22 16:54:05 +01:00
parent 14b8afe972
commit 439d6c61e0
3 changed files with 93 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -69,12 +69,12 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
const Vector3f feedforward_velocity{};
const bool force_zero_velocity_setpoint = false;
PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
_position_smoother.generateSetpoints(position, position_setpoint, feedforward_velocity, dt,
force_zero_velocity_setpoint, out_setpoints);
_position_smoothing.generateSetpoints(position, position_setpoint, feedforward_velocity, dt,
force_zero_velocity_setpoint, out_setpoints);
_position_smoother.getCurrentPosition().copyTo(trajectory_setpoint.position);
_position_smoother.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
_position_smoother.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration);
_position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position);
_position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
_position_smoothing.getCurrentAcceleration().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)) {
@ -83,10 +83,10 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
}
setHeadingSmootherLimits(goto_setpoint);
_heading_smoother.update(goto_setpoint.heading, dt);
_heading_smoothing.update(goto_setpoint.heading, dt);
trajectory_setpoint.yaw = _heading_smoother.getSmoothedHeading();
trajectory_setpoint.yawspeed = _heading_smoother.getSmoothedHeadingRate();
trajectory_setpoint.yaw = _heading_smoothing.getSmoothedHeading();
trajectory_setpoint.yawspeed = _heading_smoothing.getSmoothedHeadingRate();
_controlling_heading = true;
@ -114,7 +114,7 @@ void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)
const Vector3f initial_acceleration{};
const Vector3f initial_velocity{};
_position_smoother.reset(initial_acceleration, initial_velocity, position);
_position_smoothing.reset(initial_acceleration, initial_velocity, position);
_need_smoother_reset = false;
}
@ -128,41 +128,41 @@ void GotoControl::resetHeadingSmoother(const float heading)
}
const float initial_heading_rate{0.f};
_heading_smoother.reset(heading, initial_heading_rate);
_heading_smoothing.reset(heading, initial_heading_rate);
}
void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint)
{
// constrain horizontal velocity
float max_horizontal_speed = _vehicle_constraints.max_horizontal_speed;
float max_horizontal_accel = _vehicle_constraints.max_horizontal_accel;
float max_horizontal_speed = _goto_constraints.max_horizontal_speed;
float max_horizontal_accel = _goto_constraints.max_horizontal_accel;
if (goto_setpoint.flag_set_max_horizontal_speed
&& PX4_ISFINITE(goto_setpoint.max_horizontal_speed)) {
max_horizontal_speed = math::constrain(goto_setpoint.max_horizontal_speed, 0.f,
_vehicle_constraints.max_horizontal_speed);
_goto_constraints.max_horizontal_speed);
// linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints
if (_position_smoother.getCurrentVelocityXY().norm() <= max_horizontal_speed) {
const float speed_scale = max_horizontal_speed / _vehicle_constraints.max_horizontal_speed;
max_horizontal_accel = math::constrain(_vehicle_constraints.max_horizontal_accel * speed_scale,
VelocitySmoothing::kMinAccel,
_vehicle_constraints.max_horizontal_accel);
if (_position_smoothing.getCurrentVelocityXY().norm() <= max_horizontal_speed) {
const float speed_scale = max_horizontal_speed / _goto_constraints.max_horizontal_speed;
max_horizontal_accel = math::constrain(_goto_constraints.max_horizontal_accel * speed_scale,
0.f,
_goto_constraints.max_horizontal_accel);
}
}
_position_smoother.setCruiseSpeed(max_horizontal_speed);
_position_smoother.setMaxVelocityXY(max_horizontal_speed);
_position_smoother.setMaxAccelerationXY(max_horizontal_accel);
_position_smoothing.setCruiseSpeed(max_horizontal_speed);
_position_smoothing.setMaxVelocityXY(max_horizontal_speed);
_position_smoothing.setMaxAccelerationXY(max_horizontal_accel);
// constrain vertical velocity
const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoother.getCurrentPositionZ() >
const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoothing.getCurrentPositionZ() >
0.f;
const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_speed :
_vehicle_constraints.max_up_speed;
const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_accel :
_vehicle_constraints.max_up_accel;
const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_speed :
_goto_constraints.max_up_speed;
const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_accel :
_goto_constraints.max_up_accel;
float max_vertical_speed = vehicle_max_vertical_speed;
float max_vertical_accel = vehicle_max_vertical_accel;
@ -173,38 +173,38 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint
// linearly scale vertical acceleration limit with vertical speed limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints
if (fabsf(_position_smoother.getCurrentVelocityZ()) <= max_vertical_speed) {
if (fabsf(_position_smoothing.getCurrentVelocityZ()) <= max_vertical_speed) {
const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed;
max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, VelocitySmoothing::kMinAccel,
max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, 0.f,
vehicle_max_vertical_accel);
}
}
_position_smoother.setMaxVelocityZ(max_vertical_speed);
_position_smoother.setMaxAccelerationZ(max_vertical_accel);
_position_smoothing.setMaxVelocityZ(max_vertical_speed);
_position_smoothing.setMaxAccelerationZ(max_vertical_accel);
_position_smoother.setMaxJerkXY(_vehicle_constraints.max_jerk);
_position_smoother.setMaxJerkZ(_vehicle_constraints.max_jerk);
_position_smoothing.setMaxJerkXY(_goto_constraints.max_jerk);
_position_smoothing.setMaxJerkZ(_goto_constraints.max_jerk);
}
void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint)
{
float max_heading_rate = _vehicle_constraints.max_heading_rate;
float max_heading_accel = _vehicle_constraints.max_heading_accel;
float max_heading_rate = _goto_constraints.max_heading_rate;
float max_heading_accel = _goto_constraints.max_heading_accel;
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, HeadingSmoother::kMinHeadingRate,
_vehicle_constraints.max_heading_rate);
max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, 0.f,
_goto_constraints.max_heading_rate);
// linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints
if (fabsf(_heading_smoother.getSmoothedHeadingRate()) <= max_heading_rate) {
const float rate_scale = max_heading_rate / _vehicle_constraints.max_heading_rate;
max_heading_accel = math::constrain(_vehicle_constraints.max_heading_accel * rate_scale,
HeadingSmoother::kMinHeadingAccel, _vehicle_constraints.max_heading_accel);
if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) {
const float rate_scale = max_heading_rate / _goto_constraints.max_heading_rate;
max_heading_accel = math::constrain(_goto_constraints.max_heading_accel * rate_scale,
0.f, _goto_constraints.max_heading_accel);
}
}
_heading_smoother.setMaxHeadingRate(max_heading_rate);
_heading_smoother.setMaxHeadingAccel(max_heading_accel);
_heading_smoothing.setMaxHeadingRate(max_heading_rate);
_heading_smoothing.setMaxHeadingAccel(max_heading_accel);
}

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

@ -37,15 +37,15 @@
* A class which smooths position and heading references from "go-to" setpoints
* for planar multicopters.
*
* Be sure to set constraints with setVehicleConstraints() before calling the update() method for the first time, otherwise
* the default motion will be VERY slow.
* Be sure to set constraints with setGotoConstraints() before calling the update() method for the first time
*/
#pragma once
#include <matrix/matrix/math.hpp>
#include <lib/motion_planning/HeadingSmoothing.hpp>
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <lib/motion_planning/HeadingSmoother.hpp>
#include <mathlib/math/Limits.hpp>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/goto_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h>
@ -55,34 +55,43 @@ public:
GotoControl() = default;
~GotoControl() = default;
/**
* @brief struct containing maximum vehicle translational and rotational constraints
*
*/
struct VehicleConstraints {
float max_horizontal_speed = kMinSpeed; // [m/s]
float max_down_speed = kMinSpeed; // [m/s]
float max_up_speed = kMinSpeed; // [m/s]
float max_horizontal_accel = VelocitySmoothing::kMinAccel; // [m/s^2]
float max_down_accel = VelocitySmoothing::kMinAccel; // [m/s^2]
float max_up_accel = VelocitySmoothing::kMinAccel; // [m/s^2]
float max_jerk = VelocitySmoothing::kMinJerk; // [m/s^3]
float max_heading_rate = HeadingSmoother::kMinHeadingRate; // [rad/s]
float max_heading_accel = HeadingSmoother::kMinHeadingAccel; // [rad/s^2]
/** @brief struct containing maximum vehicle translational and rotational constraints */
struct GotoConstraints {
float max_horizontal_speed; // [m/s]
float max_down_speed; // [m/s]
float max_up_speed; // [m/s]
float max_horizontal_accel; // [m/s^2]
float max_down_accel; // [m/s^2]
float max_up_accel; // [m/s^2]
float max_jerk; // [m/s^3]
float max_heading_rate; // [rad/s]
float max_heading_accel; // [rad/s^2]
};
/**
* @brief updates the smoothers with the current setpoints and outputs the "trajectory setpoint" for lower level
* loops to track.
* @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively
* overriden (e.g. slowed down) via the speed scalers in the go-to setpoint.
*
* @param[in] dt [s] time since last control update
* @param[in] position [m] (NED) local vehicle position
* @param[in] heading [rad] (from North) vehicle heading
* @param[in] goto_setpoint struct containing current go-to setpoints
* @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
* @param vehicle_constraints Struct containing desired vehicle constraints
*/
void update(const float dt, const matrix::Vector3f &position, const float heading,
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint);
void setGotoConstraints(const GotoConstraints &vehicle_constraints)
{
_goto_constraints.max_horizontal_speed = math::max(0.f, vehicle_constraints.max_horizontal_speed);
_goto_constraints.max_down_speed = math::max(0.f, vehicle_constraints.max_down_speed);
_goto_constraints.max_up_speed = math::max(0.f, vehicle_constraints.max_up_speed);
_goto_constraints.max_horizontal_accel = math::max(0.f,
vehicle_constraints.max_horizontal_accel);
_goto_constraints.max_down_accel = math::max(0.f, vehicle_constraints.max_down_accel);
_goto_constraints.max_up_accel = math::max(0.f, vehicle_constraints.max_up_accel);
_goto_constraints.max_jerk = math::max(0.f, vehicle_constraints.max_jerk);
_goto_constraints.max_heading_rate = math::max(0.f,
vehicle_constraints.max_heading_rate);
_goto_constraints.max_heading_accel = math::max(0.f,
vehicle_constraints.max_heading_accel);
}
/** @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); }
/**
* @brief resets the position smoother at the current position with zero velocity and acceleration.
@ -99,42 +108,19 @@ public:
void resetHeadingSmoother(const float heading);
/**
* @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively
* overriden (e.g. slowed down) via the speed scalers in the go-to setpoint.
* @brief updates the smoothers with the current setpoints and outputs the "trajectory setpoint" for lower level
* loops to track.
*
* @param vehicle_constraints Struct containing desired vehicle constraints
* @param[in] dt [s] time since last control update
* @param[in] position [m] (NED) local vehicle position
* @param[in] heading [rad] (from North) vehicle heading
* @param[in] goto_setpoint struct containing current go-to setpoints
* @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
*/
void setVehicleConstraints(const VehicleConstraints &vehicle_constraints)
{
_vehicle_constraints.max_horizontal_speed = math::max(kMinSpeed, vehicle_constraints.max_horizontal_speed);
_vehicle_constraints.max_down_speed = math::max(kMinSpeed, vehicle_constraints.max_down_speed);
_vehicle_constraints.max_up_speed = math::max(kMinSpeed, vehicle_constraints.max_up_speed);
_vehicle_constraints.max_horizontal_accel = math::max(VelocitySmoothing::kMinAccel,
vehicle_constraints.max_horizontal_accel);
_vehicle_constraints.max_down_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_down_accel);
_vehicle_constraints.max_up_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_up_accel);
_vehicle_constraints.max_jerk = math::max(VelocitySmoothing::kMinJerk, vehicle_constraints.max_jerk);
_vehicle_constraints.max_heading_rate = math::max(HeadingSmoother::kMinHeadingRate,
vehicle_constraints.max_heading_rate);
_vehicle_constraints.max_heading_accel = math::max(HeadingSmoother::kMinHeadingAccel,
vehicle_constraints.max_heading_accel);
}
/**
* @brief Set the position smoother's maximum allowed horizontal position error at which trajectory integration halts
*
* @param error [m] horizontal position error
*/
void setMaxAllowedHorizontalPositionError(const float error)
{
_position_smoother.setMaxAllowedHorizontalError(error);
}
void update(const float dt, const matrix::Vector3f &position, const float heading,
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint);
private:
// [m/s] minimum value of the maximum translational speeds
static constexpr float kMinSpeed{0.1f};
/**
* @brief optionally sets dynamic translational speed limits with corresponding scale on acceleration
*
@ -149,9 +135,10 @@ private:
*/
void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint);
VehicleConstraints _vehicle_constraints{};
PositionSmoothing _position_smoother;
HeadingSmoother _heading_smoother;
GotoConstraints _goto_constraints{};
PositionSmoothing _position_smoothing;
HeadingSmoothing _heading_smoothing;
// flags that the next update() requires a valid current vehicle position to reset the smoothers
bool _need_smoother_reset{true};

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

@ -386,7 +386,7 @@ void MulticopterPositionControl::Run()
_goto_control.resetHeadingSmoother(states.yaw);
}
const GotoControl::VehicleConstraints vehicle_constraints = {
const GotoControl::GotoConstraints goto_constraints = {
_param_mpc_xy_cruise.get(),
_param_mpc_z_v_auto_dn.get(),
_param_mpc_z_v_auto_up.get(),
@ -398,7 +398,7 @@ void MulticopterPositionControl::Run()
_param_mpc_yawaauto_max.get()
};
_goto_control.setVehicleConstraints(vehicle_constraints);
_goto_control.setGotoConstraints(goto_constraints);
_goto_control.setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get());
_goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint);