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

fix: moved trajectories to new message, removed derivative filters

Browse Source
This commit is contained in:
Pedro-Roque
2025-04-15 11:55:54 +02:00
parent be27462de3
commit 982f8a07b8
5 changed files with 20 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp
+1 -7
View File
@@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
// copy quaternion setpoint to attitude setpoint topic
const Quatf q_sp{R_sp};
q_sp.copyTo(att_sp.q_d);
// calculate euler angles, for logging only, must not be used for control
const Eulerf euler{R_sp};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
@@ -257,4 +251,4 @@ void setZeroIfNanVector3f(Vector3f &vector)
// Adding zero vector overwrites elements that are NaN with zero
addIfNotNanVector3f(vector, Vector3f());
}
}
}
src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp
+3 -3
View File
@@ -45,7 +45,7 @@
using namespace matrix;
const trajectory_setpoint_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
const trajectory_setpoint6dof_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}};
void ScPositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
@@ -88,12 +88,12 @@ void ScPositionControl::setState(const PositionControlStates &states)
_att_q = states.quaternion;
}
void ScPositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint)
void ScPositionControl::setInputSetpoint(const trajectory_setpoint6dof_s &setpoint)
{
_pos_sp = Vector3f(setpoint.position);
_vel_sp = Vector3f(setpoint.velocity);
_acc_sp = Vector3f(setpoint.acceleration);
_quat_sp = Quatf(setpoint.attitude);
_quat_sp = Quatf(setpoint.quaternion);
}
bool ScPositionControl::update(const float dt)
src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp
+3 -3
View File
@@ -41,7 +41,7 @@
#include <lib/mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/trajectory_setpoint6dof.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -128,7 +128,7 @@ public:
* Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
* @param setpoint setpoints including feed-forwards to execute in update()
*/
void setInputSetpoint(const trajectory_setpoint_s &setpoint);
void setInputSetpoint(const trajectory_setpoint6dof_s &setpoint);
/**
* Apply P-position and PID-velocity controller that updates the member
@@ -162,7 +162,7 @@ public:
/**
* All setpoints are set to NAN (uncontrolled). Timestampt zero.
*/
static const trajectory_setpoint_s empty_trajectory_setpoint;
static const trajectory_setpoint6dof_s empty_trajectory_setpoint;
private:
// The range limits of the hover thrust configuration/estimate
src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp
+7 -39
View File
@@ -40,10 +40,7 @@
using namespace matrix;
SpacecraftPositionControl::SpacecraftPositionControl(ModuleParams *parent) : ModuleParams(parent),
_vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)),
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD")
_vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint))
{
updateParams();
}
@@ -58,7 +55,6 @@ void SpacecraftPositionControl::updateParams()
// update parameters from storage
ModuleParams::updateParams();
SuperBlock::updateParams();
int num_changed = 0;
@@ -156,28 +152,16 @@ PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicl
if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) {
states.velocity.xy() = velocity_xy;
states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0));
states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1));
} else {
states.velocity(0) = states.velocity(1) = NAN;
states.acceleration(0) = states.acceleration(1) = NAN;
// reset derivatives to prevent acceleration spikes when regaining velocity
_vel_x_deriv.reset();
_vel_y_deriv.reset();
}
if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) {
states.velocity(2) = vehicle_local_position.vz;
states.acceleration(2) = _vel_z_deriv.update(states.velocity(2));
} else {
states.velocity(2) = NAN;
states.acceleration(2) = NAN;
// reset derivative to prevent acceleration spikes when regaining velocity
_vel_z_deriv.reset();
}
if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2])
@@ -201,9 +185,6 @@ void SpacecraftPositionControl::updatePositionControl()
math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
_time_stamp_last_loop = vehicle_local_position.timestamp_sample;
// set _dt in controllib Block for BlockDerivative
setDt(dt);
if (_vehicle_control_mode_sub.updated()) {
const bool previous_position_control_enabled = _vehicle_control_mode.flag_control_position_enabled;
@@ -244,19 +225,11 @@ void SpacecraftPositionControl::updatePositionControl()
}
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
_setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
// Set proper attitude setpoint with quaternion
// _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
}
}
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_vel_x_deriv.reset();
_vel_y_deriv.reset();
}
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_vel_z_deriv.reset();
}
// save latest reset counters
_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
_vz_reset_counter = vehicle_local_position.vz_reset_counter;
@@ -328,8 +301,6 @@ void SpacecraftPositionControl::publishLocalPositionSetpoint(vehicle_attitude_se
local_position_setpoint.x = _setpoint.position[0];
local_position_setpoint.y = _setpoint.position[1];
local_position_setpoint.z = _setpoint.position[2];
local_position_setpoint.yaw = NAN;
local_position_setpoint.yawspeed = NAN;
local_position_setpoint.vx = _setpoint.velocity[0];
local_position_setpoint.vy = _setpoint.velocity[1];
local_position_setpoint.vz = _setpoint.velocity[2];
@@ -348,10 +319,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt,
{
if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_armed) {
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
!_vehicle_control_mode.flag_control_offboard_enabled) {
if (!_vehicle_control_mode.flag_control_offboard_enabled) {
if (_vehicle_control_mode.flag_control_attitude_enabled &&
_vehicle_control_mode.flag_control_position_enabled) {
// We are in Stabilized mode
@@ -391,7 +359,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt,
const float pitch_body = 0.0;
Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp));
q_sp.copyTo(_setpoint.attitude);
q_sp.copyTo(_setpoint.quaternion);
_setpoint.timestamp = hrt_absolute_time();
@@ -409,7 +377,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt,
}
}
trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
trajectory_setpoint6dof_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
const PositionControlStates &states, bool warn)
{
// rate limit the warnings
@@ -420,7 +388,7 @@ trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const
_last_warn = now;
}
trajectory_setpoint_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint;
trajectory_setpoint6dof_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint;
failsafe_setpoint.timestamp = now;
failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = failsafe_setpoint.velocity[2] = 0.f;
src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp
+6 -10
View File
@@ -57,7 +57,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/trajectory_setpoint6dof.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -73,13 +73,13 @@ public:
SpacecraftPositionControl(ModuleParams *parent);
~SpacecraftPositionControl() = default;
bool updatePositionControl();
void updatePositionControl();
protected:
/**
* Update our local parameter cache.
*/
void updateParams(bool force);
void updateParams();
private:
@@ -92,7 +92,7 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint6dof)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -100,7 +100,7 @@ private:
hrt_abstime _time_position_control_enabled{0};
hrt_abstime _manual_setpoint_last_called{0};
trajectory_setpoint_s _setpoint{ScPositionControl::empty_trajectory_setpoint};
trajectory_setpoint6dof_s _setpoint{ScPositionControl::empty_trajectory_setpoint};
vehicle_control_mode_s _vehicle_control_mode{};
manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */
@@ -127,10 +127,6 @@ private:
(ParamFloat<px4::params::SPC_THR_MAX>) _param_mpc_thr_max
);
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */
control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */
matrix::Vector3f target_pos_sp;
float yaw_rate;
bool stabilized_pos_sp_initialized{false};
@@ -176,5 +172,5 @@ private:
* Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.
* This should only happen briefly when transitioning and never during mode operation or by design.
*/
trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn);
trajectory_setpoint6dof_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn);
};