Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : multicopter frame simulator class
17 : */
18 :
19 : #include "SIM_Frame.h"
20 : #include <AP_Motors/AP_Motors.h>
21 : #include <AP_Baro/AP_Baro.h>
22 : #include <AP_Filesystem/AP_Filesystem.h>
23 : #include <AP_Vehicle/AP_Vehicle_Type.h>
24 : #include "SIM_Aircraft.h"
25 :
26 : #include "SIM_config.h"
27 :
28 : #include <stdio.h>
29 : #include <sys/stat.h>
30 :
31 : using namespace SITL;
32 :
33 : #if AP_SIM_ENABLED
34 : // default to no bluff body or momentum drag on planes, where the
35 : // plane model handles drag
36 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
37 : #define SIM_FRAME_BBDRAG_DEFAULT 0.0
38 : #define SIM_FRAME_MDRAG_DEFAULT 0.0
39 : #else
40 : #define SIM_FRAME_BBDRAG_DEFAULT 1.0
41 : #define SIM_FRAME_MDRAG_DEFAULT 0.2
42 : #endif
43 :
44 : // user settable multicopter model parameters. Defaults can be
45 : // overridden by a json model file
46 : const AP_Param::GroupInfo Frame::var_info[] = {
47 : // @Param: MASS
48 : // @DisplayName: model mass
49 : // @Description: mass of the multicopter model
50 : // @Units: kg
51 : AP_GROUPINFO("MASS", 1, Frame, model.mass, 3.0),
52 :
53 : // @Param: DIAG_SZ
54 : // @DisplayName: model diagonal size
55 : // @Description: diagonal motor to motor distance of the model
56 : // @Units: m
57 : AP_GROUPINFO("DIAG_SZ", 2, Frame, model.diagonal_size, 0.35),
58 :
59 : // @Param: REF_SPD
60 : // @DisplayName: reference speed
61 : // @Description: airspeed in the drag reference test, at a fixed lean angle
62 : // @Units: m/s
63 : AP_GROUPINFO("REF_SPD", 3, Frame, model.refSpd, 15.08),
64 :
65 : // @Param: REF_ANG
66 : // @DisplayName: reference angle
67 : // @Description: lean angle in the drag reference test
68 : // @Units: deg
69 : AP_GROUPINFO("REF_ANG", 4, Frame, model.refAngle, 45),
70 :
71 : // @Param: REF_VOLT
72 : // @DisplayName: reference voltage
73 : // @Description: battery voltage in the drag reference test
74 : // @Units: V
75 : AP_GROUPINFO("REF_VOLT", 5, Frame, model.refVoltage, 12.09),
76 :
77 : // @Param: REF_AMP
78 : // @DisplayName: reference current
79 : // @Description: battery current in the drag reference test
80 : // @Units: A
81 : AP_GROUPINFO("REF_AMP", 6, Frame, model.refCurrent, 29.3),
82 :
83 : // @Param: REF_ALT
84 : // @DisplayName: reference altitude
85 : // @Description: altitude AMSL of the drag reference test
86 : // @Units: m
87 : AP_GROUPINFO("REF_ALT", 7, Frame, model.refAlt, 593),
88 :
89 : // @Param: BAT_RES
90 : // @DisplayName: battery resistance
91 : // @Description: battery resistance
92 : // @Units: Ohm
93 : AP_GROUPINFO("BAT_RES", 8, Frame, model.refBatRes, 0.01),
94 :
95 : // @Param: BAT_VOLT
96 : // @DisplayName: battery full voltage
97 : // @Description: full pack battery voltage
98 : // @Units: V
99 : AP_GROUPINFO("BAT_VOLT", 9, Frame, model.maxVoltage, 12.6),
100 :
101 : // @Param: BAT_CAP
102 : // @DisplayName: battery capacity
103 : // @Description: battery capacity, zero for unlimited
104 : // @Units: Ah
105 : AP_GROUPINFO("BAT_CAP", 10, Frame, model.battCapacityAh, 0),
106 :
107 : // @Param: HOVR_THR
108 : // @DisplayName: hover throttle
109 : // @Description: throttle output (CTUN.ThO) at hover at the reference altitude
110 : // @Range: 0.05 1
111 : AP_GROUPINFO("HOVR_THR", 11, Frame, model.hoverThrOut, 0.39),
112 :
113 : // @Param: EXPO
114 : // @DisplayName: thrust expo
115 : // @Description: motor thrust expo, matching MOT_THST_EXPO
116 : AP_GROUPINFO("EXPO", 12, Frame, model.propExpo, 0.65),
117 :
118 : // @Param: ROT_RATE
119 : // @DisplayName: rotation rate
120 : // @Description: terminal yaw rotation rate
121 : // @Units: deg/s
122 : AP_GROUPINFO("ROT_RATE", 13, Frame, model.refRotRate, 120),
123 :
124 : // @Param: PWM_MIN
125 : // @DisplayName: minimum PWM
126 : // @Description: minimum motor PWM in the reference test, matching MOT_PWM_MIN
127 : // @Units: PWM
128 : AP_GROUPINFO("PWM_MIN", 14, Frame, model.pwmMin, 1000),
129 :
130 : // @Param: PWM_MAX
131 : // @DisplayName: maximum PWM
132 : // @Description: maximum motor PWM in the reference test, matching MOT_PWM_MAX
133 : // @Units: PWM
134 : AP_GROUPINFO("PWM_MAX", 15, Frame, model.pwmMax, 2000),
135 :
136 : // @Param: SPIN_MIN
137 : // @DisplayName: motor spin minimum
138 : // @Description: minimum motor spin fraction, matching MOT_SPIN_MIN
139 : // @Range: 0 1
140 : AP_GROUPINFO("SPIN_MIN", 16, Frame, model.spin_min, 0.15),
141 :
142 : // @Param: SPIN_MAX
143 : // @DisplayName: motor spin maximum
144 : // @Description: maximum motor spin fraction, matching MOT_SPIN_MAX
145 : // @Range: 0 1
146 : AP_GROUPINFO("SPIN_MAX", 17, Frame, model.spin_max, 0.95),
147 :
148 : // @Param: SLEW_MAX
149 : // @DisplayName: motor slew rate
150 : // @Description: maximum motor slew rate in throttle fraction per second
151 : // @Units: 1/s
152 : AP_GROUPINFO("SLEW_MAX", 18, Frame, model.slew_max, 150),
153 :
154 : // @Param: DISCAREA
155 : // @DisplayName: rotor disc area
156 : // @Description: total rotor disc area. Coaxial rotors count as one rotor only
157 : // @Units: m.m
158 : AP_GROUPINFO("DISCAREA", 19, Frame, model.disc_area, 0.385),
159 :
160 : // @Param: MDRAG
161 : // @DisplayName: momentum drag coefficient
162 : // @Description: momentum drag coefficient of the rotors. Defaults to zero on planes where the plane model handles drag
163 : AP_GROUPINFO("MDRAG", 20, Frame, model.mdrag_coef, SIM_FRAME_MDRAG_DEFAULT),
164 :
165 : // @Param: BBDRAG
166 : // @DisplayName: bluff body drag coefficient
167 : // @Description: scaling of the bluff body drag derived from the reference test, zero for no bluff body drag. Defaults to zero on planes where the plane model handles drag
168 : AP_GROUPINFO("BBDRAG", 21, Frame, model.bbdrag_coef, SIM_FRAME_BBDRAG_DEFAULT),
169 :
170 : AP_GROUPEND
171 : };
172 : #endif // AP_SIM_ENABLED
173 :
174 : static Motor quad_plus_motors[] =
175 : {
176 : Motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
177 : Motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
178 : Motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
179 : Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
180 : };
181 :
182 : static Motor quad_x_motors[] =
183 : {
184 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
185 : Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
186 : Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
187 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
188 : };
189 :
190 : // motor order to match betaflight conventions
191 : // See: https://fpvfrenzy.com/betaflight-motor-order/
192 : static Motor quad_bf_x_motors[] =
193 : {
194 : Motor(AP_MOTORS_MOT_1, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
195 : Motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,1),
196 : Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,3),
197 : Motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
198 : };
199 :
200 : // motor order to match betaflight conventions, reversed direction
201 : static Motor quad_bf_x_rev_motors[] =
202 : {
203 : Motor(AP_MOTORS_MOT_1, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
204 : Motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
205 : Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
206 : Motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
207 : };
208 :
209 : // motor order to match DJI conventions
210 : // See: https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
211 : static Motor quad_dji_x_motors[] =
212 : {
213 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
214 : Motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
215 : Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
216 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
217 : };
218 :
219 : // motor order so that test order matches motor order ("clockwise X")
220 : static Motor quad_cw_x_motors[] =
221 : {
222 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
223 : Motor(AP_MOTORS_MOT_2, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
224 : Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
225 : Motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
226 : };
227 :
228 : #if AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
229 : static Motor dotriaconta_octaquad_x_motors[] =
230 : {
231 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
232 : Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 17),
233 : Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 25),
234 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 9),
235 :
236 : Motor(AP_MOTORS_MOT_5, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
237 : Motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 18),
238 : Motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 26),
239 : Motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 10),
240 :
241 : Motor(AP_MOTORS_MOT_9, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
242 : Motor(AP_MOTORS_MOT_10, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 19),
243 : Motor(AP_MOTORS_MOT_11, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 27),
244 : Motor(AP_MOTORS_MOT_12, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11),
245 :
246 : Motor(AP_MOTORS_MOT_13, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
247 : Motor(AP_MOTORS_MOT_14, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 20),
248 : Motor(AP_MOTORS_MOT_15, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 28),
249 : Motor(AP_MOTORS_MOT_16, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12),
250 :
251 : Motor(AP_MOTORS_MOT_17, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
252 : Motor(AP_MOTORS_MOT_18, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 21),
253 : Motor(AP_MOTORS_MOT_19, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 29),
254 : Motor(AP_MOTORS_MOT_20, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 13),
255 :
256 : Motor(AP_MOTORS_MOT_21, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
257 : Motor(AP_MOTORS_MOT_22, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 22),
258 : Motor(AP_MOTORS_MOT_23, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 30),
259 : Motor(AP_MOTORS_MOT_24, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 14),
260 :
261 : Motor(AP_MOTORS_MOT_25, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
262 : Motor(AP_MOTORS_MOT_26, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 23),
263 : Motor(AP_MOTORS_MOT_27, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 31),
264 : Motor(AP_MOTORS_MOT_28, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 15),
265 :
266 : Motor(AP_MOTORS_MOT_29, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
267 : Motor(AP_MOTORS_MOT_30, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 24),
268 : Motor(AP_MOTORS_MOT_31, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 32),
269 : Motor(AP_MOTORS_MOT_32, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 16),
270 : };
271 : #endif // AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
272 :
273 :
274 : static Motor tiltquad_h_vectored_motors[] =
275 : {
276 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1, -1, 0, 0, 7, 10, -90),
277 : Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3, -1, 0, 0, 8, 10, -90),
278 : Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4, -1, 0, 0, 8, 10, -90),
279 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, -1, 0, 0, 7, 10, -90),
280 : };
281 :
282 : static Motor tiltquad[] =
283 : {
284 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 7, 10, -90),
285 : Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
286 : Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4, -1, 0, 0, 8, 10, -90),
287 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
288 : };
289 :
290 : // collective pitch quad helicopter, matching AP_MotorsHeli_Quad's
291 : // layout, with rotor speed control on channel 8 to match
292 : // AP_MotorsHeli_RSC's default channel
293 : // Run in a shell: ./Tools/autotest/fg_heliquad_view.sh
294 : // then Tools/autotest/sim_vehicle.py -v ArduCopter -f heli-quad --enable-fgview
295 : static Motor heliquad_motors[] =
296 : {
297 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, 7),
298 : Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3, 7),
299 : Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4, 7),
300 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2, 7),
301 : };
302 :
303 : static Motor hexa_motors[] =
304 : {
305 : Motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
306 : Motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
307 : Motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
308 : Motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
309 : Motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
310 : Motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3)
311 : };
312 :
313 : static Motor hexax_motors[] =
314 : {
315 : Motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
316 : Motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
317 : Motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
318 : Motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
319 : Motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
320 : Motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4)
321 : };
322 :
323 : static Motor hexa_dji_x_motors[] =
324 : {
325 : Motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
326 : Motor(AP_MOTORS_MOT_2, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
327 : Motor(AP_MOTORS_MOT_3, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
328 : Motor(AP_MOTORS_MOT_4, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
329 : Motor(AP_MOTORS_MOT_5, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
330 : Motor(AP_MOTORS_MOT_6, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2)
331 : };
332 :
333 : static Motor hexa_cw_x_motors[] =
334 : {
335 : Motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
336 : Motor(AP_MOTORS_MOT_2, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
337 : Motor(AP_MOTORS_MOT_3, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
338 : Motor(AP_MOTORS_MOT_4, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
339 : Motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
340 : Motor(AP_MOTORS_MOT_6, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6)
341 : };
342 :
343 : static Motor octa_motors[] =
344 : {
345 : Motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
346 : Motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
347 : Motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
348 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
349 : Motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
350 : Motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
351 : Motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
352 : Motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3)
353 : };
354 :
355 : static Motor octa_dji_x_motors[] =
356 : {
357 : Motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
358 : Motor(AP_MOTORS_MOT_2, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
359 : Motor(AP_MOTORS_MOT_3, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
360 : Motor(AP_MOTORS_MOT_4, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
361 : Motor(AP_MOTORS_MOT_5, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
362 : Motor(AP_MOTORS_MOT_6, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
363 : Motor(AP_MOTORS_MOT_7, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
364 : Motor(AP_MOTORS_MOT_8, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2)
365 : };
366 :
367 : static Motor octa_cw_x_motors[] =
368 : {
369 : Motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
370 : Motor(AP_MOTORS_MOT_2, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
371 : Motor(AP_MOTORS_MOT_3, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
372 : Motor(AP_MOTORS_MOT_4, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
373 : Motor(AP_MOTORS_MOT_5, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
374 : Motor(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
375 : Motor(AP_MOTORS_MOT_7, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
376 : Motor(AP_MOTORS_MOT_8, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8)
377 : };
378 :
379 : static Motor octa_quad_motors[] =
380 : {
381 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
382 : Motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
383 : Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
384 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
385 : Motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
386 : Motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
387 : Motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
388 : Motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6)
389 : };
390 :
391 : static Motor octa_quad_corotating_motors[] =
392 : {
393 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
394 : Motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
395 : Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
396 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
397 : Motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
398 : Motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
399 : Motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
400 : Motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6)
401 : };
402 :
403 : static Motor octa_quad_cw_corotating_motors[] =
404 : {
405 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
406 : Motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
407 : Motor(AP_MOTORS_MOT_3, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
408 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
409 : Motor(AP_MOTORS_MOT_5, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
410 : Motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
411 : Motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
412 : Motor(AP_MOTORS_MOT_8, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
413 : };
414 :
415 : static Motor octa_quad_cw_x_motors[] =
416 : {
417 : Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
418 : Motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
419 : Motor(AP_MOTORS_MOT_3, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
420 : Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
421 : Motor(AP_MOTORS_MOT_5, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
422 : Motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
423 : Motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
424 : Motor(AP_MOTORS_MOT_8, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8)
425 : };
426 :
427 :
428 : static Motor dodeca_hexa_motors[] =
429 : {
430 : Motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
431 : Motor(AP_MOTORS_MOT_2, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
432 : Motor(AP_MOTORS_MOT_3, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
433 : Motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
434 : Motor(AP_MOTORS_MOT_5, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
435 : Motor(AP_MOTORS_MOT_6, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
436 : Motor(AP_MOTORS_MOT_7, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
437 : Motor(AP_MOTORS_MOT_8, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
438 : Motor(AP_MOTORS_MOT_9, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9),
439 : Motor(AP_MOTORS_MOT_10, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10),
440 : Motor(AP_MOTORS_MOT_11, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11),
441 : Motor(AP_MOTORS_MOT_12, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12)
442 : };
443 :
444 : static Motor hexadeca_octa_motors[] =
445 : {
446 : Motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
447 : Motor(AP_MOTORS_MOT_2, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
448 : Motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
449 : Motor(AP_MOTORS_MOT_4, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
450 : Motor(AP_MOTORS_MOT_5, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
451 : Motor(AP_MOTORS_MOT_6, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
452 : Motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
453 : Motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
454 : Motor(AP_MOTORS_MOT_9, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 9),
455 : Motor(AP_MOTORS_MOT_10, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 10),
456 : Motor(AP_MOTORS_MOT_11, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 11),
457 : Motor(AP_MOTORS_MOT_12, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 12),
458 : Motor(AP_MOTORS_MOT_13, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 13),
459 : Motor(AP_MOTORS_MOT_14, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 14),
460 : Motor(AP_MOTORS_MOT_15, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 15),
461 : Motor(AP_MOTORS_MOT_16, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 16)
462 : };
463 :
464 : static Motor hexadeca_octa_cw_x_motors[] =
465 : {
466 : Motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
467 : Motor(AP_MOTORS_MOT_2, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
468 : Motor(AP_MOTORS_MOT_3, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
469 : Motor(AP_MOTORS_MOT_4, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
470 : Motor(AP_MOTORS_MOT_5, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
471 : Motor(AP_MOTORS_MOT_6, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
472 : Motor(AP_MOTORS_MOT_7, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
473 : Motor(AP_MOTORS_MOT_8, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
474 : Motor(AP_MOTORS_MOT_9, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 9),
475 : Motor(AP_MOTORS_MOT_10, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 10),
476 : Motor(AP_MOTORS_MOT_11, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 11),
477 : Motor(AP_MOTORS_MOT_12, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 12),
478 : Motor(AP_MOTORS_MOT_13, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 13),
479 : Motor(AP_MOTORS_MOT_14, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 14),
480 : Motor(AP_MOTORS_MOT_15, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 15),
481 : Motor(AP_MOTORS_MOT_16, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 16)
482 : };
483 :
484 : static Motor deca_motors[] =
485 : {
486 : Motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
487 : Motor(AP_MOTORS_MOT_2, 36, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
488 : Motor(AP_MOTORS_MOT_3, 72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
489 : Motor(AP_MOTORS_MOT_4, 108, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
490 : Motor(AP_MOTORS_MOT_5, 144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
491 : Motor(AP_MOTORS_MOT_6, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
492 : Motor(AP_MOTORS_MOT_7, -144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
493 : Motor(AP_MOTORS_MOT_8, -108, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
494 : Motor(AP_MOTORS_MOT_9, -72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9),
495 : Motor(AP_MOTORS_MOT_10, -36, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10)
496 : };
497 :
498 : static Motor deca_cw_x_motors[] =
499 : {
500 : Motor(AP_MOTORS_MOT_1, 18, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
501 : Motor(AP_MOTORS_MOT_2, 54, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
502 : Motor(AP_MOTORS_MOT_3, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
503 : Motor(AP_MOTORS_MOT_4, 126, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
504 : Motor(AP_MOTORS_MOT_5, 162, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
505 : Motor(AP_MOTORS_MOT_6, -162, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
506 : Motor(AP_MOTORS_MOT_7, -126, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
507 : Motor(AP_MOTORS_MOT_8, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
508 : Motor(AP_MOTORS_MOT_9, -54, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9),
509 : Motor(AP_MOTORS_MOT_10, -18, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10)
510 : };
511 :
512 : static Motor tri_motors[] =
513 : {
514 : Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
515 : Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
516 : Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, AP_MOTORS_MOT_7, 60, -60, -1, 0, 0),
517 : };
518 :
519 : static Motor tilttri_motors[] =
520 : {
521 : Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, AP_MOTORS_MOT_8, 0, -90),
522 : Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3, -1, 0, 0, AP_MOTORS_MOT_8, 0, -90),
523 : Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, AP_MOTORS_MOT_7, 60, -60, -1, 0, 0),
524 : };
525 :
526 : static Motor tilttri_vectored_motors[] =
527 : {
528 : Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 7, 10, -90),
529 : Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3, -1, 0, 0, 8, 10, -90),
530 : Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2)
531 : };
532 :
533 : static Motor y6_motors[] =
534 : {
535 : Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
536 : Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
537 : Motor(AP_MOTORS_MOT_3, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
538 : Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
539 : Motor(AP_MOTORS_MOT_5, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
540 : Motor(AP_MOTORS_MOT_6, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3)
541 : };
542 :
543 : /*
544 : FireflyY6 is a Y6 with front motors tiltable using servo on channel 9 (output 8)
545 : */
546 : static Motor firefly_motors[] =
547 : {
548 : Motor(AP_MOTORS_MOT_1, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
549 : Motor(AP_MOTORS_MOT_2, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 6, 0, -90),
550 : Motor(AP_MOTORS_MOT_3, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5, -1, 0, 0, 6, 0, -90),
551 : Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
552 : Motor(AP_MOTORS_MOT_5, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2, -1, 0, 0, 6, 0, -90),
553 : Motor(AP_MOTORS_MOT_6, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6, -1, 0, 0, 6, 0, -90)
554 : };
555 :
556 : typedef struct {
557 : const char *name;
558 : uint8_t num_motors;
559 : Motor *motors;
560 : } FrameTemplate;
561 :
562 : /*
563 : table of supported frame types. String order is important for
564 : partial name matching
565 : */
566 : static const FrameTemplate supported_frame_templates[] =
567 : {
568 : {"+", 4, quad_plus_motors},
569 : {"quad", 4, quad_plus_motors},
570 : {"copter", 4, quad_plus_motors},
571 : {"x", 4, quad_x_motors},
572 : {"bfxrev", 4, quad_bf_x_rev_motors},
573 : {"bfx", 4, quad_bf_x_motors},
574 : #if AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
575 : {"dotriaconta", 32, dotriaconta_octaquad_x_motors},
576 : #endif // AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
577 : {"djix", 4, quad_dji_x_motors},
578 : {"cwx", 4, quad_cw_x_motors},
579 : {"tilthvec", 4, tiltquad_h_vectored_motors},
580 : {"heli-quad", 4, heliquad_motors},
581 : {"hexadeca-octa", 16, hexadeca_octa_motors},
582 : {"hexadeca-octa-cwx", 16, hexadeca_octa_cw_x_motors},
583 : {"hexax", 6, hexax_motors},
584 : {"hexa-cwx", 6, hexa_cw_x_motors},
585 : {"hexa-dji", 6, hexa_dji_x_motors},
586 : {"hexa", 6, hexa_motors},
587 : {"octa-cwx", 8, octa_cw_x_motors},
588 : {"octa-dji", 8, octa_dji_x_motors},
589 : {"octa-quad-cwx",8, octa_quad_cw_x_motors},
590 : {"octa-quad-cor", 8, octa_quad_corotating_motors},
591 : {"octa-quad-cw-cor", 8, octa_quad_cw_corotating_motors},
592 : {"octa-quad", 8, octa_quad_motors},
593 : {"octa", 8, octa_motors},
594 : {"deca", 10, deca_motors},
595 : {"deca-cwx", 10, deca_cw_x_motors},
596 : {"dodeca-hexa", 12, dodeca_hexa_motors},
597 : {"tri", 3, tri_motors},
598 : {"tilttrivec",3, tilttri_vectored_motors},
599 : {"tilttri", 3, tilttri_motors},
600 : {"y6", 6, y6_motors},
601 : {"firefly", 6, firefly_motors},
602 : {"tilt", 4, tiltquad},
603 : };
604 :
605 : // get air density in kg/m^3
606 215891572 : float Frame::get_air_density(float alt_amsl) const
607 : {
608 215891572 : return AP_Baro::get_air_density_for_alt_amsl(alt_amsl);
609 : }
610 :
611 : /*
612 : load frame specific parameters from a json file if available
613 : */
614 29 : void Frame::load_frame_params(const char *model_json)
615 : {
616 29 : char *fname = nullptr;
617 : struct stat st;
618 29 : if (AP::FS().stat(model_json, &st) == 0) {
619 29 : fname = strdup(model_json);
620 : } else {
621 0 : IGNORE_RETURN(asprintf(&fname, "@ROMFS/models/%s", model_json));
622 0 : if (fname == nullptr || AP::FS().stat(fname, &st) != 0) {
623 0 : AP_HAL::panic("%s failed to load", model_json);
624 : }
625 : }
626 29 : if (fname == nullptr) {
627 0 : AP_HAL::panic("%s failed to load", model_json);
628 : }
629 29 : AP_JSON::value *obj = AP_JSON::load_json(fname);
630 29 : if (obj == nullptr) {
631 0 : AP_HAL::panic("%s failed to load", fname);
632 : }
633 :
634 : enum class VarType {
635 : FLOAT,
636 : AP_FLOAT,
637 : VECTOR3F,
638 : };
639 :
640 : struct json_search {
641 : const char *label;
642 : void *ptr;
643 : VarType t;
644 : };
645 :
646 29 : json_search vars[] = {
647 : #define FRAME_VAR(s) { #s, &model.s, VarType::AP_FLOAT }
648 29 : FRAME_VAR(mass),
649 29 : FRAME_VAR(diagonal_size),
650 29 : FRAME_VAR(refSpd),
651 29 : FRAME_VAR(refAngle),
652 29 : FRAME_VAR(refVoltage),
653 29 : FRAME_VAR(refCurrent),
654 29 : FRAME_VAR(refAlt),
655 29 : FRAME_VAR(maxVoltage),
656 29 : FRAME_VAR(battCapacityAh),
657 29 : FRAME_VAR(refBatRes),
658 29 : FRAME_VAR(propExpo),
659 29 : FRAME_VAR(refRotRate),
660 29 : FRAME_VAR(hoverThrOut),
661 29 : FRAME_VAR(pwmMin),
662 29 : FRAME_VAR(pwmMax),
663 29 : FRAME_VAR(spin_min),
664 29 : FRAME_VAR(spin_max),
665 29 : FRAME_VAR(slew_max),
666 29 : FRAME_VAR(disc_area),
667 29 : FRAME_VAR(mdrag_coef),
668 29 : FRAME_VAR(bbdrag_coef),
669 29 : {"moment_inertia", &model.moment_of_inertia, VarType::VECTOR3F},
670 29 : {"refTempC", &model.refTempC, VarType::FLOAT},
671 29 : {"num_motors", &model.num_motors, VarType::FLOAT},
672 29 : };
673 :
674 725 : for (uint8_t i=0; i<ARRAY_SIZE(vars); i++) {
675 1392 : auto v = obj->get(vars[i].label);
676 696 : if (v.is<AP_JSON::null>()) {
677 : // use default value
678 58 : continue;
679 : }
680 638 : if (vars[i].t == VarType::FLOAT) {
681 58 : parse_float(v, vars[i].label, *((float *)vars[i].ptr));
682 :
683 580 : } else if (vars[i].t == VarType::AP_FLOAT) {
684 : // json model values become the defaults for the SIM_FRM_ parameters
685 : float value;
686 580 : parse_float(v, vars[i].label, value);
687 580 : ((AP_Float *)vars[i].ptr)->set_default(value);
688 :
689 0 : } else if (vars[i].t == VarType::VECTOR3F) {
690 0 : parse_vector3(v, vars[i].label, *(Vector3f *)vars[i].ptr);
691 :
692 : }
693 696 : }
694 :
695 29 : json_search per_motor_vars[] = {
696 29 : {"position", &model.motor_pos, VarType::VECTOR3F},
697 29 : {"vector", &model.motor_thrust_vec, VarType::VECTOR3F},
698 29 : {"yaw", &model.yaw_factor, VarType::FLOAT},
699 29 : };
700 : char label_name[20];
701 116 : for (uint8_t i=0; i<ARRAY_SIZE(per_motor_vars); i++) {
702 2871 : for (uint8_t j=0; j<SIM_FRAME_MAX_ACTUATORS; j++) {
703 2784 : snprintf(label_name, 20, "motor%i_%s", j+1, per_motor_vars[i].label);
704 5568 : auto v = obj->get(label_name);
705 2784 : if (v.is<AP_JSON::null>()) {
706 : // use default value
707 2784 : continue;
708 : }
709 0 : if (per_motor_vars[i].t == VarType::FLOAT) {
710 0 : parse_float(v, label_name, *(((float *)per_motor_vars[i].ptr) + j));
711 :
712 0 : } else if (per_motor_vars[i].t == VarType::VECTOR3F) {
713 0 : parse_vector3(v, label_name, *(((Vector3f *)per_motor_vars[i].ptr) + j));
714 : }
715 2784 : }
716 : }
717 :
718 29 : delete obj;
719 :
720 29 : ::printf("Loaded model params from %s\n", fname);
721 29 : free(fname);
722 29 : }
723 :
724 638 : void Frame::parse_float(AP_JSON::value val, const char* label, float ¶m) {
725 638 : if (!val.is<double>()) {
726 0 : AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
727 : }
728 638 : param = val.get<double>();
729 638 : }
730 :
731 0 : void Frame::parse_vector3(AP_JSON::value val, const char* label, Vector3f ¶m) {
732 0 : if (!val.is<AP_JSON::value::array>() || !val.contains(2) || val.contains(3)) {
733 0 : AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
734 : }
735 0 : for (uint8_t j=0; j<3; j++) {
736 0 : parse_float(val.get(j), label, param[j]);
737 : }
738 0 : }
739 :
740 : #if AP_SIM_ENABLED
741 :
742 : /*
743 : initialise the frame
744 : */
745 1110 : void Frame::init(const char *frame_str)
746 : {
747 1110 : auto *sitl = AP::sitl();
748 1110 : if (sitl != nullptr) {
749 1110 : sitl->models.simframe_ptr = this;
750 : }
751 1110 : AP_Param::setup_object_defaults(this, var_info);
752 :
753 1110 : const char *colon = strchr(frame_str, ':');
754 1110 : size_t slen = strlen(frame_str);
755 1110 : if (colon != nullptr && slen > 5 && strcmp(&frame_str[slen-5], ".json") == 0) {
756 29 : load_frame_params(colon+1);
757 : }
758 :
759 1110 : if (uint8_t(model.num_motors) != num_motors) {
760 40 : ::printf("Warning model expected %u motors and got %u\n", uint8_t(model.num_motors), num_motors);
761 : }
762 :
763 1110 : update_parameters();
764 1110 : }
765 :
766 : /*
767 : apply settings from the model parameters. Called on each physics
768 : step so changes to SIM_FRM_ parameters take effect. Note that
769 : parameters loaded from eeprom are only applied after the frame is
770 : created, so this cannot be done just once in init()
771 : */
772 107946341 : void Frame::update_parameters(void)
773 : {
774 107946341 : mass = model.mass * mass_scale;
775 :
776 107946341 : const float drag_force = model.mass * GRAVITY_MSS * tanf(radians(model.refAngle));
777 :
778 107946341 : const float cos_tilt = cosf(radians(model.refAngle));
779 107946341 : const float airspeed_bf = model.refSpd * cos_tilt;
780 107946341 : const float ref_thrust = model.mass * GRAVITY_MSS / cos_tilt;
781 107946341 : const float ref_air_density = get_air_density(model.refAlt);
782 :
783 107946341 : mdrag_coef = model.mdrag_coef;
784 107946341 : const float momentum_drag = cos_tilt * mdrag_coef * airspeed_bf * sqrtf(ref_thrust * ref_air_density * model.disc_area);
785 :
786 107946341 : if (momentum_drag > drag_force) {
787 0 : mdrag_coef *= drag_force / momentum_drag;
788 0 : areaCd = 0.0;
789 : } else {
790 107946341 : areaCd = model.bbdrag_coef * (drag_force - momentum_drag) / (0.5f * ref_air_density * sq(model.refSpd));
791 : }
792 107946341 : const float drag_bcoef = is_positive(areaCd) ? model.mass / areaCd : 0.0;
793 107946341 : const float drag_mcoef = (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density);
794 215892682 : if ((!is_equal(drag_bcoef, last_drag_bcoef) || !is_equal(drag_mcoef, last_drag_mcoef)) &&
795 215892682 : isfinite(drag_bcoef) && isfinite(drag_mcoef)) {
796 909 : last_drag_bcoef = drag_bcoef;
797 909 : last_drag_mcoef = drag_mcoef;
798 909 : ::printf("Suggested EK3_DRAG_BCOEF_* = %.3f, EK3_DRAG_MCOEF = %.3f\n", drag_bcoef, drag_mcoef);
799 : }
800 :
801 107946341 : terminal_rotation_rate = model.refRotRate;
802 :
803 107946341 : const float hover_thrust = model.mass * GRAVITY_MSS;
804 107946341 : const float hover_power = model.refCurrent * model.refVoltage;
805 107946341 : const float hover_velocity_out = 2 * hover_power / hover_thrust;
806 107946341 : const float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out));
807 107946341 : const float velocity_max = hover_velocity_out / sqrtf(MAX(model.hoverThrOut.get(), 0.01f));
808 107946341 : const float effective_prop_area = effective_disc_area / num_motors;
809 107946341 : const float true_prop_area = model.disc_area / num_motors;
810 :
811 : // power_factor is ratio of power consumed per newton of thrust
812 107946341 : const float power_factor = hover_power / hover_thrust;
813 :
814 560909159 : for (uint8_t i=0; i<num_motors; i++) {
815 452962818 : motors[i].setup_params(model.pwmMin.get(), model.pwmMax.get(), model.spin_min, model.spin_max, model.propExpo, model.slew_max,
816 : model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max,
817 452962818 : model.motor_pos[i], model.motor_thrust_vec[i], model.yaw_factor[i], true_prop_area,
818 : mdrag_coef);
819 : }
820 :
821 107946341 : if (is_zero(model.moment_of_inertia.x) || is_zero(model.moment_of_inertia.y) || is_zero(model.moment_of_inertia.z)) {
822 : // if no inertia provided, assume 50% of mass on ring around center
823 107946341 : moment_of_inertia.x = model.mass * 0.25 * sq(model.diagonal_size*0.5);
824 107946341 : moment_of_inertia.y = moment_of_inertia.x;
825 107946341 : moment_of_inertia.z = model.mass * 0.5 * sq(model.diagonal_size*0.5);
826 : } else {
827 0 : moment_of_inertia = model.moment_of_inertia;
828 : }
829 :
830 107946341 : if (!is_equal(last_batt_voltage, model.maxVoltage.get()) ||
831 215891572 : !is_equal(last_batt_cap, model.battCapacityAh.get()) ||
832 107945231 : !is_equal(last_batt_res, model.refBatRes.get())) {
833 1110 : const float prev_cap = last_batt_cap;
834 1110 : last_batt_voltage = model.maxVoltage;
835 1110 : last_batt_cap = model.battCapacityAh;
836 1110 : last_batt_res = model.refBatRes;
837 1110 : battery_dirty = true;
838 :
839 : // setup reasonable defaults for battery
840 1110 : AP_Param::set_default_by_name("SIM_BATT_VOLTAGE", model.maxVoltage);
841 1110 : AP_Param::set_default_by_name("SIM_BATT_CAP_AH", model.battCapacityAh);
842 1110 : if (model.battCapacityAh > 0) {
843 25 : AP_Param::set_default_by_name("BATT_CAPACITY", model.battCapacityAh*1000);
844 1085 : } else if (prev_cap > 0) {
845 : // returning to unlimited capacity, mark vehicle capacity unknown
846 0 : AP_Param::set_default_by_name("BATT_CAPACITY", 0);
847 : }
848 : }
849 107946341 : }
850 :
851 : // returns true once when the battery model values have changed, so
852 : // the vehicle can re-setup the simulated battery
853 107945231 : bool Frame::battery_changed(void)
854 : {
855 107945231 : bool ret = battery_dirty;
856 107945231 : battery_dirty = false;
857 107945231 : return ret;
858 : }
859 :
860 : /*
861 : create a frame by name from its template
862 : */
863 1110 : Frame *Frame::create_frame(const char *name)
864 : {
865 3096 : for (uint8_t i=0; i < ARRAY_SIZE(supported_frame_templates); i++) {
866 3096 : auto &tplate = supported_frame_templates[i]; // `template` is a reserved word
867 : // do partial name matching to allow for frame variants
868 3096 : if (strncasecmp(name, tplate.name, strlen(tplate.name)) == 0) {
869 1110 : return NEW_NOTHROW Frame(tplate.name, tplate.num_motors, tplate.motors);
870 : }
871 : }
872 0 : return nullptr;
873 : }
874 :
875 : // calculate rotational and linear accelerations
876 107945231 : void Frame::calculate_forces(const Aircraft &aircraft,
877 : const struct sitl_input &input,
878 : Vector3f &rot_accel,
879 : Vector3f &body_accel,
880 : float* rpm)
881 : {
882 107945231 : Vector3f thrust; // newtons
883 107945231 : Vector3f torque;
884 :
885 107945231 : update_parameters();
886 :
887 107945231 : const float air_density = get_air_density(aircraft.get_location().alt*0.01);
888 107945231 : const Vector3f gyro = aircraft.get_gyro();
889 :
890 107945231 : Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
891 :
892 107945231 : const auto *_sitl = AP::sitl();
893 560903285 : for (uint8_t i=0; i<num_motors; i++) {
894 452958054 : Vector3f mtorque, mthrust;
895 452958054 : motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf,
896 : gyro, air_density, aircraft.get_battery_voltage(), true);
897 452958054 : torque += mtorque;
898 452958054 : thrust += mthrust;
899 : // simulate motor rpm
900 452958054 : if (!is_zero(_sitl->vibe_motor)) {
901 17508600 : rpm[motor_offset+i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f;
902 : }
903 : }
904 :
905 : // calculate total rotational acceleration
906 107945231 : rot_accel.x = torque.x / moment_of_inertia.x;
907 107945231 : rot_accel.y = torque.y / moment_of_inertia.y;
908 107945231 : rot_accel.z = torque.z / moment_of_inertia.z;
909 :
910 107945231 : if (terminal_rotation_rate > 0) {
911 : // rotational air resistance
912 107945231 : rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
913 107945231 : rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
914 107945231 : rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
915 : }
916 :
917 107945231 : if (is_positive(areaCd)) {
918 : // use the model params to calculate drag
919 81459271 : Vector3f drag_bf;
920 81459271 : drag_bf.x = areaCd * 0.5f * air_density * sq(vel_air_bf.x);
921 81459271 : if (is_negative(vel_air_bf.x)) {
922 22219331 : drag_bf.x = -drag_bf.x;
923 : }
924 :
925 81459271 : drag_bf.y = areaCd * 0.5f * air_density * sq(vel_air_bf.y);
926 81459271 : if (is_negative(vel_air_bf.y)) {
927 25285795 : drag_bf.y = -drag_bf.y;
928 : }
929 :
930 81459271 : drag_bf.z = areaCd * 0.5f * air_density * sq(vel_air_bf.z);
931 81459271 : if (is_negative(vel_air_bf.z)) {
932 30186263 : drag_bf.z = -drag_bf.z;
933 : }
934 :
935 81459271 : thrust -= drag_bf;
936 : }
937 :
938 107945231 : body_accel = thrust/aircraft.gross_mass();
939 107945231 : }
940 :
941 : // computes (total) instantaneous current
942 107945231 : float Frame::get_current_amp(void)
943 : {
944 107945231 : float current = 0;
945 560903285 : for (uint8_t i=0; i<num_motors; i++) {
946 452958054 : current += motors[i].get_current();
947 : }
948 107945231 : return current;
949 : }
950 : #endif // AP_SIM_ENABLED
|
