PX4 Firmware
PX4 Autopilot Software http://px4.io
ManualVelocitySmoothingXY.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
35 
36 #include <mathlib/mathlib.h>
37 #include <float.h>
38 
39 using namespace matrix;
40 
41 void ManualVelocitySmoothingXY::reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos)
42 {
43  for (int i = 0; i < 2; i++) {
44  _trajectory[i].reset(accel(i), vel(i), pos(i));
45  }
46 
47  resetPositionLock();
48 }
49 
51 {
52  _position_lock_active = false;
53  _position_setpoint_locked(0) = NAN;
54  _position_setpoint_locked(1) = NAN;
55 }
56 
57 void ManualVelocitySmoothingXY::update(float dt, const Vector2f &velocity_target)
58 {
59  // Update state
60  updateTrajectories(dt);
61 
62  // Lock or unlock position
63  // Has to be done before _updateTrajDurations()
64  checkPositionLock(velocity_target);
65 
66  // Update durations and sync XY
67  updateTrajDurations(velocity_target);
68 }
69 
71 {
72  for (int i = 0; i < 2; ++i) {
73  _trajectory[i].updateTraj(dt);
74 
75  _state.j(i) = _trajectory[i].getCurrentJerk();
76  _state.a(i) = _trajectory[i].getCurrentAcceleration();
77  _state.v(i) = _trajectory[i].getCurrentVelocity();
78  _state.x(i) = _trajectory[i].getCurrentPosition();
79  }
80 }
81 
83 {
84  for (int i = 0; i < 2; ++i) {
85  _trajectory[i].updateDurations(velocity_target(i));
86  }
87 
89 }
90 
92 {
93  /**
94  * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
95  * is continuous. We know that the output of the position loop (part of the velocity setpoint)
96  * will suddenly become null
97  * and only the feedforward (generated by this flight task) will remain.
98  * This is why the previous input of the velocity controller
99  * is used to set current velocity of the trajectory.
100  */
101  if (_state.v.length() < 0.1f &&
102  _state.a.length() < .2f &&
103  velocity_target.length() <= FLT_EPSILON) {
104  // Lock position
105  _position_lock_active = true;
106  _position_setpoint_locked = _state.x;
107 
108  } else {
109  // Unlock position
110  if (_position_lock_active) {
111  // Start the trajectory at the current velocity setpoint
112  _trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
113  _trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
114  _state.v = _velocity_setpoint_feedback;
115  resetPositionLock();
116  }
117 
118  _trajectory[0].setCurrentPosition(_position_estimate(0));
119  _trajectory[1].setCurrentPosition(_position_estimate(1));
120  }
121 }
void checkPositionLock(const Vector2f &velocity_target)
void update(float dt, const Vector2f &velocity_target)
#define FLT_EPSILON
Type length() const
Definition: Vector.hpp:83
void updateTrajDurations(const Vector2f &velocity_target)
float dt
Definition: px4io.c:73
static void timeSynchronization(VelocitySmoothing *traj, int n_traj)
Synchronize several trajectories to have the same total time.
void reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos)