zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Refactor FlightTaskManualPositionSmoothVel

sbg
bresch 5 years ago committed by Mathieu Bresciani
parent
080eedfd02
commit
f5d7eb4d87
7 changed files with 510 additions and 215 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 212
      src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  2. 50
      src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
  3. 2
      src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
  4. 121
      src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
  5. 110
      src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp
  6. 118
      src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
  7. 112
      src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp

212
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -44,17 +44,13 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint @@ -44,17 +44,13 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint
// Check if the previous FlightTask provided setpoints
checkSetpoints(last_setpoint);
const Vector3f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y, last_setpoint.acc_z);
const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
const Vector2f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y);
const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy);
const Vector2f pos_prev(last_setpoint.x, last_setpoint.y);
for (int i = 0; i < 2; ++i) {
_smoothing_xy[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
}
_smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2));
_smoothing_xy.reset(accel_prev, vel_prev, pos_prev);
_smoothing_z.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z);
_resetPositionLock();
_initEkfResetCounters();
return ret;
@ -64,13 +60,9 @@ void FlightTaskManualPositionSmoothVel::reActivate() @@ -64,13 +60,9 @@ void FlightTaskManualPositionSmoothVel::reActivate()
{
// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
// using the generated jerk, reset the z derivatives to zero
for (int i = 0; i < 2; ++i) {
_smoothing_xy[i].reset(0.f, _velocity(i), _position(i));
}
_smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position));
_smoothing_z.reset(0.f, 0.f, _position(2));
_resetPositionLock();
_initEkfResetCounters();
}
@ -98,25 +90,6 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se @@ -98,25 +90,6 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
}
void FlightTaskManualPositionSmoothVel::_resetPositionLock()
{
_resetPositionLockXY();
_resetPositionLockZ();
}
void FlightTaskManualPositionSmoothVel::_resetPositionLockXY()
{
_position_lock_xy_active = false;
_position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN;
}
void FlightTaskManualPositionSmoothVel::_resetPositionLockZ()
{
_position_lock_z_active = false;
_position_setpoint_z_locked = NAN;
}
void FlightTaskManualPositionSmoothVel::_initEkfResetCounters()
{
_initEkfResetCountersXY();
@ -140,25 +113,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() @@ -140,25 +113,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
// Reset trajectories if EKF did a reset
_checkEkfResetCounters();
// Update state
_updateTrajectories();
// Set max accel/vel/jerk
// Has to be done before _updateTrajDurations()
// Has to be done before _updateTrajectories()
_updateTrajConstraints();
_updateTrajVelFeedback();
_updateTrajCurrentPositionEstimate();
// Get yaw setpont, un-smoothed position setpoints
// Has to be done before _checkPositionLock()
// Get yaw setpoint, un-smoothed position setpoints
FlightTaskManualPosition::_updateSetpoints();
_velocity_target_xy = Vector2f(_velocity_setpoint);
_velocity_target_z = _velocity_setpoint(2);
// Lock or unlock position
// Has to be done before _updateTrajDurations()
_checkPositionLock();
// Update durations and sync XY
_updateTrajDurations();
_updateTrajectories(_velocity_setpoint);
// Fill the jerk, acceleration, velocity and position setpoint vectors
_setOutputState();
@ -174,14 +138,12 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters() @@ -174,14 +138,12 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersXY()
{
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
_smoothing_xy[0].setCurrentPosition(_position(0));
_smoothing_xy[1].setCurrentPosition(_position(1));
_smoothing_xy.setCurrentPosition(Vector2f(_position));
_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
}
if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
_smoothing_xy[0].setCurrentVelocity(_velocity(0));
_smoothing_xy[1].setCurrentVelocity(_velocity(1));
_smoothing_xy.setCurrentVelocity(Vector2f(_velocity));
_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
}
}
@ -199,34 +161,6 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersZ() @@ -199,34 +161,6 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersZ()
}
}
void FlightTaskManualPositionSmoothVel::_updateTrajectories()
{
_updateTrajectoriesXY();
_updateTrajectoriesZ();
}
void FlightTaskManualPositionSmoothVel::_updateTrajectoriesXY()
{
for (int i = 0; i < 2; ++i) {
_smoothing_xy[i].updateTraj(_deltatime);
_traj_xy.j(i) = _smoothing_xy[i].getCurrentJerk();
_traj_xy.a(i) = _smoothing_xy[i].getCurrentAcceleration();
_traj_xy.v(i) = _smoothing_xy[i].getCurrentVelocity();
_traj_xy.x(i) = _smoothing_xy[i].getCurrentPosition();
}
}
void FlightTaskManualPositionSmoothVel::_updateTrajectoriesZ()
{
_smoothing_z.updateTraj(_deltatime);
_traj_z.j = _smoothing_z.getCurrentJerk();
_traj_z.a = _smoothing_z.getCurrentAcceleration();
_traj_z.v = _smoothing_z.getCurrentVelocity();
_traj_z.x = _smoothing_z.getCurrentPosition();
}
void FlightTaskManualPositionSmoothVel::_updateTrajConstraints()
{
_updateTrajConstraintsXY();
@ -235,106 +169,38 @@ void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() @@ -235,106 +169,38 @@ void FlightTaskManualPositionSmoothVel::_updateTrajConstraints()
void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY()
{
for (int i = 0; i < 2; i++) {
_smoothing_xy[i].setMaxJerk(_param_mpc_jerk_max.get());
_smoothing_xy[i].setMaxAccel(_param_mpc_acc_hor_max.get());
_smoothing_xy[i].setMaxVel(_constraints.speed_xy);
}
_smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get());
_smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get());
_smoothing_xy.setMaxVel(_constraints.speed_xy);
}
void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ()
{
_smoothing_z.setMaxJerk(_param_mpc_jerk_max.get());
if (_velocity_setpoint(2) < 0.f) { // up
_smoothing_z.setMaxAccel(_param_mpc_acc_up_max.get());
_smoothing_z.setMaxVel(_constraints.speed_up);
} else { // down
_smoothing_z.setMaxAccel(_param_mpc_acc_down_max.get());
_smoothing_z.setMaxVel(_constraints.speed_down);
}
}
void FlightTaskManualPositionSmoothVel::_updateTrajDurations()
{
_updateTrajDurationsXY();
_updateTrajDurationsZ();
}
void FlightTaskManualPositionSmoothVel::_updateTrajDurationsXY()
{
for (int i = 0; i < 2; ++i) {
_smoothing_xy[i].updateDurations(_velocity_target_xy(i));
}
_smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get());
_smoothing_z.setMaxVelUp(_constraints.speed_up);
VelocitySmoothing::timeSynchronization(_smoothing_xy, 2);
_smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get());
_smoothing_z.setMaxVelDown(_constraints.speed_down);
}
void FlightTaskManualPositionSmoothVel::_updateTrajDurationsZ()
void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback()
{
_smoothing_z.updateDurations(_velocity_target_z);
_smoothing_xy.setVelSpFeedback(Vector2f(_velocity_setpoint_feedback));
_smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2));
}
void FlightTaskManualPositionSmoothVel::_checkPositionLock()
void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate()
{
/**
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
* is continuous. We know that the output of the position loop (part of the velocity setpoint)
* will suddenly become null
* and only the feedforward (generated by this flight task) will remain.
* This is why the previous input of the velocity controller
* is used to set current velocity of the trajectory.
*/
_checkPositionLockXY();
_checkPositionLockZ();
_smoothing_xy.setCurrentPositionEstimate(Vector2f(_position));
_smoothing_z.setCurrentPositionEstimate(_position(2));
}
void FlightTaskManualPositionSmoothVel::_checkPositionLockXY()
void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target)
{
if (_traj_xy.v.length() < 0.1f &&
_traj_xy.a.length() < .2f &&
_velocity_target_xy.length() <= FLT_EPSILON) {
// Lock position
_position_lock_xy_active = true;
_position_setpoint_xy_locked = _traj_xy.x;
} else {
// Unlock position
if (_position_lock_xy_active) {
_smoothing_xy[0].setCurrentVelocity(_velocity_setpoint_feedback(
0)); // Start the trajectory at the current velocity setpoint
_smoothing_xy[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
_position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN;
}
_position_lock_xy_active = false;
_smoothing_xy[0].setCurrentPosition(_position(0));
_smoothing_xy[1].setCurrentPosition(_position(1));
}
}
void FlightTaskManualPositionSmoothVel::_checkPositionLockZ()
{
if (fabsf(_traj_z.v) < 0.1f &&
fabsf(_traj_z.a) < .2f &&
fabsf(_velocity_target_z) <= FLT_EPSILON) {
// Lock position
_position_lock_z_active = true;
_position_setpoint_z_locked = _traj_z.x;
} else {
// Unlock position
if (_position_lock_z_active) {
_smoothing_z.setCurrentVelocity(_velocity_setpoint_feedback(
2)); // Start the trajectory at the current velocity setpoint
_position_setpoint_z_locked = NAN;
}
_position_lock_z_active = false;
_smoothing_z.setCurrentPosition(_position(2));
}
_smoothing_xy.update(_deltatime, Vector2f(vel_target));
_smoothing_z.update(_deltatime, vel_target(2));
}
void FlightTaskManualPositionSmoothVel::_setOutputState()
@ -345,18 +211,16 @@ void FlightTaskManualPositionSmoothVel::_setOutputState() @@ -345,18 +211,16 @@ void FlightTaskManualPositionSmoothVel::_setOutputState()
void FlightTaskManualPositionSmoothVel::_setOutputStateXY()
{
for (int i = 0; i < 2; i++) {
_jerk_setpoint(i) = _traj_xy.j(i);
_acceleration_setpoint(i) = _traj_xy.a(i);
_velocity_setpoint(i) = _traj_xy.v(i);
_position_setpoint(i) = _position_setpoint_xy_locked(i);
}
_jerk_setpoint = _smoothing_xy.getCurrentJerk();
_acceleration_setpoint = _smoothing_xy.getCurrentAcceleration();
_velocity_setpoint = _smoothing_xy.getCurrentVelocity();
_position_setpoint = _smoothing_xy.getCurrentPosition();
}
void FlightTaskManualPositionSmoothVel::_setOutputStateZ()
{
_jerk_setpoint(2) = _traj_z.j;
_acceleration_setpoint(2) = _traj_z.a;
_velocity_setpoint(2) = _traj_z.v;
_position_setpoint(2) = _position_setpoint_z_locked;
_jerk_setpoint(2) = _smoothing_z.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration();
_velocity_setpoint(2) = _smoothing_z.getCurrentVelocity();
_position_setpoint(2) = _smoothing_z.getCurrentPosition();
}

50
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -40,9 +40,11 @@ @@ -40,9 +40,11 @@
#pragma once
#include "FlightTaskManualPosition.hpp"
#include "VelocitySmoothing.hpp"
#include "ManualVelocitySmoothingXY.hpp"
#include "ManualVelocitySmoothingZ.hpp"
using matrix::Vector2f;
using matrix::Vector3f;
class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
{
@ -67,10 +69,6 @@ protected: @@ -67,10 +69,6 @@ protected:
private:
void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
void _resetPositionLock();
void _resetPositionLockXY();
void _resetPositionLockZ();
void _initEkfResetCounters();
void _initEkfResetCountersXY();
void _initEkfResetCountersZ();
@ -79,37 +77,21 @@ private: @@ -79,37 +77,21 @@ private:
void _checkEkfResetCountersXY();
void _checkEkfResetCountersZ();
void _updateTrajectories();
void _updateTrajectoriesXY();
void _updateTrajectoriesZ();
void _updateTrajConstraints();
void _updateTrajConstraintsXY();
void _updateTrajConstraintsZ();
void _updateTrajDurations();
void _updateTrajDurationsXY();
void _updateTrajDurationsZ();
void _updateTrajVelFeedback();
void _updateTrajCurrentPositionEstimate();
void _checkPositionLock();
void _checkPositionLockXY();
void _checkPositionLockZ();
void _updateTrajectories(Vector3f vel_target);
void _setOutputState();
void _setOutputStateXY();
void _setOutputStateZ();
VelocitySmoothing _smoothing_xy[2]; ///< Smoothing in x and y directions
VelocitySmoothing _smoothing_z; ///< Smoothing in z direction
Vector2f _velocity_target_xy;
float _velocity_target_z{0.f};
bool _position_lock_xy_active{false};
bool _position_lock_z_active{false};
Vector2f _position_setpoint_xy_locked;
float _position_setpoint_z_locked{NAN};
ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions
ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction
/* counters for estimator local position resets */
struct {
@ -118,18 +100,4 @@ private: @@ -118,18 +100,4 @@ private:
uint8_t z;
uint8_t vz;
} _reset_counters{0, 0, 0, 0};
struct {
Vector2f j;
Vector2f a;
Vector2f v;
Vector2f x;
} _traj_xy;
struct {
float j;
float a;
float v;
float x;
} _traj_z{0.f, 0.f, 0.f, NAN};
};

2
src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
Unescape View File

@ -37,6 +37,8 @@ px4_add_library(FlightTaskUtility @@ -37,6 +37,8 @@ px4_add_library(FlightTaskUtility
ObstacleAvoidance.cpp
StraightLine.cpp
VelocitySmoothing.cpp
ManualVelocitySmoothingXY.cpp
ManualVelocitySmoothingZ.cpp
)
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis)

121
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
Unescape View File

@ -0,0 +1,121 @@ @@ -0,0 +1,121 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ManualVelocitySmoothingXY.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
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));
}
resetPositionLock();
}
void ManualVelocitySmoothingXY::resetPositionLock()
{
_position_lock_active = false;
_position_setpoint_locked(0) = NAN;
_position_setpoint_locked(1) = NAN;
}
void ManualVelocitySmoothingXY::update(float dt, Vector2f velocity_target)
{
// Update state
updateTrajectories(dt);
// Lock or unlock position
// Has to be done before _updateTrajDurations()
checkPositionLock(velocity_target);
// Update durations and sync XY
updateTrajDurations(velocity_target);
}
void ManualVelocitySmoothingXY::updateTrajectories(float dt)
{
for (int i = 0; i < 2; ++i) {
_smoothing[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();
}
}
void ManualVelocitySmoothingXY::updateTrajDurations(Vector2f velocity_target)
{
for (int i = 0; i < 2; ++i) {
_smoothing[i].updateDurations(velocity_target(i));
}
VelocitySmoothing::timeSynchronization(_smoothing, 2);
}
void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target)
{
/**
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
* is continuous. We know that the output of the position loop (part of the velocity setpoint)
* will suddenly become null
* and only the feedforward (generated by this flight task) will remain.
* This is why the previous input of the velocity controller
* is used to set current velocity of the trajectory.
*/
if (_state.v.length() < 0.1f &&
_state.a.length() < .2f &&
velocity_target.length() <= FLT_EPSILON) {
// Lock position
_position_lock_active = true;
_position_setpoint_locked = _state.x;
} 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));
_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));
}
}

