PX4 Firmware
PX4 Autopilot Software http://px4.io
ManualSmoothingZ.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 spec{fic 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 ManualSmoothingZ.cpp
36  */
37 
38 #include "ManualSmoothingZ.hpp"
39 #include <mathlib/mathlib.h>
40 #include <float.h>
41 
42 ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick) :
43  ModuleParams(parent),
44  _vel(vel), _stick(stick), _vel_sp_prev(vel)
45 {
46  _acc_state_dependent = _param_mpc_acc_up_max.get();
47  _max_acceleration = _param_mpc_acc_up_max.get();
48 }
49 
50 void
51 ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt)
52 {
53  updateAcceleration(vel_sp, dt);
54  velocitySlewRate(vel_sp, dt);
55  _vel_sp_prev = vel_sp;
56 }
57 
58 void
59 ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
60 {
61  // check for max acceleration
63 
64  // check if zero input stick
65  const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
66 
67  // default is acceleration
69 
70  // check zero input stick
71  if (is_current_zero) {
72  intention = ManualIntentionZ::brake;
73  }
74 
75 
76  // update intention
77  if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
78 
79  // we start with lowest acceleration
80  _acc_state_dependent = _param_mpc_acc_down_max.get();
81 
82  // reset slew-rate: this ensures that there
83  // is no delay present when user demands to brake
85 
86  }
87 
88  switch (intention) {
90 
91  // limit jerk when braking to zero
92  float jerk = (_param_mpc_acc_up_max.get() - _acc_state_dependent) / dt;
93 
94  if (jerk > _param_mpc_jerk_max.get()) {
95  _acc_state_dependent = _param_mpc_jerk_max.get() * dt + _acc_state_dependent;
96 
97  } else {
98  _acc_state_dependent = _param_mpc_acc_up_max.get();
99  }
100 
101  break;
102  }
103 
105 
106  _acc_state_dependent = (getMaxAcceleration() - _param_mpc_acc_down_max.get())
107  * fabsf(_stick) + _param_mpc_acc_down_max.get();
108  break;
109  }
110  }
111 
112  _intention = intention;
113 }
114 
115 void
117 {
118  if (_stick < -FLT_EPSILON) {
119  // accelerating upward
120  _max_acceleration = _param_mpc_acc_up_max.get();
121 
122  } else if (_stick > FLT_EPSILON) {
123  // accelerating downward
124  _max_acceleration = _param_mpc_acc_down_max.get();
125 
126  } else {
127 
128  // want to brake
129  if (fabsf(_vel_sp_prev) < FLT_EPSILON) {
130  // at rest
131  _max_acceleration = _param_mpc_acc_up_max.get();
132 
133  } else if (_vel_sp_prev < 0.0f) {
134  // braking downward
135  _max_acceleration = _param_mpc_acc_down_max.get();
136 
137  } else {
138  // braking upward
139  _max_acceleration = _param_mpc_acc_up_max.get();
140  }
141  }
142 }
143 
144 void
145 ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt)
146 {
147  // limit vertical acceleration
148  float acc = 0.f;
149 
150  if (dt > FLT_EPSILON) {
151  acc = (vel_sp - _vel_sp_prev) / dt;
152  }
153 
154  float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent;
155 
156  if (fabsf(acc) > fabsf(max_acc)) {
157  vel_sp = max_acc * dt + _vel_sp_prev;
158  }
159 }
float _vel_sp_prev
previous velocity setpoint
float _max_acceleration
can be up or down maximum acceleration
void setMaxAcceleration()
Set maximum acceleration.
void velocitySlewRate(float &vel_sp, const float dt)
Add delay to velocity setpoint change.
void smoothVelFromSticks(float &vel_sp, const float dt)
Smooth velocity setpoint based on flight direction.
#define FLT_EPSILON
const float & _vel
vehicle velocity (dependency injection)
float _acc_state_dependent
acceleration that depends on _intention
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
const float & _stick
stick input (dependency injection)
ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick)
ManualIntentionZ _intention
User intention.
float dt
Definition: px4io.c:73
float getMaxAcceleration()
Get max accleration.
void updateAcceleration(float &vel_sp, const float dt)
Computes the velocity setpoint change limit.
ManualIntentionZ
User-intention.