PX4 Firmware
PX4 Autopilot Software http://px4.io
test_smooth_z.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012-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 
34 /**
35  * @file test_smooth_z.cpp
36  * Tests flight path smoothing algorithms.
37  */
38 
39 #include <unit_test.h>
40 #include <lib/flight_tasks/tasks/Utility/ManualSmoothingZ.hpp>
41 #include <float.h>
42 #include <math.h>
43 
44 class SmoothZTest : public UnitTest
45 {
46 public:
47  virtual bool run_tests();
48 
49  bool brakeUpward();
50  bool brakeDownward();
51  bool accelerateUpwardFromBrake();
52  bool accelerateDownwardFromBrake();
53 
54 };
55 
56 bool SmoothZTest::run_tests()
57 {
58  ut_run_test(brakeUpward);
59  ut_run_test(brakeDownward);
60  ut_run_test(accelerateUpwardFromBrake);
61  ut_run_test(accelerateDownwardFromBrake);
62 
63  return (_tests_failed == 0);
64 }
65 
66 bool SmoothZTest::brakeUpward()
67 {
68  /* Downward flight and want to stop */
69  float stick_current = 0.0f; // sticks are at zero position
70  float vel_sp_current = 0.0f; // desired velocity is at 0m/s
71  float vel_sp_previous = 5.0f; // the demanded previous setpoint was 5m/s downwards
72  float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
73  float acc_max_up = 5.0f;
74  float acc_max_down = 2.0f;
75 
76  ManualSmoothingZ smooth(nullptr, vel, stick_current);
77 
78  /* overwrite parameters since they might change depending on configuration */
79  smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
80  smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
81  smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
82 
83  float dt = 0.1f; // dt is set to 0.1s
84 
85  /* It should start with acceleration */
86  ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
87 
88  for (int i = 0; i < 100; i++) {
89 
90  smooth.smoothVelFromSticks(vel_sp_current, dt);
91 
92  /* Test if intention is brake */
93  ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);
94 
95  /* we should always use upward acceleration */
96  ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON));
97 
98  /* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity
99  * occurs once the vehicle is at perfect rest. */
100  ut_assert_true((vel_sp_current < vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));
101 
102 
103  /* We reset the previou setpoint to newest setpoint
104  * and set the current setpoint to 0 because we still want to brake.
105  * We also set vel to previous setpoint where we make the assumption that
106  * the vehicle can perfectly track the setpoints.
107  */
108  vel_sp_previous = vel_sp_current;
109  vel_sp_current = 0.0f;
110  vel = vel_sp_previous;
111 
112 
113  }
114 
115  return true;
116 }
117 
118 bool SmoothZTest::brakeDownward()
119 {
120  /* Downward flight and want to stop */
121  float stick_current = 0.0f; // sticks are at zero position
122  float vel_sp_current = 0.0f; // desired velocity is 0m/s
123  float vel_sp_previous = -5.0f; // the demanded previous setpoint was -5m/s downwards
124  float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
125  float acc_max_up = 5.0f;
126  float acc_max_down = 2.0f;
127 
128  ManualSmoothingZ smooth(nullptr, vel, stick_current);
129 
130  /* overwrite parameters since they might change depending on configuration */
131  smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
132  smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
133  smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
134 
135  float dt = 0.1f; // dt is set to 0.1s
136 
137  /* It should start with acceleration */
138  ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
139 
140  for (int i = 0; i < 100; i++) {
141 
142  smooth.smoothVelFromSticks(vel_sp_current, dt);
143 
144  /* Test if intention is brake */
145  ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);
146 
147  /* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity
148  * occurs once the vehicle is at perfect rest. */
149  ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));
150 
151  /* we should always use downward acceleration except when vehicle is at rest*/
152  if (fabsf(vel_sp_previous) < FLT_EPSILON) {
153  ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON);
154 
155  } else {
156  ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON);
157  }
158 
159 
160  /* We reset the previou setpoint to newest setpoint
161  * and set the current setpoint to 0 because we still want to brake.
162  * We also set vel to previous setpoint where we make the assumption that
163  * the vehicle can perfectly track the setpoints.
164  */
165  vel_sp_previous = vel_sp_current;
166  vel_sp_current = 0.0f;
167  vel = vel_sp_previous;
168 
169  }
170 
171  return true;
172 }
173 
174 bool SmoothZTest::accelerateUpwardFromBrake()
175 {
176  /* Downward flight and want to stop */
177  float stick_current = -1.0f; // sticks are at full upward position
178  float vel_sp_target = -5.0f; // desired velocity is at -5m/s
179  float vel_sp_current = vel_sp_target;
180  float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
181  float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
182  float acc_max_up = 5.0f;
183  float acc_max_down = 2.0f;
184 
185  ManualSmoothingZ smooth(nullptr, vel, stick_current);
186 
187  /* overwrite parameters since they might change depending on configuration */
188  smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
189  smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
190  smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
191 
192  float dt = 0.1f; // dt is set to 0.1s
193 
194  for (int i = 0; i < 100; i++) {
195 
196  smooth.smoothVelFromSticks(vel_sp_current, dt);
197 
198  /* Test if intention is acceleration */
199  ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
200 
201  /* we should always use upward acceleration */
202  ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON);
203 
204  /* New setpoint has to be larger than previous setpoint or equal to target velocity
205  * vel_sp_current. The negative sign is because of NED frame.
206  */
207  ut_assert_true((-vel_sp_current > -vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_previous) < FLT_EPSILON));
208 
209 
210  /* We reset the previous setpoint to newest setpoint and reset the current setpoint.
211  * We also set the current velocity to the previous setpoint with the assumption that
212  * the vehicle does perfect tracking.
213  */
214  vel_sp_previous = vel_sp_current;
215  vel_sp_current = vel_sp_target;
216  vel = vel_sp_previous;
217 
218  }
219 
220  return true;
221 }
222 
223 bool SmoothZTest::accelerateDownwardFromBrake()
224 {
225  /* Downward flight and want to stop */
226  float stick_current = 1.0f; // sticks are at full downward position
227  float vel_sp_target = 5.0f; // desired velocity is at 5m/s
228  float vel_sp_current = vel_sp_target;
229  float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
230  float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
231  float acc_max_up = 5.0f;
232  float acc_max_down = 2.0f;
233 
234  ManualSmoothingZ smooth(nullptr, vel, stick_current);
235 
236  /* overwrite parameters since they might change depending on configuration */
237  smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
238  smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
239  smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
240 
241  float dt = 0.1f; // dt is set to 0.1s
242 
243  for (int i = 0; i < 100; i++) {
244 
245  smooth.smoothVelFromSticks(vel_sp_current, dt);
246 
247  /* Test if intention is acceleration */
248  ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
249 
250  /* we should always use downward acceleration except when target velocity is reached */
251  if (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON) {
252  ut_assert_true(smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON);
253 
254  } else {
255  ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON);
256  }
257 
258  /* New setpoint has to be larger than previous setpoint or equal to target velocity
259  * vel_sp_current (NED frame).
260  */
261  ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON));
262 
263 
264  /* We reset the previous setpoint to newest setpoint and reset the current setpoint.
265  * We also set the current velocity to the previous setpoint with the assumption that
266  * the vehicle does perfect tracking.
267  */
268  vel_sp_previous = vel_sp_current;
269  vel_sp_current = vel_sp_target;
270  vel = vel_sp_previous;
271 
272  }
273 
274  return true;
275 }
276 
277 ut_declare_test_c(test_smooth_z, SmoothZTest)
SmoothZTest::accelerateDownwardFromBrake
bool accelerateDownwardFromBrake()
Definition: test_smooth_z.cpp:223
ut_declare_test_c
#define ut_declare_test_c(test_function, test_class)
Definition: unit_test.h:40
test_smooth_z
int test_smooth_z(int argc, char *argv[])
ManualSmoothingZ::smoothVelFromSticks
void smoothVelFromSticks(float &vel_sp, const float dt)
Smooth velocity setpoint based on flight direction.
Definition: ManualSmoothingZ.cpp:51
UnitTest
Base class to be used for unit tests.
Definition: unit_test.h:54
FLT_EPSILON
#define FLT_EPSILON
Definition: stdlib_imports.hpp:26
UnitTest::_tests_failed
int _tests_failed
The number of unit tests which failed.
Definition: unit_test.h:206
SmoothZTest::run_tests
virtual bool run_tests()
Override to run your unit tests.
Definition: test_smooth_z.cpp:56
ManualSmoothingZ::overwriteAccelerationDown
void overwriteAccelerationDown(float acc_max_down)
Definition: ManualSmoothingZ.hpp:86
ManualSmoothingZ::overwriteAccelerationUp
void overwriteAccelerationUp(float acc_max_up)
Definition: ManualSmoothingZ.hpp:85
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
ut_assert_true
#define ut_assert_true(test)
To assert specifically to true.
Definition: unit_test.h:127
SmoothZTest::brakeUpward
bool brakeUpward()
Definition: test_smooth_z.cpp:66
ManualIntentionZ::brake
unit_test.h
ManualSmoothingZ::overwriteJerkMax
void overwriteJerkMax(float jerk_max)
Definition: ManualSmoothingZ.hpp:87
ManualSmoothingZ.hpp
ManualSmoothingZ::getIntention
ManualIntentionZ getIntention()
Get user intention.
Definition: ManualSmoothingZ.hpp:79
dt
float dt
Definition: px4io.c:73
ManualSmoothingZ::getMaxAcceleration
float getMaxAcceleration()
Get max accleration.
Definition: ManualSmoothingZ.hpp:73
SmoothZTest::accelerateUpwardFromBrake
bool accelerateUpwardFromBrake()
Definition: test_smooth_z.cpp:174
ut_run_test
#define ut_run_test(test)
Runs a single unit test.
Definition: unit_test.h:96
SmoothZTest::brakeDownward
bool brakeDownward()
Definition: test_smooth_z.cpp:118
ManualIntentionZ::acceleration