110
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp
Unescape View File

@ -0,0 +1,110 @@ @@ -0,0 +1,110 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ManualVelocitySmoothingXY.hpp
*
*/
#pragma once
#include "VelocitySmoothing.hpp"
#include <matrix/matrix/math.hpp>
using matrix::Vector2f;
class ManualVelocitySmoothingXY
{
public:
ManualVelocitySmoothingXY() = default;
virtual ~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 setMaxAccel(const float max_accel) {
_smoothing[0].setMaxAccel(max_accel);
_smoothing[1].setMaxAccel(max_accel);
}
void setMaxVel(const float max_vel) {
_smoothing[0].setMaxVel(max_vel);
_smoothing[1].setMaxVel(max_vel);
}
Vector2f getCurrentJerk() const { return _state.j; }
Vector2f getCurrentAcceleration() const { return _state.a; }
void setCurrentVelocity(const Vector2f vel) {
_state.v = vel;
_smoothing[0].setCurrentVelocity(vel(0));
_smoothing[1].setCurrentVelocity(vel(1));
}
Vector2f getCurrentVelocity() const { return _state.v; }
void setCurrentPosition(const Vector2f pos) {
_state.x = pos;
_smoothing[0].setCurrentPosition(pos(0));
_smoothing[1].setCurrentPosition(pos(1));
}
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
bool _position_lock_active{false};
Vector2f _position_setpoint_locked;
Vector2f _velocity_setpoint_feedback;
Vector2f _position_estimate;
struct {
Vector2f j;
Vector2f a;
Vector2f v;
Vector2f x;
} _state;
};

