fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

ManualVelocitySmoothing - Cosmetic changes (renaming, indentation,..)

Browse Source 
Set the classes as final and set destructor to non-virtual
This commit is contained in:
bresch
2019-09-19 17:31:49 +02:00
committed by Mathieu Bresciani
parent 7cdb2364e9
commit ce96d98621
4 changed files with 80 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
+14 -13
View File
@@ -41,11 +41,12 @@ using namespace matrix;
void ManualVelocitySmoothingXY::reset(Vector2f accel, Vector2f vel, Vector2f pos)
{
for (int i = 0; i < 2; i++) {
_smoothing[i].reset(accel(i), vel(i), pos(i));
_trajectory[i].reset(accel(i), vel(i), pos(i));
}
resetPositionLock();
}
void ManualVelocitySmoothingXY::resetPositionLock()
{
_position_lock_active = false;
@@ -69,22 +70,22 @@ void ManualVelocitySmoothingXY::update(float dt, Vector2f velocity_target)
void ManualVelocitySmoothingXY::updateTrajectories(float dt)
{
for (int i = 0; i < 2; ++i) {
_smoothing[i].updateTraj(dt);
_trajectory[i].updateTraj(dt);
_state.j(i) = _smoothing[i].getCurrentJerk();
_state.a(i) = _smoothing[i].getCurrentAcceleration();
_state.v(i) = _smoothing[i].getCurrentVelocity();
_state.x(i) = _smoothing[i].getCurrentPosition();
_state.j(i) = _trajectory[i].getCurrentJerk();
_state.a(i) = _trajectory[i].getCurrentAcceleration();
_state.v(i) = _trajectory[i].getCurrentVelocity();
_state.x(i) = _trajectory[i].getCurrentPosition();
}
}
void ManualVelocitySmoothingXY::updateTrajDurations(Vector2f velocity_target)
{
for (int i = 0; i < 2; ++i) {
_smoothing[i].updateDurations(velocity_target(i));
_trajectory[i].updateDurations(velocity_target(i));
}
VelocitySmoothing::timeSynchronization(_smoothing, 2);
VelocitySmoothing::timeSynchronization(_trajectory, 2);
}
void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target)
@@ -107,15 +108,15 @@ void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target)
} else {
// Unlock position
if (_position_lock_active) {
// Start the trajectory at the current velocity setpoint
_smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
_smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
// Start the trajectory at the current velocity setpoint
_trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
_trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
_position_setpoint_locked(0) = NAN;
_position_setpoint_locked(1) = NAN;
}
_position_lock_active = false;
_smoothing[0].setCurrentPosition(_position_estimate(0));
_smoothing[1].setCurrentPosition(_position_estimate(1));
_trajectory[0].setCurrentPosition(_position_estimate(0));
_trajectory[1].setCurrentPosition(_position_estimate(1));
}
}
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp
+31 -23
View File
@@ -44,59 +44,67 @@
using matrix::Vector2f;
class ManualVelocitySmoothingXY
class ManualVelocitySmoothingXY final
{
public:
ManualVelocitySmoothingXY() = default;
virtual ~ManualVelocitySmoothingXY() = default;
~ManualVelocitySmoothingXY() = default;
void reset(Vector2f accel, Vector2f vel, Vector2f pos);
void update(float dt, Vector2f velocity_target);
void setVelSpFeedback(const Vector2f fb) { _velocity_setpoint_feedback = fb; }
void setMaxJerk(const float max_jerk) {
_smoothing[0].setMaxJerk(max_jerk);
_smoothing[1].setMaxJerk(max_jerk);
void setMaxJerk(const float max_jerk)
{
_trajectory[0].setMaxJerk(max_jerk);
_trajectory[1].setMaxJerk(max_jerk);
}
float getMaxJerk() const { return _smoothing[0].getMaxJerk(); }
float getMaxJerk() const { return _trajectory[0].getMaxJerk(); }
void setMaxAccel(const float max_accel) {
_smoothing[0].setMaxAccel(max_accel);
_smoothing[1].setMaxAccel(max_accel);
void setMaxAccel(const float max_accel)
{
_trajectory[0].setMaxAccel(max_accel);
_trajectory[1].setMaxAccel(max_accel);
}
float getMaxAccel() const { return _smoothing[0].getMaxAccel(); }
void setMaxVel(const float max_vel) {
_smoothing[0].setMaxVel(max_vel);
_smoothing[1].setMaxVel(max_vel);
float getMaxAccel() const { return _trajectory[0].getMaxAccel(); }
void setMaxVel(const float max_vel)
{
_trajectory[0].setMaxVel(max_vel);
_trajectory[1].setMaxVel(max_vel);
}
float getMaxVel() const { return _smoothing[0].getMaxVel(); }
float getMaxVel() const { return _trajectory[0].getMaxVel(); }
Vector2f getCurrentJerk() const { return _state.j; }
Vector2f getCurrentAcceleration() const { return _state.a; }
void setCurrentVelocity(const Vector2f vel) {
void setCurrentVelocity(const Vector2f vel)
{
_state.v = vel;
_smoothing[0].setCurrentVelocity(vel(0));
_smoothing[1].setCurrentVelocity(vel(1));
_trajectory[0].setCurrentVelocity(vel(0));
_trajectory[1].setCurrentVelocity(vel(1));
}
Vector2f getCurrentVelocity() const { return _state.v; }
void setCurrentPosition(const Vector2f pos) {
void setCurrentPosition(const Vector2f pos)
{
_state.x = pos;
_smoothing[0].setCurrentPosition(pos(0));
_smoothing[1].setCurrentPosition(pos(1));
_trajectory[0].setCurrentPosition(pos(0));
_trajectory[1].setCurrentPosition(pos(1));
// TODO: check lock/unlock in case of EKF reset
}
Vector2f getCurrentPosition() const { return _position_setpoint_locked; }
void setCurrentPositionEstimate(Vector2f pos) { _position_estimate = pos; }
private:
void resetPositionLock();
void updateTrajectories(float dt);
void updateTrajConstraints();
void checkPositionLock(Vector2f velocity_target);
void updateTrajDurations(Vector2f velocity_target);
VelocitySmoothing _smoothing[2]; ///< Smoothing in x and y directions
VelocitySmoothing _trajectory[2]; ///< Trajectory in x and y directions
bool _position_lock_active{false};
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
+15 -14
View File
@@ -38,10 +38,11 @@
void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos)
{
_smoothing.reset(accel, vel, pos);
_trajectory.reset(accel, vel, pos);
resetPositionLock();
}
void ManualVelocitySmoothingZ::resetPositionLock()
{
_position_lock_active = false;
@@ -62,28 +63,28 @@ void ManualVelocitySmoothingZ::update(float dt, float velocity_target)
checkPositionLock(velocity_target);
// Update durations
_smoothing.updateDurations(velocity_target);
_trajectory.updateDurations(velocity_target);
}
void ManualVelocitySmoothingZ::updateTrajectories(float dt)
{
_smoothing.updateTraj(dt);
_trajectory.updateTraj(dt);
_state.j = _smoothing.getCurrentJerk();
_state.a = _smoothing.getCurrentAcceleration();
_state.v = _smoothing.getCurrentVelocity();
_state.x = _smoothing.getCurrentPosition();
_state.j = _trajectory.getCurrentJerk();
_state.a = _trajectory.getCurrentAcceleration();
_state.v = _trajectory.getCurrentVelocity();
_state.x = _trajectory.getCurrentPosition();
}
void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target)
{
if (velocity_target < 0.f) { // up
_smoothing.setMaxAccel(_max_accel_up);
_smoothing.setMaxVel(_max_vel_up);
_trajectory.setMaxAccel(_max_accel_up);
_trajectory.setMaxVel(_max_vel_up);
} else { // down
_smoothing.setMaxAccel(_max_accel_down);
_smoothing.setMaxVel(_max_vel_down);
_trajectory.setMaxAccel(_max_accel_down);
_trajectory.setMaxVel(_max_vel_down);
}
}
@@ -107,12 +108,12 @@ void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target)
} else {
// Unlock position
if (_position_lock_active) {
// Start the trajectory at the current velocity setpoint
_smoothing.setCurrentVelocity(_velocity_setpoint_feedback);
// Start the trajectory at the current velocity setpoint
_trajectory.setCurrentVelocity(_velocity_setpoint_feedback);
_position_setpoint_locked = NAN;
}
_position_lock_active = false;
_smoothing.setCurrentPosition(_position_estimate);
_trajectory.setCurrentPosition(_position_estimate);
}
}
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp
+20 -14
View File
@@ -40,44 +40,50 @@
#include "VelocitySmoothing.hpp"
class ManualVelocitySmoothingZ
class ManualVelocitySmoothingZ final
{
public:
ManualVelocitySmoothingZ() = default;
virtual ~ManualVelocitySmoothingZ() = default;
~ManualVelocitySmoothingZ() = default;
void reset(float accel, float vel, float pos);
void update(float dt, float velocity_target);
void setVelSpFeedback(const float fb) { _velocity_setpoint_feedback = fb; }
void setMaxJerk(const float max_jerk) {
_smoothing.setMaxJerk(max_jerk);
void setMaxJerk(const float max_jerk)
{
_trajectory.setMaxJerk(max_jerk);
}
void setMaxAccelUp(const float max_accel_up) {
void setMaxAccelUp(const float max_accel_up)
{
_max_accel_up = max_accel_up;
}
void setMaxAccelDown(const float max_accel_down) {
void setMaxAccelDown(const float max_accel_down)
{
_max_accel_down = max_accel_down;
}
void setMaxVelUp(const float max_vel_up) {
void setMaxVelUp(const float max_vel_up)
{
_max_vel_up = max_vel_up;
}
void setMaxVelDown(const float max_vel_down) {
void setMaxVelDown(const float max_vel_down)
{
_max_vel_down = max_vel_down;
}
float getCurrentJerk() const { return _state.j; }
float getCurrentAcceleration() const { return _state.a; }
void setCurrentVelocity(const float vel) {
void setCurrentVelocity(const float vel)
{
_state.v = vel;
_smoothing.setCurrentVelocity(vel);
_trajectory.setCurrentVelocity(vel);
}
float getCurrentVelocity() const { return _state.v; }
void setCurrentPosition(const float pos) {
void setCurrentPosition(const float pos)
{
_state.x = pos;
_smoothing.setCurrentPosition(pos);
_trajectory.setCurrentPosition(pos);
}
float getCurrentPosition() const { return _position_setpoint_locked; }
void setCurrentPositionEstimate(float pos) { _position_estimate = pos; }
@@ -89,7 +95,7 @@ private:
void checkPositionLock(float velocity_target);
void updateTrajDurations(float velocity_target);
VelocitySmoothing _smoothing; ///< Smoothing in z direction
VelocitySmoothing _trajectory; ///< Trajectory in z direction
bool _position_lock_active{false};