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 : glider model for high altitude balloon drop
17 :
18 : controls:
19 : - servo6: balloon lift, 1000 for no lift, 2000 for maximum lift
20 : - servo10: balloon cut, this cuts away the balloon when high
21 :
22 : Note that the glider starts off in a lifted by tail pose, with pitch
23 : -80 degrees. The balloon then lifts the glider above the ground. The
24 : balloon automatically bursts at a height of SIM_GLD_BLN_BRST meters,
25 : or can be cut away early with servo10.
26 :
27 : The maximum rate of the balloon lift is in SIM_GLD_BLN_RATE, in m/s
28 :
29 : To perform a takeoff first arm on the ground then use
30 : servo set 6 2000
31 : to release the ground hold. Use this to cut away the balloon:
32 : servo set 10 2000
33 :
34 : For an automatic mission, NAV_ALTITUDE_WAIT should be used to wait
35 : for a desired altitude under balloon lift. Then a DO_SET_SERVO with
36 : servo 10 and a value of 2000 to cut away the balloon.
37 : */
38 :
39 : #include "SIM_Glider.h"
40 :
41 : #if AP_SIM_GLIDER_ENABLED
42 :
43 : #include <stdio.h>
44 : #include <AP_Logger/AP_Logger.h>
45 : #include <GCS_MAVLink/GCS.h>
46 :
47 : extern const AP_HAL::HAL& hal;
48 :
49 : using namespace SITL;
50 :
51 : // SITL glider parameters
52 : const AP_Param::GroupInfo Glider::var_info[] = {
53 : // @Param: BLN_BRST
54 : // @DisplayName: balloon burst height
55 : // @Description: balloon burst height
56 : // @Units: m
57 : AP_GROUPINFO("BLN_BRST", 1, Glider, balloon_burst_amsl, 30000),
58 :
59 : // @Param: BLN_RATE
60 : // @DisplayName: balloon climb rate
61 : // @Description: balloon climb rate. If the value is less than zero then the balloon is disabled.
62 : // @Units: m/s
63 : AP_GROUPINFO("BLN_RATE", 2, Glider, balloon_rate, 5.5),
64 :
65 : AP_GROUPEND
66 : };
67 :
68 1 : Glider::Glider(const char *frame_str) :
69 1 : Aircraft(frame_str)
70 : {
71 1 : ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
72 1 : carriage_state = carriageState::WAITING_FOR_PICKUP;
73 1 : AP::sitl()->models.glider_ptr = this;
74 1 : AP_Param::setup_object_defaults(this, var_info);
75 1 : }
76 :
77 :
78 : // Torque calculation function
79 646440 : Vector3f Glider::getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const
80 : {
81 : // Calculate dynamic pressure
82 646440 : const auto &m = model;
83 646440 : double qPa = 0.5*air_density*sq(velocity_air_bf.length());
84 646440 : const float aileron_rad = inputAileron * radians(m.aileronDeflectionLimitDeg);
85 646440 : const float elevator_rad = inputElevator * radians(m.elevatorDeflectionLimitDeg);
86 646440 : const float rudder_rad = inputRudder * radians(m.rudderDeflectionLimitDeg);
87 646440 : const float tas = MAX(airspeed * eas2tas, 1);
88 :
89 646440 : float Cl = (m.Cl2 * sq(alpharad) + m.Cl1 * alpharad + m.Cl0) * betarad;
90 646440 : float Cm = m.Cm2 * sq(alpharad) + m.Cm1 * alpharad + m.Cm0;
91 646440 : float Cn = (m.Cn2 * sq(alpharad) + m.Cn1 * alpharad + m.Cn0) * betarad;
92 :
93 646440 : Cl += m.deltaClperRadianElev * elevator_rad;
94 646440 : Cm += m.deltaCmperRadianElev * elevator_rad;
95 646440 : Cn += m.deltaCnperRadianElev * elevator_rad;
96 :
97 646440 : Cl += m.deltaClperRadianRud * rudder_rad;
98 646440 : Cm += m.deltaCmperRadianRud * rudder_rad;
99 646440 : Cn += m.deltaCnperRadianRud * rudder_rad;
100 :
101 646440 : Cl += (m.deltaClperRadianAil2 * sq(alpharad) + m.deltaClperRadianAil1 * alpharad + m.deltaClperRadianAil0) * aileron_rad;
102 646440 : Cm += m.deltaCmperRadianAil * aileron_rad;
103 646440 : Cn += (m.deltaCnperRadianAil2 * sq(alpharad) + m.deltaCnperRadianAil1 * alpharad + m.deltaCnperRadianAil0) * aileron_rad;
104 :
105 : // derivatives
106 646440 : float Clp = m.Clp2 * sq(alpharad) + m.Clp1 * alpharad + m.Clp0;
107 646440 : float Clr = m.Clr2 * sq(alpharad) + m.Clr1 * alpharad + m.Clr0;
108 646440 : float Cnp = m.Cnp2 * sq(alpharad) + m.Cnp1 * alpharad + m.Cnp0;
109 646440 : float Cnr = m.Cnr2 * sq(alpharad) + m.Cnr1 * alpharad + m.Cnr0;
110 :
111 : // normalise gyro rates
112 646440 : Vector3f pqr_norm = gyro;
113 646440 : pqr_norm.x *= 0.5 * m.refSpan / tas;
114 646440 : pqr_norm.y *= 0.5 * m.refChord / tas;
115 646440 : pqr_norm.z *= 0.5 * m.refSpan / tas;
116 :
117 646440 : Cl += pqr_norm.x * Clp;
118 646440 : Cl += pqr_norm.z * Clr;
119 646440 : Cn += pqr_norm.x * Cnp;
120 646440 : Cn += pqr_norm.z * Cnr;
121 :
122 646440 : Cm += pqr_norm.y * m.Cmq;
123 :
124 646440 : float Mx = Cl * qPa * m.Sref * m.refSpan;
125 646440 : float My = Cm * qPa * m.Sref * m.refChord;
126 646440 : float Mz = Cn * qPa * m.Sref * m.refSpan;
127 :
128 :
129 : #if 0
130 : // @LoggerMessage: GLT
131 : // @Description: Simulated Glider Angles and coefficients
132 : // @Field: TimeUS: Time since system startup
133 : // @Field: Alpha: alpha angle
134 : // @Field: Beta: beta angle
135 : // @Field: Cl: lift coefficent
136 : // @Field: Cm: roll coffecient
137 : // @Field: Cn: yaw coefficient
138 : AP::logger().Write("GLT", "TimeUS,Alpha,Beta,Cl,Cm,Cn", "Qfffff",
139 : AP_HAL::micros64(),
140 : degrees(alpharad),
141 : degrees(betarad),
142 : Cl, Cm, Cn);
143 : #endif
144 :
145 646440 : return Vector3f(Mx/m.IXX, My/m.IYY, Mz/m.IZZ);
146 : }
147 :
148 : // Force calculation, return vector in Newtons
149 646440 : Vector3f Glider::getForce(float inputAileron, float inputElevator, float inputRudder)
150 : {
151 646440 : const auto &m = model;
152 646440 : const float aileron_rad = inputAileron * radians(m.aileronDeflectionLimitDeg);
153 646440 : const float elevator_rad = inputElevator * radians(m.elevatorDeflectionLimitDeg);
154 646440 : const float rudder_rad = inputRudder * radians(m.rudderDeflectionLimitDeg);
155 :
156 : // dynamic pressure
157 646440 : double qPa = 0.5*air_density*sq(velocity_air_bf.length());
158 :
159 646440 : float CA = m.CA2 * sq(alpharad) + m.CA1 * alpharad + m.CA0;
160 646440 : float CY = (m.CY2 * sq(alpharad) + m.CY1 * alpharad + m.CY0) * betarad;
161 646440 : float CN = m.CN2 * sq(alpharad) + m.CN1 * alpharad + m.CN0;
162 :
163 646440 : CN += m.deltaCNperRadianElev * elevator_rad;
164 646440 : CA += m.deltaCAperRadianElev * elevator_rad;
165 646440 : CY += m.deltaCYperRadianElev * elevator_rad;
166 :
167 646440 : CN += m.deltaCNperRadianRud * rudder_rad;
168 646440 : CA += m.deltaCAperRadianRud * rudder_rad;
169 646440 : CY += m.deltaCYperRadianRud * rudder_rad;
170 :
171 646440 : CN += m.deltaCNperRadianAil * aileron_rad;
172 646440 : CA += m.deltaCAperRadianAil * aileron_rad;
173 646440 : CY += m.deltaCYperRadianAil * aileron_rad;
174 :
175 646440 : float Fx = -CA * qPa * m.Sref;
176 646440 : float Fy = CY * qPa * m.Sref;
177 646440 : float Fz = -CN * qPa * m.Sref;
178 :
179 646440 : Vector3f ret = Vector3f(Fx, Fy, Fz);
180 :
181 646440 : float Flift = Fx * sin(alpharad) - Fz * cos(alpharad);
182 646440 : float Fdrag = -Fx * cos(alpharad) - Fz * sin(alpharad);
183 :
184 646440 : if (carriage_state == carriageState::RELEASED) {
185 585249 : uint32_t now = AP_HAL::millis();
186 585249 : sim_LD = 0.1 * constrain_float(Flift/MAX(1.0e-6,Fdrag),0,20) + 0.9 * sim_LD;
187 585249 : if (now - last_drag_ms > 10 &&
188 71834 : airspeed > 1) {
189 42065 : last_drag_ms = now;
190 : #if HAL_LOGGING_ENABLED
191 : // @LoggerMessage: SLD
192 : // @Description: Simulated Glider Dropped Calculations
193 : // @Field: TimeUS: Time since system startup
194 : // @Field: AltFt: altitude in feet
195 : // @Field: AltM: altitude in metres
196 : // @Field: EAS: equivalent airspeed
197 : // @Field: TAS: true airspeed
198 : // @Field: AD: air density
199 : // @Field: Fl: lift
200 : // @Field: Fd: drag
201 : // @Field: LD: lift/drag ratio
202 : // @Field: Elev: elevator output
203 : // @Field: AoA: angle of attack
204 : // @Field: Fx: X-axis force
205 : // @Field: Fy: Y-axis force
206 : // @Field: Fz: Z-axis force
207 : // @Field: q: air pressure
208 84130 : AP::logger().Write("SLD", "TimeUS,AltFt,AltM,EAS,TAS,AD,Fl,Fd,LD,Elev,AoA,Fx,Fy,Fz,q", "Qffffffffffffff",
209 : AP_HAL::micros64(),
210 42065 : (location.alt*0.01)/FEET_TO_METERS,
211 42065 : location.alt*0.01,
212 42065 : velocity_air_bf.length()/eas2tas,
213 42065 : velocity_air_bf.length(),
214 42065 : air_density,
215 42065 : Flift, Fdrag, sim_LD,
216 42065 : degrees(elevator_rad),
217 42065 : degrees(alpharad),
218 : Fx, Fy, Fz,
219 : qPa);
220 : // @LoggerMessage: SL2
221 : // @Description: More Simulated Glider Dropped Calculations
222 : // @Field: TimeUS: Time since system startup
223 : // @Field: AltFt: altitude in feet
224 : // @Field: KEAS: equivalent airspeed in knots
225 : // @Field: KTAS: true airspeed in knots
226 : // @Field: AD: air density
227 : // @Field: Fl: lift
228 : // @Field: Fd: drag
229 : // @Field: LD: lift/drag ratio
230 : // @Field: Elev: elevator output
231 : // @Field: Ail: aileron output
232 : // @Field: Rud: rudder output
233 : // @Field: AoA: Angle of Attack
234 : // @Field: SSA: Side Slip Angle
235 : // @Field: q: air pressire
236 : // @Field: Az: z-axis body-frame acceleration
237 84130 : AP::logger().Write("SL2", "TimeUS,AltFt,KEAS,KTAS,AD,Fl,Fd,LD,Elev,Ail,Rud,AoA,SSA,q,Az", "Qffffffffffffff",
238 : AP_HAL::micros64(),
239 42065 : (location.alt*0.01)/FEET_TO_METERS,
240 42065 : M_PER_SEC_TO_KNOTS*velocity_air_bf.length()/eas2tas,
241 42065 : M_PER_SEC_TO_KNOTS*velocity_air_bf.length(),
242 42065 : air_density,
243 42065 : Flift, Fdrag, sim_LD,
244 42065 : degrees(elevator_rad),
245 42065 : degrees(aileron_rad),
246 42065 : degrees(rudder_rad),
247 42065 : degrees(alpharad),
248 42065 : degrees(betarad),
249 : qPa,
250 42065 : accel_body.z);
251 :
252 : // @LoggerMessage: SCTL
253 : // @Description: Simulated Glider Drop control outputs
254 : // @Field: TimeUS: Time since system startup
255 : // @Field: Ail: aileron output
256 : // @Field: Elev: elevator output
257 : // @Field: Rudd: rudder output
258 84130 : AP::logger().Write("SCTL", "TimeUS,Ail,Elev,Rudd", "Qfff",
259 : AP_HAL::micros64(),
260 42065 : degrees(aileron_rad),
261 42065 : degrees(elevator_rad),
262 42065 : degrees(rudder_rad));
263 : #endif // HAL_LOGGING_ENABLED
264 : }
265 : }
266 :
267 :
268 646440 : return ret;
269 : }
270 :
271 1768371 : void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
272 : {
273 1768371 : filtered_servo_setup(1, 1100, 1900, model.aileronDeflectionLimitDeg);
274 1768371 : filtered_servo_setup(4, 1100, 1900, model.aileronDeflectionLimitDeg);
275 1768371 : filtered_servo_setup(2, 1100, 1900, model.elevatorDeflectionLimitDeg);
276 1768371 : filtered_servo_setup(3, 1100, 1900, model.rudderDeflectionLimitDeg);
277 :
278 1768371 : float aileron = 0.5*(filtered_servo_angle(input, 1) + filtered_servo_angle(input, 4));
279 1768371 : float elevator = filtered_servo_angle(input, 2);
280 1768371 : float rudder = filtered_servo_angle(input, 3);
281 1768371 : float balloon = MAX(0.0f, filtered_servo_range(input, 5)); // Don't let the balloon receive downwards commands.
282 1768371 : float balloon_cut = filtered_servo_range(input, 9);
283 :
284 : // Move balloon upwards using balloon velocity from channel 6
285 : // Aircraft is released from ground constraint when channel 6 PWM > 1010
286 : // Once released, plane will be dropped when balloon_burst_amsl is reached or channel 6 is set to PWM 1000
287 1768371 : if (carriage_state == carriageState::WAITING_FOR_RELEASE) {
288 1118467 : balloon_velocity = Vector3f(-wind_ef.x, -wind_ef.y, -wind_ef.z -balloon_rate * balloon);
289 1118467 : balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
290 1118467 : const float height_AMSL = 0.01f * (float)home.alt - position.z;
291 : // release at burst height or when balloon cut output goes high
292 2236934 : if (hal.scheduler->is_system_initialized() &&
293 1118467 : (height_AMSL > balloon_burst_amsl || balloon_cut > 0.8)) {
294 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "pre-release at %i m AMSL", (int)height_AMSL);
295 1 : carriage_state = carriageState::PRE_RELEASE;
296 : }
297 649904 : } else if (carriage_state == carriageState::PRE_RELEASE) {
298 : // slow down for release
299 2397 : balloon_velocity *= 0.999;
300 2397 : balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
301 2397 : if (balloon_velocity.length() < 0.5) {
302 1 : carriage_state = carriageState::RELEASED;
303 1 : use_smoothing = false;
304 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "released at %.0f m AMSL", (0.01f * home.alt) - position.z);
305 : }
306 647507 : } else if (carriage_state == carriageState::WAITING_FOR_PICKUP) {
307 : // Don't allow the balloon to drag sideways until the pickup
308 62259 : balloon_velocity = Vector3f(0.0f, 0.0f, -balloon_rate * balloon);
309 62259 : balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
310 : }
311 :
312 : // calculate angle of attack
313 1768371 : alpharad = atan2f(velocity_air_bf.z, velocity_air_bf.x);
314 1768371 : betarad = atan2f(velocity_air_bf.y,velocity_air_bf.x);
315 :
316 1768371 : alpharad = constrain_float(alpharad, -model.alphaRadMax, model.alphaRadMax);
317 1768371 : betarad = constrain_float(betarad, -model.betaRadMax, model.betaRadMax);
318 :
319 1768371 : Vector3f force;
320 :
321 1768371 : if (!update_balloon(balloon, force, rot_accel)) {
322 646440 : force = getForce(aileron, elevator, rudder);
323 646440 : rot_accel = getTorque(aileron, elevator, rudder, force);
324 : }
325 :
326 1768371 : accel_body = force / model.mass;
327 :
328 1768371 : if (on_ground()) {
329 : // add some ground friction
330 29537 : Vector3f vel_body = dcm.transposed() * velocity_ef;
331 29537 : accel_body.x -= vel_body.x * 0.3f;
332 : }
333 :
334 : // constrain accelerations
335 1768371 : accel_body.x = constrain_float(accel_body.x, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
336 1768371 : accel_body.y = constrain_float(accel_body.y, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
337 1768371 : accel_body.z = constrain_float(accel_body.z, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
338 :
339 1768371 : }
340 :
341 : /*
342 : update the plane simulation by one time step
343 : */
344 1768371 : void Glider::update(const struct sitl_input &input)
345 : {
346 1768371 : Vector3f rot_accel;
347 :
348 1768371 : update_wind(input);
349 :
350 1768371 : calculate_forces(input, rot_accel, accel_body);
351 :
352 1768371 : if (carriage_state == carriageState::WAITING_FOR_PICKUP) {
353 : // Handle special case where plane is being held nose down waiting to be lifted
354 62258 : accel_body = dcm.transposed() * Vector3f(0.0f, 0.0f, -GRAVITY_MSS);
355 62258 : velocity_ef.zero();
356 62258 : gyro.zero();
357 62258 : dcm.from_euler(0.0f, radians(-80.0f), radians(home_yaw));
358 62258 : use_smoothing = true;
359 62258 : adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
360 : } else {
361 1706113 : update_dynamics(rot_accel);
362 : }
363 1768371 : update_external_payload(input);
364 :
365 : // update lat/lon/altitude
366 1768371 : update_position();
367 1768371 : time_advance();
368 :
369 : // update magnetic field
370 1768371 : update_mag_field_bf();
371 :
372 1768371 : update_battery();
373 1768371 : }
374 :
375 : /*
376 : return true if we are on the ground
377 : */
378 6886710 : bool Glider::on_ground() const
379 : {
380 6886710 : switch (carriage_state) {
381 2340996 : case carriageState::NONE:
382 : case carriageState::RELEASED:
383 2340996 : return hagl() <= 0.001;
384 4545714 : case carriageState::WAITING_FOR_PICKUP:
385 : case carriageState::WAITING_FOR_RELEASE:
386 : case carriageState::PRE_RELEASE:
387 : // prevent bouncing around ground
388 : // don't do ground interaction if being carried
389 4545714 : break;
390 : }
391 4545714 : return false;
392 : }
393 :
394 : /*
395 : implement balloon lift
396 : controlled by SIM_BLN_BRST and SIM_BLN_RATE
397 : */
398 1768371 : bool Glider::update_balloon(float balloon, Vector3f &force, Vector3f &rot_accel)
399 : {
400 : // by setting a negative balloon rate we disable the balloon
401 1768371 : if (balloon_rate < 0) {
402 0 : carriage_state = carriageState::RELEASED;
403 : }
404 :
405 1768371 : if (!hal.util->get_soft_armed()) {
406 63107 : return false;
407 : }
408 :
409 1705264 : switch (carriage_state) {
410 583333 : case carriageState::NONE:
411 : case carriageState::RELEASED:
412 : // balloon not active
413 583333 : disable_origin_movement = false;
414 583333 : return false;
415 :
416 1121931 : case carriageState::WAITING_FOR_PICKUP:
417 : case carriageState::WAITING_FOR_RELEASE:
418 : case carriageState::PRE_RELEASE:
419 : // while under balloon disable origin movement to allow for balloon position calculations
420 1121931 : disable_origin_movement = true;
421 1121931 : break;
422 : }
423 :
424 : // assume a 50m tether with a 1Hz pogo frequency and damping ratio of 0.2
425 1121931 : Vector3f tether_pos_bf{-1.0f,0.0f,0.0f}; // tether attaches to vehicle tail approx 1m behind c.g.
426 1121931 : const float omega = model.tetherPogoFreq * M_2PI; // rad/sec
427 1121931 : const float zeta = 0.7f;
428 1121931 : float tether_stiffness = model.mass * sq(omega); // N/m
429 1121931 : float tether_damping = 2.0f * zeta * omega / model.mass; // N/(m/s)
430 : // NED relative position vector from tether attachment on plane to balloon attachment
431 1121931 : Vector3f relative_position = balloon_position - (position.tofloat() + (dcm * tether_pos_bf));
432 1121931 : const float separation_distance = relative_position.length();
433 :
434 : // NED unit vector pointing from tether attachment on plane to attachment on balloon
435 1121931 : Vector3f tether_unit_vec_ef = relative_position.normalized();
436 :
437 : // NED velocity of attachment point on plane
438 1121931 : Vector3f attachment_velocity_ef = velocity_ef + dcm * (gyro % tether_pos_bf);
439 :
440 : // NED velocity of attachment point on balloon as seen by observer on attachemnt point on plane
441 1121931 : Vector3f relative_velocity = balloon_velocity - attachment_velocity_ef;
442 :
443 1121931 : float separation_speed = relative_velocity * tether_unit_vec_ef;
444 :
445 : // rate increase in separation between attachment point on plane and balloon
446 : // tension force in tether due to stiffness and damping
447 1121931 : float tension_force = MAX(0.0f, (separation_distance - model.tetherLength) * tether_stiffness);
448 1121931 : if (tension_force > 0.0f) {
449 1113304 : tension_force += constrain_float(separation_speed * tether_damping, 0.0f, 0.05f * tension_force);
450 : }
451 :
452 1121931 : if (carriage_state == carriageState::WAITING_FOR_PICKUP && tension_force > 1.2f * model.mass * GRAVITY_MSS && balloon > 0.01f) {
453 1 : carriage_state = carriageState::WAITING_FOR_RELEASE;
454 : }
455 :
456 1121931 : if (carriage_state == carriageState::WAITING_FOR_RELEASE ||
457 3464 : carriage_state == carriageState::PRE_RELEASE) {
458 1120864 : Vector3f tension_force_vector_ef = tether_unit_vec_ef * tension_force;
459 1120864 : Vector3f tension_force_vector_bf = dcm.transposed() * tension_force_vector_ef;
460 1120864 : force = tension_force_vector_bf;
461 :
462 : // drag force due to lateral motion assuming projected area from Y is 20% of projected area seen from Z and
463 : // assuming bluff body drag characteristic. In reality we would need an aero model that worked flying backwards,
464 : // but this will have to do for now.
465 1120864 : Vector3f aero_force_bf = Vector3f(0.0f, 0.2f * velocity_air_bf.y * fabsf(velocity_air_bf.y), velocity_air_bf.z * fabsf(velocity_air_bf.z));
466 1120864 : aero_force_bf *= air_density * model.Sref;
467 1120864 : force -= aero_force_bf;
468 :
469 1120864 : Vector3f tension_moment_vector_bf = tether_pos_bf % tension_force_vector_bf;
470 1120864 : Vector3f tension_rot_accel = Vector3f(tension_moment_vector_bf.x/model.IXX, tension_moment_vector_bf.y/model.IYY, tension_moment_vector_bf.z/model.IZZ);
471 1120864 : rot_accel = tension_rot_accel;
472 :
473 : // add some rotation damping due to air resistance assuming a 2 sec damping time constant at SL density
474 : // TODO model roll damping with more accuracy using Clp data for zero alpha as a first approximation
475 1120864 : rot_accel -= gyro * 0.5 * air_density;
476 1120864 : } else {
477 : // tether is either slack awaiting pickup or released
478 1067 : rot_accel.zero();
479 1067 : force = dcm.transposed() * Vector3f(0.0f, 0.0f, -GRAVITY_MSS * model.mass);
480 : }
481 :
482 : // balloon is active
483 1121931 : return true;
484 : }
485 :
486 : #endif // AP_SIM_GLIDER_ENABLED
|