118
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
Unescape View File

@ -0,0 +1,118 @@ @@ -0,0 +1,118 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ManualVelocitySmoothingZ.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos)
{
_smoothing.reset(accel, vel, pos);
resetPositionLock();
}
void ManualVelocitySmoothingZ::resetPositionLock()
{
_position_lock_active = false;
_position_setpoint_locked = NAN;
}
void ManualVelocitySmoothingZ::update(float dt, float velocity_target)
{
// Update state
updateTrajectories(dt);
// Set max accel/vel/jerk
// Has to be done before _updateTrajDurations()
updateTrajConstraints(velocity_target);
// Lock or unlock position
// Has to be done before _updateTrajDurations()
checkPositionLock(velocity_target);
// Update durations
_smoothing.updateDurations(velocity_target);
}
void ManualVelocitySmoothingZ::updateTrajectories(float dt)
{
_smoothing.updateTraj(dt);
_state.j = _smoothing.getCurrentJerk();
_state.a = _smoothing.getCurrentAcceleration();
_state.v = _smoothing.getCurrentVelocity();
_state.x = _smoothing.getCurrentPosition();
}
void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target)
{
if (velocity_target < 0.f) { // up
_smoothing.setMaxAccel(_max_accel_up);
_smoothing.setMaxVel(_max_vel_up);
} else { // down
_smoothing.setMaxAccel(_max_accel_down);
_smoothing.setMaxVel(_max_vel_down);
}
}
void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target)
{
/**
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
* is continuous. We know that the output of the position loop (part of the velocity setpoint)
* will suddenly become null
* and only the feedforward (generated by this flight task) will remain.
* This is why the previous input of the velocity controller
* is used to set current velocity of the trajectory.
*/
if (fabsf(_state.v) < 0.1f &&
fabsf(_state.a) < .2f &&
fabsf(velocity_target) <= FLT_EPSILON) {
// Lock position
_position_lock_active = true;
_position_setpoint_locked = _state.x;
} else {
// Unlock position
if (_position_lock_active) {
// Start the trajectory at the current velocity setpoint
_smoothing.setCurrentVelocity(_velocity_setpoint_feedback);
_position_setpoint_locked = NAN;
}
_position_lock_active = false;
_smoothing.setCurrentPosition(_position_estimate);
}
}

