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

inno_vtol_dynamics
Author(s): Roman Fedorenko, Dmitry Ponomarev, Ezra Tal, Winter Guerra
autogenerated on Sat Jul 1 2023 02:13:44