PX4 Firmware
PX4 Autopilot Software http://px4.io
ManualSmoothingXY.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 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 
34 /**
35  * @file ManualSmoothingXY.cpp
36  */
37 
38 #include "ManualSmoothingXY.hpp"
39 #include <mathlib/mathlib.h>
40 #include <float.h>
41 #include <px4_platform_common/defines.h>
42 
43 using namespace matrix;
44 
45 ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const Vector2f &vel) :
46  ModuleParams(parent), _vel_sp_prev(vel)
47 
48 {
49  _acc_state_dependent = _param_mpc_acc_hor.get();
50  _jerk_state_dependent = _param_mpc_jerk_max.get();
51 }
52 
53 void
54 ManualSmoothingXY::smoothVelocity(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
55  const float &yawrate_sp, const float dt)
56 {
57  _updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
58  _velocitySlewRate(vel_sp, dt);
59  _vel_sp_prev = vel_sp;
60 }
61 
62 void
63 ManualSmoothingXY::_updateAcceleration(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
64  const float &yawrate_sp, const float dt)
65 {
66  Intention intention = _getIntention(vel_sp, vel, yaw, yawrate_sp);
67 
68  // Adapt acceleration and jerk based on current state and
69  // intention. Jerk is only used for braking.
70  _setStateAcceleration(vel_sp, vel, intention, dt);
71 }
72 
74 ManualSmoothingXY::_getIntention(const Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
75  const float &yawrate_sp)
76 {
77 
78  if (vel_sp.length() > FLT_EPSILON) {
79  // Distinguish between acceleration, deceleration and direction change
80 
81  // Check if stick direction and current velocity are within 135.
82  // If current setpoint and velocity are more than 135 apart, we assume
83  // that the user demanded a direction change.
84  // The detection is done in body frame.
85  // Rotate velocity setpoint into body frame
86  Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
87  Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
88 
89  if (vel_sp_heading.length() > FLT_EPSILON) {
90  vel_sp_heading.normalize();
91  }
92 
93  if (vel_heading.length() > FLT_EPSILON) {
94  vel_heading.normalize();
95  }
96 
97  const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f;
98 
99  // In almost all cases we want to use acceleration.
100  // Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
101  // Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand.
102  bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
103  bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_max) && !yawspeed_demand
104  && (vel.length() > 2.0f);
105  bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
106 
107  if (direction_change) {
109 
110  } else if (deceleration) {
112 
113  } else {
115  }
116  }
117 
118  return Intention::brake; //default is brake
119 }
120 
121 void
123  const Intention &intention, const float dt)
124 {
125  switch (intention) {
126  case Intention::brake: {
127 
129  // Vehicle switched from direction change to brake.
130  // Don't do any slwerate and just stop.
131  _jerk_state_dependent = 1e6f;
132  _vel_sp_prev = vel;
133 
134  } else if (intention != _intention) {
135  // start the brake with lowest acceleration which
136  // makes stopping smoother
137  _acc_state_dependent = _param_mpc_dec_hor_slow.get();
138 
139  // Adjust jerk based on current velocity. This ensures
140  // that the vehicle will stop much quicker at large speed but
141  // very slow at low speed.
142  _jerk_state_dependent = 1e6f; // default
143 
144  if (_param_mpc_jerk_max.get() > _param_mpc_jerk_min.get() && _param_mpc_jerk_min.get() > FLT_EPSILON) {
145 
146  _jerk_state_dependent = math::min((_param_mpc_jerk_max.get() - _param_mpc_jerk_min.get())
147  / _vel_max * vel.length() + _param_mpc_jerk_min.get(), _param_mpc_jerk_max.get());
148  }
149 
150  // User wants to brake smoothly but does NOT want the vehicle to
151  // continue to fly in the opposite direction. slewrate has to be reset
152  // by setting previous velocity setpoint to current velocity. */
153  _vel_sp_prev = vel;
154  }
155 
156  /* limit jerk when braking to zero */
157  float jerk = (_param_mpc_acc_hor_max.get() - _acc_state_dependent) / dt;
158 
159  if (jerk > _jerk_state_dependent) {
162 
163  } else {
164  _acc_state_dependent = _param_mpc_acc_hor_max.get();
165  }
166 
167  break;
168  }
169 
171  // We allow for fast change by setting previous setpoint to current setpoint.
172 
173 
174  // Because previous setpoint is equal to current setpoint,
175  // slewrate will have no effect. Nonetheless, just set
176  // acceleration to maximum.
177  _acc_state_dependent = _param_mpc_acc_hor_max.get();
178  break;
179  }
180 
182  // Limit acceleration linearly based on velocity setpoint.
183  _acc_state_dependent = (_param_mpc_acc_hor.get() - _param_mpc_dec_hor_slow.get())
184  / _vel_max * vel_sp.length() + _param_mpc_dec_hor_slow.get();
185  break;
186  }
187 
189  _acc_state_dependent = _param_mpc_dec_hor_slow.get();
190  break;
191  }
192  }
193 
194  // Update intention for next iteration.
195  _intention = intention;
196 }
197 
198 void
200 {
201  // Adjust velocity setpoint if demand exceeds acceleration. /
202  Vector2f acc{};
203 
204  if (dt > FLT_EPSILON) {
205  acc = (vel_sp - _vel_sp_prev) / dt;
206  }
207 
208  if (acc.length() > _acc_state_dependent) {
209  vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev;
210  }
211 }
212 
213 Vector2f
215 {
216  Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
217  Vector3f vec_heading = q_yaw.conjugate_inversed(Vector3f(vec(0), vec(1), 0.0f));
218  return Vector2f(vec_heading);
219 }
220 
221 Vector2f
223 {
224  Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
225  Vector3f vec_world = q_yaw.conjugate(Vector3f(vec(0), vec(1), 0.0f));
226  return Vector2f(vec_world);
227 }
matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw)
Rotate vector from local frame into heading frame.
ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel)
void normalize()
Definition: Vector.hpp:87
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Definition: Quaternion.hpp:398
float _acc_state_dependent
velocity change limit that depends on Intention
#define FLT_EPSILON
Quaternion class.
Definition: Dcm.hpp:24
matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw)
Rotate vector from heading frame to local frame.
Vector normalized() const
Definition: Vector.hpp:103
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector2< float > Vector2f
Definition: Vector2.hpp:69
Type length() const
Definition: Vector.hpp:83
float _jerk_state_dependent
acceleration change limit during brake
Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp)
Gets user intention.
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
matrix::Vector2f _vel_sp_prev
previous velocity setpoint
void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp, const float dt)
Sets velocity change limits (=acceleration).
Vector3< float > Vector3f
Definition: Vector3.hpp:136
float _vel_max
maximum horizontal speed allowed
float dt
Definition: px4io.c:73
void smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp, const float dt)
Smoothing of velocity setpoint horizontally based on flight direction.
AxisAngle< float > AxisAnglef
Definition: AxisAngle.hpp:160
void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
Limits the velocity setpoint change.
Intention _intention
user intention
void _setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, const float dt)
Set acceleration depending on Intention.
Vector3< Type > conjugate_inversed(const Vector3< Type > &vec) const
Rotates vector v_2 in frame 2 to vector v_1 in frame 1 using the rotation quaternion q_21 describing ...
Definition: Quaternion.hpp:414