112
src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp
Unescape View File

@ -0,0 +1,112 @@ @@ -0,0 +1,112 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ManualVelocitySmoothingZ.hpp
*
*/
#pragma once
#include "VelocitySmoothing.hpp"
class ManualVelocitySmoothingZ
{
public:
ManualVelocitySmoothingZ() = default;
virtual ~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 setMaxAccelUp(const float max_accel_up) {
_max_accel_up = max_accel_up;
}
void setMaxAccelDown(const float max_accel_down) {
_max_accel_down = max_accel_down;
}
void setMaxVelUp(const float max_vel_up) {
_max_vel_up = max_vel_up;
}
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) {
_state.v = vel;
_smoothing.setCurrentVelocity(vel);
}
float getCurrentVelocity() const { return _state.v; }
void setCurrentPosition(const float pos) {
_state.x = pos;
_smoothing.setCurrentPosition(pos);
}
float getCurrentPosition() const { return _position_setpoint_locked; }
void setCurrentPositionEstimate(float pos) { _position_estimate = pos; }
private:
void resetPositionLock();
void updateTrajectories(float dt);
void updateTrajConstraints(float vel_target);
void checkPositionLock(float velocity_target);
void updateTrajDurations(float velocity_target);
VelocitySmoothing _smoothing; ///< Smoothing in z direction
bool _position_lock_active{false};
float _position_setpoint_locked;
float _velocity_setpoint_feedback;
float _position_estimate;
struct {
float j;
float a;
float v;
float x;
} _state;
float _max_accel_up{0.f};
float _max_accel_down{0.f};
float _max_vel_up{0.f};
float _max_vel_down{0.f};
};
Write Preview
Loading…
Cancel
Save