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 : very simple plane simulator class. Not aerodynamically accurate,
17 : just enough to be able to debug control logic for new frame types
18 : */
19 :
20 : #include "SIM_Plane.h"
21 :
22 : #include <stdio.h>
23 : #include <AP_Filesystem/AP_Filesystem_config.h>
24 : #include <AP_Filesystem/AP_Filesystem.h>
25 :
26 : using namespace SITL;
27 :
28 691 : Plane::Plane(const char *frame_str) :
29 691 : Aircraft(frame_str)
30 : {
31 :
32 691 : const char *colon = strchr(frame_str, ':');
33 691 : size_t slen = strlen(frame_str);
34 : // The last 5 letters are ".json"
35 691 : if (colon != nullptr && slen > 5 && strcmp(&frame_str[slen-5], ".json") == 0) {
36 0 : load_coeffs(colon+1);
37 : } else {
38 691 : coefficient = default_coefficients;
39 : }
40 :
41 : // Chosen to approximately match a historical (simpler) battery model
42 691 : constexpr float default_battery_resistance_ohm = 0.0145f;
43 1382 : battery.setup(sitl->batt_capacity_ah,
44 : default_battery_resistance_ohm,
45 691 : sitl->batt_voltage,
46 : ambient_outside_temperature_degC());
47 :
48 691 : mass = 2.0f;
49 :
50 : /*
51 : scaling from motor power to Newtons. Allows the plane to hold
52 : vertically against gravity when the motor is at hover_throttle
53 : */
54 691 : thrust_scale = (mass * GRAVITY_MSS) / hover_throttle;
55 691 : frame_height = 0.1f;
56 :
57 691 : ground_behavior = GROUND_BEHAVIOR_FWD_ONLY;
58 691 : lock_step_scheduled = true;
59 :
60 691 : if (strstr(frame_str, "-heavy")) {
61 0 : mass = 8;
62 : }
63 691 : if (strstr(frame_str, "-jet")) {
64 : // a 22kg "jet", level top speed is 102m/s
65 2 : mass = 22;
66 2 : thrust_scale = (mass * GRAVITY_MSS) / hover_throttle;
67 : }
68 691 : if (strstr(frame_str, "-revthrust")) {
69 0 : reverse_thrust = true;
70 : }
71 691 : if (strstr(frame_str, "-elevon")) {
72 2 : elevons = true;
73 689 : } else if (strstr(frame_str, "-vtail")) {
74 2 : vtail = true;
75 687 : } else if (strstr(frame_str, "-dspoilers")) {
76 2 : dspoilers = true;
77 685 : } else if (strstr(frame_str, "-redundant")) {
78 6 : redundant = true;
79 : }
80 691 : if (strstr(frame_str, "-elevrev")) {
81 447 : reverse_elevator_rudder = true;
82 : }
83 691 : if (strstr(frame_str, "-catapult")) {
84 12 : have_launcher = true;
85 12 : launch_accel = 15;
86 12 : launch_time = 2;
87 : }
88 691 : if (strstr(frame_str, "-bungee")) {
89 0 : have_launcher = true;
90 0 : launch_accel = 7;
91 0 : launch_time = 4;
92 : }
93 691 : if (strstr(frame_str, "-throw")) {
94 0 : have_launcher = true;
95 0 : launch_accel = 25;
96 0 : launch_time = 0.4;
97 : }
98 691 : if (strstr(frame_str, "-tailsitter")) {
99 0 : tailsitter = true;
100 0 : ground_behavior = GROUND_BEHAVIOR_TAILSITTER;
101 0 : thrust_scale *= 1.5;
102 : }
103 691 : if (strstr(frame_str, "-steering")) {
104 0 : have_steering = true;
105 : }
106 :
107 : #if AP_FILESYSTEM_FILE_READING_ENABLED
108 691 : if (strstr(frame_str, "-3d")) {
109 7 : aerobatic = true;
110 7 : thrust_scale *= 1.5;
111 : }
112 : #endif
113 :
114 691 : if (strstr(frame_str, "-ice")) {
115 8 : ice_engine = true;
116 : }
117 :
118 691 : if (strstr(frame_str, "-soaring")) {
119 6 : mass = 2.0;
120 6 : coefficient.c_drag_p = 0.05;
121 : }
122 691 : }
123 :
124 0 : void Plane::load_coeffs(const char *model_json)
125 : {
126 0 : char *fname = nullptr;
127 : struct stat st;
128 0 : if (AP::FS().stat(model_json, &st) == 0) {
129 0 : fname = strdup(model_json);
130 : } else {
131 0 : IGNORE_RETURN(asprintf(&fname, "@ROMFS/models/%s", model_json));
132 0 : if (fname == nullptr || AP::FS().stat(fname, &st) != 0) {
133 0 : AP_HAL::panic("%s failed to load", model_json);
134 : }
135 : }
136 0 : if (fname == nullptr) {
137 0 : AP_HAL::panic("%s failed to load", model_json);
138 : }
139 0 : AP_JSON::value *obj = AP_JSON::load_json(fname);
140 0 : if (obj == nullptr) {
141 0 : AP_HAL::panic("%s failed to load", fname);
142 : }
143 :
144 : enum class VarType {
145 : FLOAT,
146 : VECTOR3F,
147 : };
148 :
149 : struct json_search {
150 : const char *label;
151 : void *ptr;
152 : VarType t;
153 : };
154 :
155 0 : json_search vars[] = {
156 : #define COFF_FLOAT(s) { #s, &coefficient.s, VarType::FLOAT }
157 0 : COFF_FLOAT(s),
158 0 : COFF_FLOAT(b),
159 0 : COFF_FLOAT(c),
160 0 : COFF_FLOAT(c_lift_0),
161 0 : COFF_FLOAT(c_lift_deltae),
162 0 : COFF_FLOAT(c_lift_a),
163 0 : COFF_FLOAT(c_lift_q),
164 0 : COFF_FLOAT(mcoeff),
165 0 : COFF_FLOAT(oswald),
166 0 : COFF_FLOAT(alpha_stall),
167 0 : COFF_FLOAT(c_drag_q),
168 0 : COFF_FLOAT(c_drag_deltae),
169 0 : COFF_FLOAT(c_drag_p),
170 0 : COFF_FLOAT(c_y_0),
171 0 : COFF_FLOAT(c_y_b),
172 0 : COFF_FLOAT(c_y_p),
173 0 : COFF_FLOAT(c_y_r),
174 0 : COFF_FLOAT(c_y_deltaa),
175 0 : COFF_FLOAT(c_y_deltar),
176 0 : COFF_FLOAT(c_l_0),
177 0 : COFF_FLOAT(c_l_p),
178 0 : COFF_FLOAT(c_l_b),
179 0 : COFF_FLOAT(c_l_r),
180 0 : COFF_FLOAT(c_l_deltaa),
181 0 : COFF_FLOAT(c_l_deltar),
182 0 : COFF_FLOAT(c_m_0),
183 0 : COFF_FLOAT(c_m_a),
184 0 : COFF_FLOAT(c_m_q),
185 0 : COFF_FLOAT(c_m_deltae),
186 0 : COFF_FLOAT(c_n_0),
187 0 : COFF_FLOAT(c_n_b),
188 0 : COFF_FLOAT(c_n_p),
189 0 : COFF_FLOAT(c_n_r),
190 0 : COFF_FLOAT(c_n_deltaa),
191 0 : COFF_FLOAT(c_n_deltar),
192 0 : COFF_FLOAT(deltaa_max),
193 0 : COFF_FLOAT(deltae_max),
194 0 : COFF_FLOAT(deltar_max),
195 0 : { "CGOffset", &coefficient.CGOffset, VarType::VECTOR3F },
196 0 : };
197 :
198 0 : for (uint8_t i=0; i<ARRAY_SIZE(vars); i++) {
199 0 : auto v = obj->get(vars[i].label);
200 0 : if (v.is<AP_JSON::null>()) {
201 : // use default value
202 0 : continue;
203 : }
204 0 : if (vars[i].t == VarType::FLOAT) {
205 0 : parse_float(v, vars[i].label, *((float *)vars[i].ptr));
206 :
207 0 : } else if (vars[i].t == VarType::VECTOR3F) {
208 0 : parse_vector3(v, vars[i].label, *(Vector3f *)vars[i].ptr);
209 :
210 : }
211 0 : }
212 :
213 0 : delete obj;
214 :
215 0 : ::printf("Loaded plane aero coefficients from %s\n", fname);
216 0 : free(fname);
217 0 : }
218 :
219 0 : void Plane::parse_float(AP_JSON::value val, const char* label, float ¶m) {
220 0 : if (!val.is<double>()) {
221 0 : AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
222 : }
223 0 : param = val.get<double>();
224 0 : }
225 :
226 0 : void Plane::parse_vector3(AP_JSON::value val, const char* label, Vector3f ¶m) {
227 0 : if (!val.is<AP_JSON::value::array>() || !val.contains(2) || val.contains(3)) {
228 0 : AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
229 : }
230 0 : for (uint8_t j=0; j<3; j++) {
231 0 : parse_float(val.get(j), label, param[j]);
232 : }
233 0 : }
234 :
235 : /*
236 : the following functions are from last_letter
237 : https://github.com/Georacer/last_letter/blob/master/last_letter/src/aerodynamicsLib.cpp
238 : many thanks to Georacer!
239 : */
240 65958815 : float Plane::liftCoeff(float alpha) const
241 : {
242 65958815 : const float alpha0 = coefficient.alpha_stall;
243 65958815 : const float M = coefficient.mcoeff;
244 65958815 : const float c_lift_0 = coefficient.c_lift_0;
245 65958815 : const float c_lift_a0 = coefficient.c_lift_a;
246 :
247 : // clamp the value of alpha to avoid exp(90) in calculation of sigmoid
248 65958815 : const float max_alpha_delta = 0.8f;
249 65958815 : if (alpha-alpha0 > max_alpha_delta) {
250 13338578 : alpha = alpha0 + max_alpha_delta;
251 52620237 : } else if (alpha0-alpha > max_alpha_delta) {
252 5353790 : alpha = alpha0 - max_alpha_delta;
253 : }
254 65958815 : double sigmoid = ( 1+exp(-M*(alpha-alpha0))+exp(M*(alpha+alpha0)) ) / (1+exp(-M*(alpha-alpha0))) / (1+exp(M*(alpha+alpha0)));
255 65958815 : double linear = (1.0-sigmoid) * (c_lift_0 + c_lift_a0*alpha); //Lift at small AoA
256 65958815 : double flatPlate = sigmoid*(2*copysign(1,alpha)*pow(sin(alpha),2)*cos(alpha)); //Lift beyond stall
257 :
258 65958815 : float result = linear+flatPlate;
259 65958815 : return result;
260 : }
261 :
262 65958815 : float Plane::dragCoeff(float alpha) const
263 : {
264 65958815 : const float b = coefficient.b;
265 65958815 : const float s = coefficient.s;
266 65958815 : const float c_drag_p = coefficient.c_drag_p;
267 65958815 : const float c_lift_0 = coefficient.c_lift_0;
268 65958815 : const float c_lift_a0 = coefficient.c_lift_a;
269 65958815 : const float oswald = coefficient.oswald;
270 :
271 65958815 : double AR = pow(b,2)/s;
272 65958815 : double c_drag_a = c_drag_p + pow(c_lift_0+c_lift_a0*alpha,2)/(M_PI*oswald*AR);
273 :
274 65958815 : return c_drag_a;
275 : }
276 :
277 : // Torque calculation function
278 65958815 : Vector3f Plane::getTorque(float inputAileron, float inputElevator, float inputRudder, float inputThrust, const Vector3f &force) const
279 : {
280 65958815 : float alpha = angle_of_attack;
281 :
282 : //calculate aerodynamic torque
283 65958815 : float effective_airspeed = airspeed;
284 :
285 65958815 : if (tailsitter || aerobatic) {
286 : /*
287 : tailsitters get airspeed from prop-wash
288 : */
289 473910 : effective_airspeed += inputThrust * 20;
290 :
291 : // reduce effective angle of attack as thrust increases
292 473910 : alpha *= constrain_float(1 - inputThrust, 0, 1);
293 : }
294 :
295 65958815 : const float s = coefficient.s;
296 65958815 : const float c = coefficient.c;
297 65958815 : const float b = coefficient.b;
298 65958815 : const float c_l_0 = coefficient.c_l_0;
299 65958815 : const float c_l_b = coefficient.c_l_b;
300 65958815 : const float c_l_p = coefficient.c_l_p;
301 65958815 : const float c_l_r = coefficient.c_l_r;
302 65958815 : const float c_l_deltaa = coefficient.c_l_deltaa;
303 65958815 : const float c_l_deltar = coefficient.c_l_deltar;
304 65958815 : const float c_m_0 = coefficient.c_m_0;
305 65958815 : const float c_m_a = coefficient.c_m_a;
306 65958815 : const float c_m_q = coefficient.c_m_q;
307 65958815 : const float c_m_deltae = coefficient.c_m_deltae;
308 65958815 : const float c_n_0 = coefficient.c_n_0;
309 65958815 : const float c_n_b = coefficient.c_n_b;
310 65958815 : const float c_n_p = coefficient.c_n_p;
311 65958815 : const float c_n_r = coefficient.c_n_r;
312 65958815 : const float c_n_deltaa = coefficient.c_n_deltaa;
313 65958815 : const float c_n_deltar = coefficient.c_n_deltar;
314 65958815 : const Vector3f &CGOffset = coefficient.CGOffset;
315 :
316 65958815 : float rho = air_density;
317 :
318 : //read angular rates
319 65958815 : double p = gyro.x;
320 65958815 : double q = gyro.y;
321 65958815 : double r = gyro.z;
322 :
323 65958815 : double qbar = 1.0/2.0*rho*pow(effective_airspeed,2)*s; //Calculate dynamic pressure
324 : double la, na, ma;
325 65958815 : if (is_zero(effective_airspeed))
326 : {
327 14748319 : la = 0;
328 14748319 : ma = 0;
329 14748319 : na = 0;
330 : }
331 : else
332 : {
333 51210496 : la = qbar*b*(c_l_0 + c_l_b*beta + c_l_p*b*p/(2*effective_airspeed) + c_l_r*b*r/(2*effective_airspeed) + c_l_deltaa*inputAileron + c_l_deltar*inputRudder);
334 51210496 : ma = qbar*c*(c_m_0 + c_m_a*alpha + c_m_q*c*q/(2*effective_airspeed) + c_m_deltae*inputElevator);
335 51210496 : na = qbar*b*(c_n_0 + c_n_b*beta + c_n_p*b*p/(2*effective_airspeed) + c_n_r*b*r/(2*effective_airspeed) + c_n_deltaa*inputAileron + c_n_deltar*inputRudder);
336 : }
337 :
338 :
339 : // Add torque to force misalignment with CG
340 : // r x F, where r is the distance from CoG to CoL
341 65958815 : la += CGOffset.y * force.z - CGOffset.z * force.y;
342 65958815 : ma += -CGOffset.x * force.z + CGOffset.z * force.x;
343 65958815 : na += -CGOffset.y * force.x + CGOffset.x * force.y;
344 :
345 65958815 : return Vector3f(la, ma, na);
346 : }
347 :
348 : // Force calculation function from last_letter
349 65958815 : Vector3f Plane::getForce(float inputAileron, float inputElevator, float inputRudder) const
350 : {
351 65958815 : const float alpha = angle_of_attack;
352 65958815 : const float c_drag_q = coefficient.c_drag_q;
353 65958815 : const float c_lift_q = coefficient.c_lift_q;
354 65958815 : const float s = coefficient.s;
355 65958815 : const float c = coefficient.c;
356 65958815 : const float b = coefficient.b;
357 65958815 : const float c_drag_deltae = coefficient.c_drag_deltae;
358 65958815 : const float c_lift_deltae = coefficient.c_lift_deltae;
359 65958815 : const float c_y_0 = coefficient.c_y_0;
360 65958815 : const float c_y_b = coefficient.c_y_b;
361 65958815 : const float c_y_p = coefficient.c_y_p;
362 65958815 : const float c_y_r = coefficient.c_y_r;
363 65958815 : const float c_y_deltaa = coefficient.c_y_deltaa;
364 65958815 : const float c_y_deltar = coefficient.c_y_deltar;
365 :
366 65958815 : float rho = air_density;
367 :
368 : //request lift and drag alpha-coefficients from the corresponding functions
369 65958815 : double c_lift_a = liftCoeff(alpha);
370 65958815 : double c_drag_a = dragCoeff(alpha);
371 :
372 : //convert coefficients to the body frame
373 65958815 : double c_x_a = -c_drag_a*cos(alpha)+c_lift_a*sin(alpha);
374 65958815 : double c_x_q = -c_drag_q*cos(alpha)+c_lift_q*sin(alpha);
375 65958815 : double c_z_a = -c_drag_a*sin(alpha)-c_lift_a*cos(alpha);
376 65958815 : double c_z_q = -c_drag_q*sin(alpha)-c_lift_q*cos(alpha);
377 :
378 : //read angular rates
379 65958815 : double p = gyro.x;
380 65958815 : double q = gyro.y;
381 65958815 : double r = gyro.z;
382 :
383 : //calculate aerodynamic force
384 65958815 : double qbar = 1.0/2.0*rho*pow(airspeed,2)*s; //Calculate dynamic pressure
385 : double ax, ay, az;
386 65958815 : if (is_zero(airspeed))
387 : {
388 14752491 : ax = 0;
389 14752491 : ay = 0;
390 14752491 : az = 0;
391 : }
392 : else
393 : {
394 51206324 : ax = qbar*(c_x_a + c_x_q*c*q/(2*airspeed) - c_drag_deltae*cos(alpha)*fabs(inputElevator) + c_lift_deltae*sin(alpha)*inputElevator);
395 : // split c_x_deltae to include "abs" term
396 51206324 : ay = qbar*(c_y_0 + c_y_b*beta + c_y_p*b*p/(2*airspeed) + c_y_r*b*r/(2*airspeed) + c_y_deltaa*inputAileron + c_y_deltar*inputRudder);
397 51206324 : az = qbar*(c_z_a + c_z_q*c*q/(2*airspeed) - c_drag_deltae*sin(alpha)*fabs(inputElevator) - c_lift_deltae*cos(alpha)*inputElevator);
398 : // split c_z_deltae to include "abs" term
399 : }
400 65958815 : return Vector3f(ax, ay, az);
401 : }
402 :
403 65958815 : void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel)
404 : {
405 65958815 : float aileron = filtered_servo_angle(input, 0);
406 65958815 : float elevator = filtered_servo_angle(input, 1);
407 65958815 : float rudder = filtered_servo_angle(input, 3);
408 65958815 : bool launch_triggered = input.servos[6] > 1700;
409 65958815 : if (reverse_elevator_rudder) {
410 32990762 : elevator = -elevator;
411 32990762 : rudder = -rudder;
412 : }
413 65958815 : if (elevons) {
414 : // fake an elevon plane
415 648653 : float ch1 = aileron;
416 648653 : float ch2 = elevator;
417 648653 : aileron = (ch2-ch1)/2.0f;
418 : // the minus does away with the need for RC2_REVERSED=-1
419 648653 : elevator = -(ch2+ch1)/2.0f;
420 :
421 : // assume no rudder
422 648653 : rudder = 0;
423 65310162 : } else if (vtail) {
424 : // fake a vtail plane
425 277685 : float ch1 = elevator;
426 277685 : float ch2 = rudder;
427 : // this matches VTAIL_OUTPUT==2
428 277685 : elevator = (ch2-ch1)/2.0f;
429 277685 : rudder = (ch2+ch1)/2.0f;
430 65032477 : } else if (dspoilers) {
431 : // fake a differential spoiler plane. Use outputs 1, 2, 4 and 5
432 276364 : float dspoiler1_left = filtered_servo_angle(input, 0);
433 276364 : float dspoiler1_right = filtered_servo_angle(input, 1);
434 276364 : float dspoiler2_left = filtered_servo_angle(input, 3);
435 276364 : float dspoiler2_right = filtered_servo_angle(input, 4);
436 276364 : float elevon_left = (dspoiler1_left + dspoiler2_left)/2;
437 276364 : float elevon_right = (dspoiler1_right + dspoiler2_right)/2;
438 276364 : aileron = (elevon_right-elevon_left)/2;
439 276364 : elevator = (elevon_left+elevon_right)/2;
440 276364 : rudder = fabsf(dspoiler1_right - dspoiler2_right)/2 - fabsf(dspoiler1_left - dspoiler2_left)/2;
441 64756113 : } else if (redundant) {
442 : // channels 1/9 are left/right ailierons
443 : // channels 2/10 are left/right elevators
444 : // channels 4/12 are top/bottom rudders
445 504794 : aileron = (filtered_servo_angle(input, 0) + filtered_servo_angle(input, 8)) / 2.0;
446 504794 : elevator = (filtered_servo_angle(input, 1) + filtered_servo_angle(input, 9)) / 2.0;
447 504794 : rudder = (filtered_servo_angle(input, 3) + filtered_servo_angle(input, 11)) / 2.0;
448 : }
449 : //printf("Aileron: %.1f elevator: %.1f rudder: %.1f\n", aileron, elevator, rudder);
450 :
451 65958815 : float thrust = reverse_thrust ? filtered_servo_angle(input, 2) : filtered_servo_range(input, 2);
452 :
453 65958815 : if (ice_engine) {
454 1398922 : thrust = icengine.update(input);
455 : } else {
456 64559893 : if (battery_is_empty()) {
457 683 : thrust = 0.0f;
458 : }
459 : }
460 :
461 : // calculate angle of attack
462 65958815 : angle_of_attack = atan2f(velocity_air_bf.z, velocity_air_bf.x);
463 65958815 : beta = atan2f(velocity_air_bf.y,velocity_air_bf.x);
464 :
465 65958815 : if (tailsitter || aerobatic) {
466 : /*
467 : tailsitters get 4x the control surfaces
468 : */
469 473910 : aileron *= 4;
470 473910 : elevator *= 4;
471 473910 : rudder *= 4;
472 : }
473 :
474 65958815 : Vector3f force = getForce(aileron, elevator, rudder);
475 65958815 : rot_accel = getTorque(aileron, elevator, rudder, thrust, force);
476 :
477 65958815 : if (have_launcher) {
478 : /*
479 : simple simulation of a launcher
480 : */
481 1558853 : if (launch_triggered) {
482 1090812 : uint64_t now = AP_HAL::millis64();
483 1090812 : if (launch_start_ms == 0) {
484 8 : launch_start_ms = now;
485 : }
486 1090812 : if (now - launch_start_ms < launch_time*1000) {
487 19205 : force.x += mass * launch_accel;
488 19205 : force.z += mass * launch_accel/3;
489 : }
490 : } else {
491 : // allow reset of catapult
492 468041 : launch_start_ms = 0;
493 : }
494 : }
495 :
496 : // simulate engine RPM
497 65958815 : motor_mask |= (1U<<2);
498 65958815 : rpm[2] = thrust * 7000;
499 :
500 : // scale thrust to newtons
501 65958815 : thrust *= thrust_scale;
502 :
503 65958815 : accel_body = Vector3f(thrust, 0, 0) + force;
504 65958815 : accel_body /= mass;
505 :
506 : // add some noise
507 65958815 : if (thrust_scale > 0) {
508 64098694 : add_noise(fabsf(thrust) / thrust_scale);
509 : }
510 :
511 65958815 : if (on_ground() && !tailsitter) {
512 : // add some ground friction
513 21063324 : Vector3f vel_body = dcm.transposed() * velocity_ef;
514 21063324 : accel_body.x -= vel_body.x * 0.3f;
515 : }
516 65958815 : }
517 :
518 : /*
519 : update the plane simulation by one time step
520 : */
521 39472855 : void Plane::update(const struct sitl_input &input)
522 : {
523 39472855 : Vector3f rot_accel;
524 :
525 39472855 : update_wind(input);
526 :
527 39472855 : calculate_forces(input, rot_accel);
528 39472855 : update_battery(input);
529 39472855 : update_dynamics(rot_accel);
530 :
531 : /*
532 : add in ground steering, this should be replaced with a proper
533 : calculation of a nose wheel effect
534 : */
535 39472855 : if (have_steering && on_ground()) {
536 0 : const float steering = filtered_servo_angle(input, 4);
537 0 : const Vector3f velocity_bf = dcm.transposed() * velocity_ef;
538 0 : const float steer_scale = radians(5);
539 0 : gyro.z += steering * velocity_bf.x * steer_scale;
540 : }
541 :
542 39472855 : update_external_payload(input);
543 :
544 : // update lat/lon/altitude
545 39472855 : update_position();
546 39472855 : time_advance();
547 :
548 : // update magnetic field
549 39472855 : update_mag_field_bf();
550 39472855 : }
551 :
552 39472855 : void Plane::update_battery(const struct sitl_input &input) {
553 39472855 : battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah);
554 :
555 39472855 : float throttle = reverse_thrust ? filtered_servo_angle(input, 2) : filtered_servo_range(input, 2);
556 39472855 : battery_current = 50.0f * sq(throttle);
557 39472855 : battery.consume_energy(battery_current, AP_HAL::micros64());
558 39472855 : battery_voltage = battery.get_voltage();
559 39472855 : battery_temperature_degC = battery.get_temperature_degC();
560 39472855 : }
|
