test_vtol_dynamics.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2022-2023 RaccoonLab.
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation, version 3.
7  *
8  * This program is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
17  */
18 
19 
20 #include <gtest/gtest.h>
21 #include <iostream>
22 #include <Eigen/Geometry>
23 #include <random>
24 #include "vtolDynamicsSim.hpp"
25 #include "common_math.hpp"
26 
27 
28 TEST(VtolDynamics, calculateWind){
29  VtolDynamics vtolDynamicsSim;
30  auto wind_mean_velocity = Eigen::Vector3d(0, 10, 0);
31  double wind_variance = 0.0;
32  vtolDynamicsSim.setWindParameter(wind_mean_velocity, wind_variance);
33  Eigen::Vector3d expected_wind = Eigen::Vector3d(0, 10, 0);
34 
35  Eigen::Vector3d actual_wind = vtolDynamicsSim.calculateWind();
36  ASSERT_EQ(actual_wind, expected_wind);
37 }
38 
39 TEST(VtolDynamics, calculateAnglesOfAtack){
40  VtolDynamics vtolDynamicsSim;
41  Eigen::Vector3d airSpeed;
42  double result, expected;
43 
44  std::vector<std::pair<Eigen::Vector3d, double>> dataset;
45  dataset.push_back((std::make_pair(Eigen::Vector3d(0, 0, 0), 0.0)));
46 
47  dataset.push_back((std::make_pair(Eigen::Vector3d(10, 1, 1), 0.099669)));
48  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 10, 1), 0.785398)));
49  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 1, 10), 1.471128)));
50  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 2, 3), 1.2490)));
51 
52  dataset.push_back((std::make_pair(Eigen::Vector3d(-10, 1, 1), 3.041924)));
53  dataset.push_back((std::make_pair(Eigen::Vector3d(-1, 10, 1), 2.356194)));
54  dataset.push_back((std::make_pair(Eigen::Vector3d(-1, 1, 10), 1.670465)));
55  dataset.push_back((std::make_pair(Eigen::Vector3d(-1, 2, 3), 1.892547)));
56 
57  dataset.push_back((std::make_pair(Eigen::Vector3d(10, 1, -1), -0.099669)));
58  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 10, -1), -0.785398)));
59  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 1, -10), -1.471128)));
60  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 2, -3), -1.249046)));
61 
62  for(auto pair : dataset){
63  result = vtolDynamicsSim.calculateAnglesOfAtack(pair.first);
64  EXPECT_NEAR(result, pair.second, 0.0001);
65  }
66 }
67 
68 TEST(VtolDynamics, calculateAnglesOfSideslip){
69  VtolDynamics vtolDynamicsSim;
70  Eigen::Vector3d airSpeed;
71  double result, expected;
72 
73  std::vector<std::pair<Eigen::Vector3d, double>> dataset;
74  dataset.push_back((std::make_pair(Eigen::Vector3d(0, 0, 0), 0.0)));
75 
76  dataset.push_back((std::make_pair(Eigen::Vector3d(10, 1, 1), 0.099177)));
77  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 10, 1), 1.430307)));
78  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 1, 10), 0.099177)));
79  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 2, 3), 0.563943)));
80 
81  dataset.push_back((std::make_pair(Eigen::Vector3d(10, -1, 1), -0.099177)));
82  dataset.push_back((std::make_pair(Eigen::Vector3d(1, -10, 1), -1.430307)));
83  dataset.push_back((std::make_pair(Eigen::Vector3d(1, -1, 10), -0.099177)));
84  dataset.push_back((std::make_pair(Eigen::Vector3d(1, -2, 3), -0.563943)));
85 
86  dataset.push_back((std::make_pair(Eigen::Vector3d(10, 1, -1), 0.099177)));
87  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 10, -1), 1.430307)));
88  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 1, -10), 0.099177)));
89  dataset.push_back((std::make_pair(Eigen::Vector3d(1, 2, -3), 0.563943)));
90 
91  for(auto pair : dataset){
92  result = vtolDynamicsSim.calculateAnglesOfSideslip(pair.first);
93  EXPECT_NEAR(result, pair.second, 0.001);
94  }
95 }
96 
97 TEST(CommonMath, findPrevRowIdxInIncreasingSequence){
98  VtolDynamics vtolDynamicsSim;
99  Eigen::MatrixXd table(8, 1);
100  table << 5, 10, 15, 20, 25, 30, 35, 40;
101  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, -1), 0);
102  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 10), 0);
103  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 10.1), 1);
104  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 15.1), 2);
105  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 34.9), 5);
106  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 35.1), 6);
107  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 39.9), 6);
108  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 40.1), 6);
109  ASSERT_EQ(Math::findPrevRowIdxInIncreasingSequence(table, 50.0), 6);
110 }
111 
112 TEST(CommonMath, findPrevRowIdxInMonotonicSequenceIncreasing){
113  VtolDynamics vtolDynamicsSim;
114  Eigen::MatrixXd table(8, 1);
115  table << 5, 10, 15, 20, 25, 30, 35, 40;
116  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, -1), 0);
117  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 10), 0);
118  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 10.1), 1);
119  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 15.1), 2);
120  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 34.9), 5);
121  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 35.1), 6);
122  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 39.9), 6);
123  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 40.1), 6);
124  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 50.0), 6);
125 }
126 
127 TEST(CommonMath, findPrevRowIdxInMonotonicSequenceDecreasing){
128  VtolDynamics vtolDynamicsSim;
129  Eigen::MatrixXd table(8, 1);
130  table << 40, 35, 30, 25, 20, 15, 10, 5;
131  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, -1), 6);
132  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 10), 5);
133  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 10.1), 5);
134  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 15.1), 4);
135  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 34.9), 1);
136  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 35.1), 0);
137  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 39.9), 0);
138  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 40.1), 0);
139  ASSERT_EQ(Math::findPrevRowIdxInMonotonicSequence(table, 50.0), 0);
140 }
141 
142 TEST(calculateCLPolynomial, test_normal_scalar){
143  VtolDynamics vtolDynamicsSim;
144 
145  Eigen::MatrixXd table(2, 2);
146  table << 0, 0,
147  1, 1;
148  double airSpeedMod = 0.5;
149  Eigen::VectorXd polynomialCoeffs(1);
150 
151  ASSERT_TRUE(Math::calculatePolynomial(table, airSpeedMod, polynomialCoeffs));
152  ASSERT_EQ(polynomialCoeffs[0], airSpeedMod);
153 }
154 
155 TEST(calculateCLPolynomial, test_normal_vector){
156  VtolDynamics vtolDynamicsSim;
157  Eigen::VectorXd polynomialCoeffs(2);
158 
159  Eigen::MatrixXd table(2, 3);
160  table << 0, 0, 1,
161  1, 1, 2;
162  double airSpeedMod = 0.5;
163 
164  Eigen::VectorXd expectedPolynomialCoeffs(2);
165  expectedPolynomialCoeffs << 0.5, 1.5;
166 
167  ASSERT_TRUE(Math::calculatePolynomial(table, airSpeedMod, polynomialCoeffs));
168  ASSERT_EQ(polynomialCoeffs[0], expectedPolynomialCoeffs[0]);
169  ASSERT_EQ(polynomialCoeffs[1], expectedPolynomialCoeffs[1]);
170 }
171 
172 TEST(calculateCLPolynomial, test_wrong_input_size){
173  VtolDynamics vtolDynamicsSim;
174 
175  Eigen::MatrixXd table(1, 2);
176  table << 0, 0;
177  double airSpeedMod = 0.5;
178  Eigen::VectorXd polynomialCoeffs(1);
179 
180  ASSERT_FALSE(Math::calculatePolynomial(table, airSpeedMod, polynomialCoeffs));
181 }
182 
183 TEST(calculateCLPolynomial, test_wrong_table){
184  VtolDynamics vtolDynamicsSim;
185 
186  Eigen::MatrixXd table(2, 2);
187  table << 0, 0,
188  0, 0;
189  double airSpeedMod = 0.5;
190  Eigen::VectorXd polynomialCoeffs(1);
191 
192  ASSERT_FALSE(Math::calculatePolynomial(table, airSpeedMod, polynomialCoeffs));
193 }
194 
195 TEST(VtolDynamics, calculateCLPolynomial){
196  VtolDynamics vtolDynamicsSim;
197  ASSERT_EQ(vtolDynamicsSim.init(), 0);
198  Eigen::VectorXd calculatedpolynomialCoeffs(7);
199  Eigen::VectorXd expectedPolynomialCoeffs(7);
200  Eigen::VectorXd diff(7);
201  double airSpeedMod;
202 
203  airSpeedMod = 10;
204  expectedPolynomialCoeffs << -3.9340e-11, 8.2040e-09, 1.9350e-07, -3.0750e-05,
205  -4.2090e-04, 0.055200, 0.44380;
206  vtolDynamicsSim.calculateCLPolynomial(airSpeedMod, calculatedpolynomialCoeffs);
207  for(size_t idx = 0; idx < expectedPolynomialCoeffs.size(); idx++){
208  EXPECT_NEAR(calculatedpolynomialCoeffs[idx], expectedPolynomialCoeffs[idx], 0.00001);
209  }
210 
211  airSpeedMod = 0;
212  expectedPolynomialCoeffs << -1.5820e-11, 8.0740e-09, 9.4100e-08, -3.1150e-05,
213  -2.8150e-04, 0.055940, 0.38260;
214  vtolDynamicsSim.calculateCLPolynomial(airSpeedMod, calculatedpolynomialCoeffs);
215  for(size_t idx = 0; idx < expectedPolynomialCoeffs.size(); idx++){
216  EXPECT_NEAR(calculatedpolynomialCoeffs[idx], expectedPolynomialCoeffs[idx], 0.00001);
217  }
218 
219  airSpeedMod = -10;
220  expectedPolynomialCoeffs << 7.7000e-12, 7.9440e-09, -5.3000e-09, -3.1550e-05,
221  -1.4210e-04, 0.056680, 0.32140;
222  vtolDynamicsSim.calculateCLPolynomial(airSpeedMod, calculatedpolynomialCoeffs);
223  for(size_t idx = 0; idx < expectedPolynomialCoeffs.size(); idx++){
224  EXPECT_NEAR(calculatedpolynomialCoeffs[idx], expectedPolynomialCoeffs[idx], 0.00001);
225  }
226 
227  airSpeedMod = 45;
228  expectedPolynomialCoeffs << -5.9110e-11, 7.8790e-09, 2.5740e-07, -2.9610e-05,
229  -4.8380e-04, 0.054580, 0.46370;
230  vtolDynamicsSim.calculateCLPolynomial(airSpeedMod, calculatedpolynomialCoeffs);
231  for(size_t idx = 0; idx < expectedPolynomialCoeffs.size(); idx++){
232  EXPECT_NEAR(calculatedpolynomialCoeffs[idx], expectedPolynomialCoeffs[idx], 0.00001);
233  }
234 }
235 
236 TEST(VtolDynamics, calculateCSPolynomial){
237  VtolDynamics vtolDynamicsSim;
238 
239  Eigen::VectorXd calculatedpolynomialCoeffs(7);
240  double airSpeedMod = 5;
241 
242  vtolDynamicsSim.calculateCSPolynomial(airSpeedMod, calculatedpolynomialCoeffs);
243 }
244 
245 TEST(VtolDynamics, calculateCDPolynomial){
246  VtolDynamics vtolDynamicsSim;
247 
248  Eigen::VectorXd calculatedpolynomialCoeffs(7);
249  double airSpeedMod = 5;
250 
251  vtolDynamicsSim.calculateCDPolynomial(airSpeedMod, calculatedpolynomialCoeffs);
252  // @todo If build type is DEBUG, Eigen generates an assert
253 }
254 
255 TEST(CommonMath, polyval){
256  VtolDynamics vtolDynamicsSim;
257  Eigen::VectorXd poly(7);
258  poly << 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7;
259  double value = 0.5;
260  double expected_output = 3.1859;
261 
262  double actual_output = Math::polyval(poly, value);
263  EXPECT_NEAR(actual_output, expected_output, 0.001);
264 }
265 
266 TEST(VtolDynamics, griddata){
267  VtolDynamics vtolDynamicsSim;
268  ASSERT_EQ(vtolDynamicsSim.init(), 0);
269  Eigen::MatrixXd x(1, 3);
270  Eigen::MatrixXd y(1, 4);
271  Eigen::MatrixXd f(4, 3);
272  double x_value;
273  double y_value;
274  double actual_result;
275  double expected_result;
276 
277  x << 1, 2, 3;
278  y << 2, 3, 4, 5;
279  f << 2.5, 3.0, 3.5,
280  3.0, 3.5, 4.0,
281  3.5, 4.0, 4.5,
282  4.0, 4.5, 5.0;
283 
284  x_value = 2.25;
285  y_value = 3.75;
286  expected_result = 4.0;
287  actual_result = Math::griddata(x, y, f, x_value, y_value);
288  EXPECT_NEAR(actual_result, expected_result, 0.001);
289 
290  x_value = 1.1;
291  y_value = 4.75;
292  expected_result = 3.9250;
293  actual_result = Math::griddata(x, y, f, x_value, y_value);
294  EXPECT_NEAR(actual_result, expected_result, 0.001);
295 }
296 
297 TEST(VtolDynamics, calculateCSRudder){
298  VtolDynamics vtolDynamicsSim;
299  ASSERT_EQ(vtolDynamicsSim.init(), 0);
300 
301  struct DataSet{
302  double rudder_position;
303  double airspeed;
304  double expected;
305  };
306  std::vector<DataSet> data_set;
307  double result;
308 
309  data_set.push_back({.rudder_position = 0, .airspeed = 5, .expected = -1.5009e-04});
310  data_set.push_back({.rudder_position = 0, .airspeed = 5.1, .expected = -1.2303e-04});
311  data_set.push_back({.rudder_position = 0, .airspeed = 8.5, .expected = 5.9762e-04});
312  data_set.push_back({.rudder_position = 0, .airspeed = 8.66025, .expected = 6.0903e-04});
313  data_set.push_back({.rudder_position = 0, .airspeed = 10, .expected = 7.0445e-04});
314  data_set.push_back({.rudder_position = 0, .airspeed = 20, .expected = 9.2322e-04});
315  data_set.push_back({.rudder_position = 0, .airspeed = 40, .expected = -0.0013107});
316  data_set.push_back({.rudder_position = -20, .airspeed = 5, .expected = -0.034155});
317  data_set.push_back({.rudder_position = 0, .airspeed = 5, .expected = -1.5009e-04});
318  data_set.push_back({.rudder_position = 20, .airspeed = 5, .expected = 0.037053});
319 
320  for(auto test_case : data_set){
321  result = vtolDynamicsSim.calculateCSRudder(test_case.rudder_position, test_case.airspeed);
322  EXPECT_NEAR(result, test_case.expected, 0.001);
323  }
324 }
325 
326 TEST(VtolDynamics, calculateCSBeta){
327  VtolDynamics vtolDynamicsSim;
328  ASSERT_EQ(vtolDynamicsSim.init(), 0);
329 
330  struct DataSet{
331  double aos_degree;
332  double airspeed;
333  double expected;
334  };
335  std::vector<DataSet> data_set;
336  double result;
337 
338  data_set.push_back({.aos_degree = 0, .airspeed = 5, .expected = -0.0032540});
339  data_set.push_back({.aos_degree = 0, .airspeed = 10, .expected = -0.0040036});
340  data_set.push_back({.aos_degree = 0, .airspeed = 15, .expected = -0.0037597});
341  data_set.push_back({.aos_degree = 0, .airspeed = 20, .expected = -0.0033221});
342 
343 
344  for(auto test_case : data_set){
345  result = vtolDynamicsSim.calculateCSBeta(test_case.aos_degree, test_case.airspeed);
346  EXPECT_NEAR(result, test_case.expected, 0.0000001);
347  }
348 }
349 
350 TEST(VtolDynamics, DISABLED_calculateCmxAileron){
351  VtolDynamics vtolDynamicsSim;
352  ASSERT_EQ(vtolDynamicsSim.init(), 0);
353  double Cmx_aileron, airspeedNorm, aileron_pos, dynamicPressure;
354  double characteristicLength = 1.5;
355 
356  airspeedNorm = 20;
357  dynamicPressure = vtolDynamicsSim.calculateDynamicPressure(airspeedNorm);
358  for(aileron_pos = -2e1; aileron_pos <= 2e1; aileron_pos += 4){
359  Cmx_aileron = vtolDynamicsSim.calculateCmyElevator(aileron_pos, airspeedNorm);
360  Cmx_aileron *= 0.5 * dynamicPressure * characteristicLength;
361  std::cout << aileron_pos << " Cmx_aileron = " << Cmx_aileron << std::endl;
362  }
363 }
364 
365 TEST(VtolDynamics, calculateAerodynamics){
366  VtolDynamics vtolDynamicsSim;
367  ASSERT_EQ(vtolDynamicsSim.init(), 0);
368  Eigen::Vector3d Faero, Maero;
369 
370  Eigen::Vector3d airspeed(0.000001, -9.999999, 0.000001);
371  double AoA = 0.958191;
372  double AoS = -1.570796;
373  std::array<double, 3> servos{0.0, 0.0, 0.0};
374 
375  Eigen::Vector3d extectedFaero(-4.8133e-07, 2.9513e+01, -6.0493e-06);
376  Eigen::Vector3d extectedMaero(0.21470, 0.69480, -0.31633);
377 
378  vtolDynamicsSim.calculateAerodynamics(airspeed, AoA, AoS, servos, Faero, Maero);
379 
380  for(size_t idx = 0; idx < 3; idx++){
381  EXPECT_NEAR(Faero[idx], extectedFaero[idx], 0.001);
382  EXPECT_NEAR(Maero[idx], extectedMaero[idx], 0.001);
383  }
384 }
385 
386 TEST(VtolDynamics, calculateAerodynamicsCaseAileron){
387  VtolDynamics vtolDynamicsSim;
388  ASSERT_EQ(vtolDynamicsSim.init(), 0);
389  Eigen::Vector3d expectedResult, Faero, Maero;
390 
391  Eigen::Vector3d airspeed(5, 5, 5);
392  double AoA = 0.1;
393  double AoS = 0.1;
394  std::array<double, 3> servos{0.5, 0.0, 0.0};
395 
396  Eigen::Vector3d extectedFaero(7.4133, -4.3077, -6.6924);
397  Eigen::Vector3d extectedMaero(0.333818, 1.754507, -0.037038);
398 
399  vtolDynamicsSim.calculateAerodynamics(airspeed, AoA, AoS, servos, Faero, Maero);
400 
401  for(size_t idx = 0; idx < 3; idx++){
402  EXPECT_NEAR(Faero[idx], extectedFaero[idx], 0.02);
403  EXPECT_NEAR(Maero[idx], extectedMaero[idx], 0.02);
404  }
405 }
406 
407 TEST(VtolDynamics, calculateAerodynamicsCaseElevator){
408  VtolDynamics vtolDynamicsSim;
409  ASSERT_EQ(vtolDynamicsSim.init(), 0);
410  Eigen::Vector3d expectedResult, Faero, Maero;
411 
412  Eigen::Vector3d airspeed(5, 5, 5);
413  double AoA = 0.1;
414  double AoS = 0.1;
415  std::array<double, 3> servos{0.0, 5.0, 0.0};
416 
417  Eigen::Vector3d extectedFaero(7.4133, -4.3077, -6.6924);
418  Eigen::Vector3d extectedMaero(0.190243, 1.220935, -0.037038);
419 
420  vtolDynamicsSim.calculateAerodynamics(airspeed, AoA, AoS, servos, Faero, Maero);
421 
422  for(size_t idx = 0; idx < 3; idx++){
423  EXPECT_NEAR(Faero[idx], extectedFaero[idx], 0.02);
424  EXPECT_NEAR(Maero[idx], extectedMaero[idx], 0.02);
425  }
426 }
427 
428 TEST(VtolDynamics, calculateAerodynamicsAoA){
429  VtolDynamics vtolDynamicsSim;
430  ASSERT_EQ(vtolDynamicsSim.init(), 0);
431  Eigen::Vector3d diff, expectedResult, Faero, Maero;
432 
433  Eigen::Vector3d airspeed(5, 5, 5);
434  double AoA = 27.0 * 3.1415 / 180.0;
435  double AoS = 0;
436  std::array<double, 3> servos{0.0, 0.0, 0.0};
437 
438  Eigen::Vector3d extectedFaero(6.0625, -7.7260, -17.5536);
439  Eigen::Vector3d extectedMaero(0.16512, 1.26568, -0.11093);
440 
441  vtolDynamicsSim.calculateAerodynamics(airspeed, AoA, AoS, servos, Faero, Maero);
442 
443  for(size_t idx = 0; idx < 3; idx++){
444  EXPECT_NEAR(Faero[idx], extectedFaero[idx], 0.04);
445  EXPECT_NEAR(Maero[idx], extectedMaero[idx], 0.04);
446  }
447 }
448 
449 TEST(VtolDynamics, calculateAerodynamicsRealCase){
450  VtolDynamics vtolDynamicsSim;
451  ASSERT_EQ(vtolDynamicsSim.init(), 0);
452  Eigen::Vector3d expectedResult, Faero, Maero;
453 
454  Eigen::Vector3d airspeed(2.93128, 0.619653, 0.266774);
455  double AoA = 45 * 3.1415 / 180.0;
456  double AoS = 11.8888 * 3.1415 / 180.0;
457  std::array<double, 3> servos{0.0, 0.0, 0.0};
458 
459  Eigen::Vector3d extectedFaero(-2.28665, -0.92928, -2.66499);
460  Eigen::Vector3d extectedMaero(0.017652, 0.074924, -0.024468);
461 
462  vtolDynamicsSim.calculateAerodynamics(airspeed, AoA, AoS, servos, Faero, Maero);
463 
464  for(size_t idx = 0; idx < 3; idx++){
465  EXPECT_NEAR(Faero[idx], extectedFaero[idx], 0.04);
466  EXPECT_NEAR(Maero[idx], extectedMaero[idx], 0.04);
467  }
468 }
469 
470 TEST(thruster, thrusterFirstZeroCmd){
471  VtolDynamics vtolDynamicsSim;
472  ASSERT_EQ(vtolDynamicsSim.init(), 0);
473  double control,
474  actualThrust, actualTorque, actualRpm,
475  expectedThrust, expectedTorque, expectedRpm;
476 
477  control = 0;
478  expectedThrust = 0;
479  expectedTorque = 0;
480  expectedRpm = 0;
481  vtolDynamicsSim.thruster(control, actualThrust, actualTorque, actualRpm);
482  EXPECT_NEAR(actualThrust, expectedThrust, 0.001);
483  EXPECT_NEAR(actualTorque, expectedTorque, 0.00001);
484  EXPECT_NEAR(actualRpm, expectedRpm, 0.00001);
485 }
486 TEST(thruster, thrusterSecond){
487  VtolDynamics vtolDynamicsSim;
488  ASSERT_EQ(vtolDynamicsSim.init(), 0);
489  double control,
490  actualThrust, actualTorque, actualRpm,
491  expectedThrust, expectedTorque, expectedRpm;
492 
493  control = 134.254698;
494  expectedThrust = 3.590800;
495  expectedTorque = 0.013696;
496  expectedRpm = 732.298;
497  vtolDynamicsSim.thruster(control, actualThrust, actualTorque, actualRpm);
498  EXPECT_NEAR(actualThrust, expectedThrust, 0.0001);
499  EXPECT_NEAR(actualTorque, expectedTorque, 0.000001);
500  EXPECT_NEAR(actualRpm, expectedRpm, 0.001);
501 }
502 TEST(thruster, thrusterThird){
503  VtolDynamics vtolDynamicsSim;
504  ASSERT_EQ(vtolDynamicsSim.init(), 0);
505  double control,
506  actualThrust, actualTorque, actualRpm,
507  expectedThrust, expectedTorque, expectedRpm;
508 
509  control = 500.004648;
510  expectedThrust = 15.8930;
511  expectedTorque = 0.27273;
512  expectedRpm = 2727.3;
513  vtolDynamicsSim.thruster(control, actualThrust, actualTorque, actualRpm);
514  EXPECT_NEAR(actualThrust, expectedThrust, 0.001);
515  EXPECT_NEAR(actualTorque, expectedTorque, 0.00001);
516  EXPECT_NEAR(actualRpm, expectedRpm, 0.2);
517 }
518 
523 void calculateNewState(double dt,
524  std::vector<double> motors,
525  Eigen::Vector3d Maero,
526  Eigen::Vector3d Faero,
527  Eigen::Vector3d initialLinearVelocity,
528  Eigen::Vector3d initialAngularVelocity,
529  Eigen::Vector3d initialPosition,
530  Eigen::Quaterniond initialAttitude,
531  Eigen::Vector3d& expectedAngAccel,
532  Eigen::Vector3d& expectedLinAccel,
533  Eigen::Vector3d& angularAcceleration,
534  Eigen::Vector3d& linearAcceleration){
535  VtolDynamics vtolDynamicsSim;
536  ASSERT_EQ(vtolDynamicsSim.init(), 0);
537  vtolDynamicsSim.setInitialVelocity(initialLinearVelocity, initialAngularVelocity);
538  vtolDynamicsSim.setInitialPosition(initialPosition, initialAttitude);
539 
540  vtolDynamicsSim.calculateNewState(Maero, Faero, motors, dt);
541  angularAcceleration = vtolDynamicsSim.getAngularAcceleration();
542  linearAcceleration = vtolDynamicsSim.getLinearAcceleration();
543 }
544 
545 TEST(VtolDynamics, calculateNewState_1_OnlyAttitude){
546  double dt = 0.002500;
547  std::vector<double> motors{0, 0, 0, 0, 0};
548  Eigen::Quaterniond initAttitude(1, 0.2, 0.10, 0.05);
549  Eigen::Vector3d angAccel, linAccel, diff,
550  Faero(0.0, 0.0, 0.0),
551  Maero(0.0, 0.0, 0.0),
552  initialLinVel(0, 0, 0),
553  initAngVel(0, 0, 0),
554  initPose(0, 0, 0),
555  expectedAngAccel(0.0, 0.0, 0.0),
556  expectedLinAccel(2.5377e-16, -5.0753e-16, 9.8066e+00);
557 
558  calculateNewState(dt, motors, Maero, Faero,
559  initialLinVel, initAngVel, initPose, initAttitude,
560  expectedAngAccel, expectedLinAccel,
561  angAccel, linAccel);
562 
563  for(size_t idx = 0; idx < 3; idx++){
564  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 1e-04);
565  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 1e-04);
566  }
567 }
568 
569 TEST(VtolDynamics, calculateNewState_2_OnlyAngularVelocity){
570  double dt = 0.002500;
571  std::vector<double> motors{0, 0, 0, 0, 0};
572  Eigen::Quaterniond initAttitude(1, 0.00, 0.00, 0.00);
573  Eigen::Vector3d angAccel, linAccel, diff,
574  Faero(0.0, 0.0, 0.0),
575  Maero(0.0, 0.0, 0.0),
576  initialLinVel(0, 0, 0),
577  initAngVel(0.3, 0.2, 0.1),
578  initPose(0, 0, 0),
579  expectedAngAccel(-1.9719e-02, 2.9589e-02, -8.3459e-04),
580  expectedLinAccel(9.9127e-19, 1.9825e-18, 9.8066e+00);
581 
582  calculateNewState(dt, motors, Maero, Faero,
583  initialLinVel, initAngVel, initPose, initAttitude,
584  expectedAngAccel, expectedLinAccel,
585  angAccel, linAccel);
586 
587  for(size_t idx = 0; idx < 3; idx++){
588  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 6e-05);
589  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 6e-05);
590  }
591 }
592 
593 TEST(VtolDynamics, calculateNewState_3_OnlyFaero){
594  double dt = 0.002500;
595  std::vector<double> motors{0, 0, 0, 0, 0};
596  Eigen::Quaterniond initAttitude(1, 0.00, 0.00, 0.00);
597  Eigen::Vector3d angAccel, linAccel, diff,
598  Faero(5.7448e-01, 2.9513e+01, 6.1333e-01),
599  Maero(0.0, 0.0, 0.0),
600  initialLinVel(0, 0, 0),
601  initAngVel(0.0, 0.0, 0.0),
602  initPose(0, 0, 0),
603  expectedAngAccel(0.0, 0.0, 0.0),
604  expectedLinAccel(0.082069, 4.216143, 9.894269);
605 
606  calculateNewState(dt, motors, Maero, Faero,
607  initialLinVel, initAngVel, initPose, initAttitude,
608  expectedAngAccel, expectedLinAccel,
609  angAccel, linAccel);
610 
611  for(size_t idx = 0; idx < 3; idx++){
612  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 6e-05);
613  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 6e-05);
614  }
615 }
616 
617 TEST(VtolDynamics, calculateNewState_4_OnlyMaero){
618  double dt = 0.002500;
619  std::vector<double> motors{0, 0, 0, 0, 0};
620  Eigen::Quaterniond initAttitude(1, 0.00, 0.00, 0.00);
621  Eigen::Vector3d angAccel, linAccel, diff,
622  Faero(0.0, 0.0, 0.0),
623  Maero(-0.214696, -0.694801, -0.316328),
624  initialLinVel(0, 0, 0),
625  initAngVel(0.0, 0.0, 0.0),
626  initPose(0, 0, 0),
627  expectedAngAccel(-0.34251, -1.07821, -0.25057),
628  expectedLinAccel(7.7443e-21, -3.8722e-21, 9.8066e+00);
629 
630  calculateNewState(dt, motors, Maero, Faero,
631  initialLinVel, initAngVel, initPose, initAttitude,
632  expectedAngAccel, expectedLinAccel,
633  angAccel, linAccel);
634 
635  for(size_t idx = 0; idx < 3; idx++){
636  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 6e-05);
637  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 6e-05);
638  }
639 }
640 
641 TEST(VtolDynamics, calculateNewState_5_OnlyCopterMotorsWithEqualPower){
642  double dt = 0.002500;
643  std::vector<double> motors{700, 700, 700, 700, 0};
644  Eigen::Quaterniond initAttitude(1, 0.00, 0.00, 0.00);
645  Eigen::Vector3d angAccel, linAccel, diff,
646  Faero(0.0, 0.0, 0.0),
647  Maero(0.0, 0.0, 0.0),
648  initialLinVel(0, 0, 0),
649  initAngVel(0.0, 0.0, 0.0),
650  initPose(0, 0, 0),
651  expectedAngAccel(0.00000, 0.00000, 0.00000),
652  expectedLinAccel(0.00000, 0.00000, -6.36769);
653 
654  calculateNewState(dt, motors, Maero, Faero,
655  initialLinVel, initAngVel, initPose, initAttitude,
656  expectedAngAccel, expectedLinAccel,
657  angAccel, linAccel);
658 
659  for(size_t idx = 0; idx < 3; idx++){
660  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 6e-05);
661  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 6e-05);
662  }
663 }
664 
665 TEST(VtolDynamics, calculateNewState_6_OnlyCopterMotorsWithNotEqualPower){
666  double dt = 0.002500;
667  std::vector<double> motors{700, 680, 660, 640, 0};
668  Eigen::Quaterniond initAttitude(1, 0.00, 0.00, 0.00);
669  Eigen::Vector3d angAccel, linAccel, diff,
670  Faero(0.0, 0.0, 0.0),
671  Maero(0.0, 0.0, 0.0),
672  initialLinVel(0, 0, 0),
673  initAngVel(0.0, 0.0, 0.0),
674  initPose(0, 0, 0),
675  expectedAngAccel(0.1354, 1.2944, 0.10723),
676  expectedLinAccel(-1.3753e-04, 1.2938e-05, -5.0505e+00);
677 
678  calculateNewState(dt, motors, Maero, Faero,
679  initialLinVel, initAngVel, initPose, initAttitude,
680  expectedAngAccel, expectedLinAccel,
681  angAccel, linAccel);
682 
683  for(size_t idx = 0; idx < 3; idx++){
684  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 6e-05);
685  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 6e-05);
686  }
687 }
688 
689 TEST(VtolDynamics, calculateNewState_7_OnlyICE){
690  double dt = 0.002500;
691  std::vector<double> motors{0, 0, 0, 0, 500};
692  Eigen::Quaterniond initAttitude(1, 0.00, 0.00, 0.00);
693  Eigen::Vector3d angAccel, linAccel, diff,
694  Faero(0.0, 0.0, 0.0),
695  Maero(0.0, 0.0, 0.0),
696  initialLinVel(0, 0, 0),
697  initAngVel(0.0, 0.0, 0.0),
698  initPose(0, 0, 0),
699  expectedAngAccel(-0.43508, 0.00000, 0.00000),
700  expectedLinAccel(2.2705e+00, 3.8722e-21, 9.8066e+00);
701 
702  calculateNewState(dt, motors, Maero, Faero,
703  initialLinVel, initAngVel, initPose, initAttitude,
704  expectedAngAccel, expectedLinAccel,
705  angAccel, linAccel);
706 
707  for(size_t idx = 0; idx < 3; idx++){
708  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 6e-05);
709  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 6e-05);
710  }
711 }
712 
713 TEST(VtolDynamics, calculateNewState_8_ComplexWithoutInitialAttitude){
714  double dt = 0.002500;
715  std::vector<double> motors{600, 550, 450, 500, 650};
716  Eigen::Quaterniond initAttitude(1, 0, 0, 0);
717 
718  Eigen::Vector3d angAccel, linAccel, diff,
719  Faero(15.0, 10.0, 5.0),
720  Maero(5.0, 10.0, 15.0),
721  initialLinVel(15, 3, 1),
722  initAngVel(0.5, 0.4, 0.3),
723  initPose(0, 0, 10),
724  expectedAngAccel(5.1203, 16.15784, 11.9625),
725  expectedLinAccel(5.60908, 1.44474, 0.80233);
726 
727  calculateNewState(dt, motors, Maero, Faero,
728  initialLinVel, initAngVel, initPose, initAttitude,
729  expectedAngAccel, expectedLinAccel,
730  angAccel, linAccel);
731 
732  for(size_t idx = 0; idx < 3; idx++){
733  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 1e-03);
734  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 1e-03);
735  }
736 }
737 
738 TEST(VtolDynamics, calculateNewState_9_EightComplexFull){
739  double dt = 0.002500;
740  std::vector<double> motors{600, 550, 450, 500, 650};
741  Eigen::Quaterniond initAttitude(0.9833, 0.1436, 0.106, 0.03427);
742 
743  Eigen::Vector3d angAccel, linAccel, diff,
744  Faero(15.0, 10.0, 5.0),
745  Maero(5.0, 10.0, 15.0),
746  initialLinVel(15, 3, 1),
747  initAngVel(0.5, 0.4, 0.3),
748  initPose(0, 0, 10),
749  expectedAngAccel(5.1202, 16.15784, 11.9625),
750  expectedLinAccel(3.45031, 4.40765, 0.68005);
751 
752  calculateNewState(dt, motors, Maero, Faero,
753  initialLinVel, initAngVel, initPose, initAttitude,
754  expectedAngAccel, expectedLinAccel,
755  angAccel, linAccel);
756 
757  for(size_t idx = 0; idx < 3; idx++){
758  EXPECT_NEAR(angAccel[idx], expectedAngAccel[idx], 1e-03);
759  EXPECT_NEAR(linAccel[idx], expectedLinAccel[idx], 1e-03);
760  }
761 }
762 
763 
764 int main(int argc, char *argv[]){
765  testing::InitGoogleTest(&argc, argv);
766  ros::init(argc, argv, "tester");
767  return RUN_ALL_TESTS();
768 }
VtolDynamics
Vtol dynamics simulator class.
Definition: vtolDynamicsSim.hpp:147
VtolDynamics::init
int8_t init() override
Use rosparam here to initialize sim.
Definition: vtolDynamicsSim.cpp:48
VtolDynamics::getAngularAcceleration
Eigen::Vector3d getAngularAcceleration() const
Definition: vtolDynamicsSim.cpp:676
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
calculateNewState
void calculateNewState(double dt, std::vector< double > motors, Eigen::Vector3d Maero, Eigen::Vector3d Faero, Eigen::Vector3d initialLinearVelocity, Eigen::Vector3d initialAngularVelocity, Eigen::Vector3d initialPosition, Eigen::Quaterniond initialAttitude, Eigen::Vector3d &expectedAngAccel, Eigen::Vector3d &expectedLinAccel, Eigen::Vector3d &angularAcceleration, Eigen::Vector3d &linearAcceleration)
Definition: test_vtol_dynamics.cpp:523
Math::findPrevRowIdxInIncreasingSequence
size_t findPrevRowIdxInIncreasingSequence(const Eigen::MatrixXd &table, double value)
Given an increasing sequence and a key, return the index of the previous element closest to the key.
Definition: common_math.cpp:59
VtolDynamics::calculateCmyElevator
double calculateCmyElevator(double elevator_pos, double airspeed) const
Definition: vtolDynamicsSim.cpp:612
TEST
TEST(VtolDynamics, calculateWind)
Definition: test_vtol_dynamics.cpp:28
VtolDynamics::setInitialPosition
void setInitialPosition(const Eigen::Vector3d &position, const Eigen::Quaterniond &attitudeXYZW) override
Definition: vtolDynamicsSim.cpp:151
main
int main(int argc, char *argv[])
Definition: test_vtol_dynamics.cpp:764
vtolDynamicsSim.hpp
VtolDynamics::calculateAnglesOfSideslip
double calculateAnglesOfSideslip(const Eigen::Vector3d &airSpeed) const
Definition: vtolDynamicsSim.cpp:418
VtolDynamics::calculateCSBeta
double calculateCSBeta(double AoS_deg, double airspeed) const
Definition: vtolDynamicsSim.cpp:606
VtolDynamics::calculateCLPolynomial
void calculateCLPolynomial(double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
Definition: vtolDynamicsSim.cpp:579
VtolDynamics::calculateDynamicPressure
double calculateDynamicPressure(double airSpeedMod) const
Definition: vtolDynamicsSim.cpp:398
Math::calculatePolynomial
bool calculatePolynomial(const Eigen::MatrixXd &table, double airSpeedMod, Eigen::VectorXd &polynomialCoeffs)
Definition: common_math.cpp:87
VtolDynamics::calculateCSPolynomial
void calculateCSPolynomial(double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
Definition: vtolDynamicsSim.cpp:583
VtolDynamics::thruster
void thruster(double actuator, double &thrust, double &torque, double &rpm) const
Definition: vtolDynamicsSim.cpp:507
VtolDynamics::getLinearAcceleration
Eigen::Vector3d getLinearAcceleration() const
Definition: vtolDynamicsSim.cpp:679
VtolDynamics::setWindParameter
void setWindParameter(Eigen::Vector3d windMeanVelocityNED, double wind_velocityVariance) override
Definition: vtolDynamicsSim.cpp:671
Math::griddata
double griddata(const Eigen::MatrixXd &x, const Eigen::MatrixXd &y, const Eigen::MatrixXd &z, double x_val, double y_val)
Definition: common_math.cpp:68
VtolDynamics::calculateWind
Eigen::Vector3d calculateWind()
Definition: vtolDynamicsSim.cpp:364
VtolDynamics::calculateNewState
void calculateNewState(const Eigen::Vector3d &Maero, const Eigen::Vector3d &Faero, const std::vector< double > &motors, double dt_sec)
Definition: vtolDynamicsSim.cpp:526
Math::findPrevRowIdxInMonotonicSequence
size_t findPrevRowIdxInMonotonicSequence(const Eigen::MatrixXd &matrix, double key)
Given monotonic sequence (increasing or decreasing) and key, return the index of the previous element...
Definition: common_math.cpp:38
VtolDynamics::calculateCSRudder
double calculateCSRudder(double rudder_pos, double airspeed) const
Definition: vtolDynamicsSim.cpp:603
VtolDynamics::setInitialVelocity
void setInitialVelocity(const Eigen::Vector3d &linearVelocity, const Eigen::Vector3d &angularVelocity)
Definition: vtolDynamicsSim.cpp:158
common_math.hpp
VtolDynamics::calculateAerodynamics
void calculateAerodynamics(const Eigen::Vector3d &airspeed, double AoA, double AoS, const std::array< double, 3 > &servos, Eigen::Vector3d &Faero, Eigen::Vector3d &Maero)
Definition: vtolDynamicsSim.cpp:434
f
static const double f
Definition: geodetic.cpp:22
VtolDynamics::calculateCDPolynomial
void calculateCDPolynomial(double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
Definition: vtolDynamicsSim.cpp:587
VtolDynamics::calculateAnglesOfAtack
double calculateAnglesOfAtack(const Eigen::Vector3d &airSpeed) const
Definition: vtolDynamicsSim.cpp:407
Math::polyval
double polyval(const Eigen::VectorXd &poly, double val)
Definition: common_math.cpp:30

inno_vtol_dynamics
Author(s): Roman Fedorenko, Dmitry Ponomarev, Ezra Tal, Winter Guerra
autogenerated on Mon Dec 9 2024 03:13:35