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 : Blimp simulator class
17 : */
18 :
19 : #include "SIM_Blimp.h"
20 : #include <AP_Logger/AP_Logger.h>
21 :
22 : #include <stdio.h>
23 :
24 : using namespace SITL;
25 :
26 : extern const AP_HAL::HAL& hal;
27 :
28 0 : Blimp::Blimp(const char *frame_str) :
29 0 : Aircraft(frame_str)
30 : {
31 0 : mass = 0.07; //i.e. 70g
32 0 : radius = 0.25; // i.e. 25 cm
33 0 : moment_of_inertia = {0.004375, 0.004375, 0.004375}; //m*r^2 for hoop...
34 0 : cog = {0, 0, 0.1}; //10 cm down from center (i.e. center of buoyancy), for now
35 0 : k_tan = 0.6e-7; //Tangential (thrust) and normal force multipliers for the fins
36 0 : k_nor = 0;
37 0 : k_m = 0.15; //Thrust multiplier for motors
38 0 : gondolawidth = 0.1; //10 cm
39 0 : drag_constant = 0.04;
40 0 : drag_gyr_constant = 0.15;
41 : // linear drag dominates at low speed/rate; without it quadratic-only
42 : // drag decays as 1/t and the vehicle never quite stops moving/yawing
43 0 : drag_lin_constant = 0.008;
44 0 : drag_gyr_lin_constant = 0.035;
45 :
46 0 : lock_step_scheduled = true;
47 :
48 0 : constexpr float default_battery_resistance_ohm = 0.01f;
49 0 : battery.setup(sitl->batt_capacity_ah,
50 : default_battery_resistance_ohm,
51 0 : sitl->batt_voltage,
52 : ambient_outside_temperature_degC());
53 :
54 0 : ::printf("Starting Blimp model\n");
55 :
56 0 : if (strstr(frame_str, "motor")) {
57 0 : motorblimp = true;
58 0 : ::printf("Running motorblimp frame.\n");
59 : }
60 0 : }
61 :
62 : // calculate rotational and linear accelerations
63 0 : void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc, Vector3f &rot_accel)
64 : {
65 0 : float delta_time = frame_time_us * 1.0e-6f;
66 :
67 0 : if (!hal.scheduler->is_system_initialized()) {
68 0 : return;
69 : }
70 :
71 0 : if (!motorblimp) { //Finned blimp
72 : //all fin setup
73 0 : for (uint8_t i=0; i<4; i++) {
74 0 : fin[i].last_angle = fin[i].angle;
75 0 : if (!battery_is_empty()) {
76 0 : if (input.servos[i] == 0) {
77 0 : fin[i].angle = 0;
78 0 : fin[i].servo_angle = 0;
79 : } else {
80 : // filtered_servo_angle() normalises PWM against a fixed 1500 +/- 500
81 : // range, but blimp.parm uses SERVOn_MIN 500 / TRIM 1350 / MAX 2200;
82 : // the +13.5 degree offset recentres that asymmetric range to a
83 : // symmetric -76.5..+76.5 degrees with 0 degrees at servo trim
84 0 : fin[i].angle = filtered_servo_angle(input, i)*radians(45.0f)+radians(13.5);
85 0 : fin[i].servo_angle = filtered_servo_angle(input, i);
86 : }
87 : }
88 :
89 0 : if (fin[i].angle < fin[i].last_angle) fin[i].dir = 0; //thus 0 = "angle is reducing"
90 0 : else fin[i].dir = 1;
91 :
92 0 : fin[i].vel = degrees(fin[i].angle - fin[i].last_angle)/delta_time; //Could also do multi-point derivative filter - DerivativeFilter.cpp
93 : //deg/s (should really be rad/s, but that would require modifying k_tan, k_nor)
94 : //all other angles should be in radians.
95 0 : fin[i].vel = constrain_float(fin[i].vel, -450, 450);
96 0 : fin[i].T = sq(fin[i].vel) * k_tan;
97 0 : fin[i].N = sq(fin[i].vel) * k_nor;
98 0 : if (fin[i].dir == 0) fin[i].N = -fin[i].N; //normal force flips when fin changes direction
99 :
100 0 : fin[i].Fx = 0;
101 0 : fin[i].Fy = 0;
102 0 : fin[i].Fz = 0;
103 : }
104 :
105 : //Back fin
106 0 : fin[0].Fx = fin[0].T*cos(fin[0].angle);// + fin[0].N*sin(fin[0].angle); //causes forward movement
107 0 : fin[0].Fz = fin[0].T*sin(fin[0].angle);// - fin[0].N*cos(fin[0].angle); //causes height & wobble in y
108 :
109 : //Front fin
110 0 : fin[1].Fx = -fin[1].T*cos(fin[1].angle);// - fin[1].N*sin(fin[1].angle); //causes backward movement
111 0 : fin[1].Fz = fin[1].T*sin(fin[1].angle);// - fin[1].N*cos(fin[1].angle); //causes height & wobble in y
112 :
113 : //Right fin
114 0 : fin[2].Fy = -fin[2].T*cos(fin[2].angle);// - fin[2].N*sin(fin[2].angle); //causes left movement
115 0 : fin[2].Fx = fin[2].T*sin(fin[2].angle);// - fin[2].N*cos(fin[2].angle); //cause yaw & wobble in z
116 :
117 : //Left fin
118 0 : fin[3].Fy = fin[3].T*cos(fin[3].angle);// + fin[3].N*sin(fin[3].angle); //causes right movement
119 0 : fin[3].Fx = fin[3].T*sin(fin[3].angle);// + fin[3].N*cos(fin[3].angle); //causes yaw & wobble in z
120 :
121 0 : Vector3f F_BF{0,0,0};
122 0 : for (uint8_t i=0; i<4; i++) {
123 0 : F_BF.x = F_BF.x + fin[i].Fx;
124 0 : F_BF.y = F_BF.y + fin[i].Fy;
125 0 : F_BF.z = F_BF.z + fin[i].Fz;
126 : }
127 :
128 0 : body_acc.x = F_BF.x/mass; //mass in kg, thus accel in m/s/s
129 0 : body_acc.y = F_BF.y/mass;
130 0 : body_acc.z = F_BF.z/mass;
131 :
132 0 : Vector3f rot_T{0,0,0};
133 :
134 : #if HAL_LOGGING_ENABLED
135 : // @LoggerMessage: SFT
136 : // @Description: Simulated Blimp Fin Thrust
137 : // @Field: TimeUS: Time since system startup
138 : // @Field: f0: Fin 0 tangential thrust
139 : // @Field: f1: Fin 1 tangential thrust
140 : // @Field: f2: Fin 2 tangential thrust
141 : // @Field: f3: Fin 3 tangential thrust
142 0 : AP::logger().WriteStreaming("SFT", "TimeUS,f0,f1,f2,f3",
143 : "Qffff",
144 : AP_HAL::micros64(),
145 0 : fin[0].T, fin[1].T, fin[2].T, fin[3].T);
146 : // @LoggerMessage: SFN
147 : // @Description: Simulated Blimp Fin Forces
148 : // @Field: TimeUS: Time since system startup
149 : // @Field: n0: Fin 0 normal force
150 : // @Field: n1: Fin 1 normal force
151 : // @Field: n2: Fin 2 normal force
152 : // @Field: n3: Fin 3 normal force
153 0 : AP::logger().WriteStreaming("SFN", "TimeUS,n0,n1,n2,n3",
154 : "Qffff",
155 : AP_HAL::micros64(),
156 0 : fin[0].N, fin[1].N, fin[2].N, fin[3].N);
157 : // @LoggerMessage: SBA1
158 : // @Description: Simulated Blimp Body-Frame accelerations
159 : // @Field: TimeUS: Time since system startup
160 : // @Field: ax: x-axis acceleration
161 : // @Field: ay: y-axis acceleration
162 : // @Field: az: z-axis acceleration
163 0 : AP::logger().WriteStreaming("SBA1", "TimeUS,ax,ay,az",
164 : "Qfff",
165 : AP_HAL::micros64(),
166 0 : body_acc.x, body_acc.y, body_acc.z);
167 : // @LoggerMessage: SFA1
168 : // @Description: Simulated Blimp Fin Angles
169 : // @Field: TimeUS: Time since system startup
170 : // @Field: f0: fin 0 angle
171 : // @Field: f1: fin 1 angle
172 : // @Field: f2: fin 2 angle
173 : // @Field: f3: fin 3 angle
174 0 : AP::logger().WriteStreaming("SFA1", "TimeUS,f0,f1,f2,f3",
175 : "Qffff",
176 : AP_HAL::micros64(),
177 0 : fin[0].angle, fin[1].angle, fin[2].angle, fin[3].angle);
178 : // @LoggerMessage: SFAN
179 : // @Description: Simulated Blimp Servo Angles
180 : // @Field: TimeUS: Time since system startup
181 : // @Field: f0: fin 0 servo angle
182 : // @Field: f1: fin 1 servo angle
183 : // @Field: f2: fin 2 servo angle
184 : // @Field: f3: fin 3 servo angle
185 0 : AP::logger().WriteStreaming("SFAN", "TimeUS,f0,f1,f2,f3",
186 : "Qffff",
187 : AP_HAL::micros64(),
188 0 : fin[0].servo_angle, fin[1].servo_angle, fin[2].servo_angle, fin[3].servo_angle);
189 : // @LoggerMessage: SSAN
190 : // @Description: Simulated Blimp Servo Inputs
191 : // @Field: TimeUS: Time since system startup
192 : // @Field: f0: fin 0 servo angle input
193 : // @Field: f1: fin 1 servo angle input
194 : // @Field: f2: fin 2 servo angle input
195 : // @Field: f3: fin 3 servo angle input
196 0 : AP::logger().WriteStreaming("SSAN", "TimeUS,f0,f1,f2,f3",
197 : "QHHHH",
198 : AP_HAL::micros64(),
199 0 : input.servos[0], input.servos[1], input.servos[2], input.servos[3]);
200 : // @LoggerMessage: SFV1
201 : // @Description: Simulated Blimp Fin Velocities
202 : // @Field: TimeUS: Time since system startup
203 : // @Field: f0: fin 0 velocity
204 : // @Field: f1: fin 1 velocity
205 : // @Field: f2: fin 2 velocity
206 : // @Field: f3: fin 3 velocity
207 0 : AP::logger().WriteStreaming("SFV1", "TimeUS,f0,f1,f2,f3",
208 : "Qffff",
209 : AP_HAL::micros64(),
210 0 : fin[0].vel, fin[1].vel, fin[2].vel, fin[3].vel);
211 : // @LoggerMessage: SRT1
212 : // @Description: Simulated Blimp Rotational forces
213 : // @Field: TimeUS: Time since system startup
214 : // @Field: rtx: zero
215 : // @Field: rty: zero
216 : // @Field: rtz: zero
217 0 : AP::logger().WriteStreaming("SRT1", "TimeUS,rtx,rty,rtz",
218 : "Qfff",
219 : AP_HAL::micros64(),
220 0 : rot_T.x, rot_T.y, rot_T.z);
221 : #endif // HAL_LOGGING_ENABLED
222 :
223 : #if 0 //"Wobble" attempt
224 : rot_T.y = fin[0].Fz * radius + fin[1].Fz * radius;
225 : // @LoggerMessage: SRT2
226 : // @Description: Transformed Simulated Blimp Rotational forces
227 : // @Field: TimeUS: Time since system startup
228 : // @Field: rtx: x-axis wobble rotational force
229 : // @Field: rty: y-axis wobble rotational force
230 : // @Field: rtz: z-axis wobble rotational force
231 : AP::logger().WriteStreaming("SRT2", "TimeUS,rtx,rty,rtz",
232 : "Qfff",
233 : AP_HAL::micros64(),
234 : rot_T.x, rot_T.y, rot_T.z);
235 : // the blimp has pendulum stability due to the centre of gravity being lower than the centre of buoyancy
236 : Vector3f ang; //x,y,z correspond to roll, pitch, yaw.
237 : dcm.to_euler(&ang.x, &ang.y, &ang.z); //rpy in radians
238 : Vector3f ang_ef = dcm * ang;
239 : rot_T.x -= mass*GRAVITY_MSS*sinf(M_PI-ang_ef.x)/cog.z;
240 : rot_T.y -= mass*GRAVITY_MSS*sinf(M_PI-ang_ef.y)/cog.z;
241 : // @LoggerMessage: SRT3
242 : // @Description: Simulated Blimp Torques
243 : // @Field: TimeUS: Time since system startup
244 : // @Field: rtx: torque around x axis
245 : // @Field: rty: torque around y axis
246 : // @Field: rtz: torque around z axis
247 : AP::logger().WriteStreaming("SRT3", "TimeUS,rtx,rty,rtz",
248 : "Qfff",
249 : AP_HAL::micros64(),
250 : rot_T.x, rot_T.y, rot_T.z);
251 : // @LoggerMessage: SAN1
252 : // @Description: Simulated Blimp Angles
253 : // @Field: TimeUS: Time since system startup
254 : // @Field: anx: x angle
255 : // @Field: any: y angle
256 : // @Field: anz: z angle
257 : AP::logger().WriteStreaming("SAN1", "TimeUS,anx,any,anz",
258 : "Qfff",
259 : AP_HAL::micros64(),
260 : ang.x, ang.y, ang.z);
261 : // @LoggerMessage: SAN2
262 : // @Description: Simulated Blimp Angles
263 : // @Field: TimeUS: Time since system startup
264 : // @Field: anx: x earth-frame angle
265 : // @Field: any: y earth-frame angle
266 : // @Field: anz: z earth-frame angle
267 : AP::logger().WriteStreaming("SAN2", "TimeUS,anx,any,anz",
268 : "Qfff",
269 : AP_HAL::micros64(),
270 : ang_ef.x, ang_ef.y, ang_ef.z);
271 : // @LoggerMessage: SAF1
272 : // @Description: Simulated Blimp Sin-Angles
273 : // @Field: TimeUS: Time since system startup
274 : // @Field: afx: sin(x angle)
275 : // @Field: afy: sin(y angle)
276 : // @Field: afz: sin(z angle)
277 : AP::logger().WriteStreaming("SAF1", "TimeUS,afx,afy,afz",
278 : "Qfff",
279 : AP_HAL::micros64(),
280 : sinf(ang.x), sinf(ang.y), sinf(ang.z));
281 : // @LoggerMessage: SMGC
282 : // @Description: Simulated Blimp Mass and COG
283 : // @Field: TimeUS: Time since system startup
284 : // @Field: m: mass
285 : // @Field: g: gravity
286 : // @Field: cz: centre-of-gravity, z-axis
287 : AP::logger().WriteStreaming("SMGC", "TimeUS,m,g,cz",
288 : "Qfff",
289 : AP_HAL::micros64(),
290 : mass, GRAVITY_MSS, cog.z);
291 : #endif
292 :
293 : //Blimp yaw
294 0 : rot_T.z = fin[2].Fx * radius - fin[3].Fx * radius;//in N*m (Torque = force * lever arm)
295 : //rot accel = torque / moment of inertia
296 : //Torque = moment force.
297 0 : rot_accel.x = rot_T.x / moment_of_inertia.x;
298 0 : rot_accel.y = rot_T.y / moment_of_inertia.y;
299 0 : rot_accel.z = rot_T.z / moment_of_inertia.z;
300 :
301 : } else { //MotorBlimp
302 0 : for (uint8_t i=0; i<4; i++) {
303 0 : if (battery_is_empty() || input.servos[i] == 0) {
304 0 : mot[i].throttle = 0;
305 : } else {
306 0 : mot[i].throttle = filtered_servo_angle(input, i);
307 : }
308 0 : mot[i].thrust = mot[i].throttle * k_m;
309 :
310 0 : mot[i].Fx = 0;
311 0 : mot[i].Fy = 0;
312 0 : mot[i].Fz = 0;
313 : }
314 :
315 : //FrontLeft motor
316 0 : mot[0].Fx = mot[0].thrust;
317 : //FrontRight motor
318 0 : mot[1].Fx = mot[1].thrust;
319 : //Up motor
320 0 : mot[2].Fz = -mot[2].thrust;
321 : //Right motor
322 0 : mot[3].Fy = mot[3].thrust;
323 :
324 0 : Vector3f F_BF{0,0,0};
325 0 : for (uint8_t i=0; i<4; i++) {
326 0 : F_BF.x = F_BF.x + mot[i].Fx;
327 0 : F_BF.y = F_BF.y + mot[i].Fy;
328 0 : F_BF.z = F_BF.z + mot[i].Fz;
329 : }
330 0 : body_acc.x = F_BF.x/mass; //mass in kg, thus accel in m/s/s
331 0 : body_acc.y = F_BF.y/mass;
332 0 : body_acc.z = F_BF.z/mass;
333 :
334 0 : Vector3f rot_T{0,0,0};
335 0 : rot_T.z = mot[0].thrust * gondolawidth - mot[1].thrust * gondolawidth; //in N*m (Torque = force * lever arm)
336 : //skipping swing effect generated by the motors.
337 0 : rot_accel.x = rot_T.x / moment_of_inertia.x;
338 0 : rot_accel.y = rot_T.y / moment_of_inertia.y;
339 0 : rot_accel.z = rot_T.z / moment_of_inertia.z;
340 : }
341 : }
342 :
343 : /*
344 : update the blimp simulation by one time step
345 : */
346 0 : void Blimp::update(const struct sitl_input &input)
347 : {
348 0 : float delta_time = frame_time_us * 1.0e-6f;
349 :
350 0 : Vector3f rot_accel = Vector3f(0,0,0);
351 0 : calculate_forces(input, accel_body, rot_accel);
352 :
353 0 : if (hal.scheduler->is_system_initialized()) {
354 : // quadratic rotational drag, dominant at higher rates
355 0 : float gyr_sq = gyro.length_squared();
356 0 : if (is_positive(gyr_sq)) {
357 0 : Vector3f force_gyr = (gyro.normalized() * drag_gyr_constant * gyr_sq);
358 0 : Vector3f ef_drag_accel_gyr = -force_gyr / mass;
359 0 : Vector3f bf_drag_accel_gyr = dcm.transposed() * ef_drag_accel_gyr;
360 0 : rot_accel += bf_drag_accel_gyr;
361 : }
362 : // linear rotational drag so residual rates decay to zero
363 0 : rot_accel -= gyro * (drag_gyr_lin_constant / mass);
364 : }
365 :
366 : // update rotational rates in body frame
367 0 : gyro += rot_accel * delta_time;
368 :
369 0 : gyro.x = constrain_float(gyro.x, -radians(2000.0f), radians(2000.0f));
370 0 : gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
371 0 : gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
372 :
373 : // Vector3f ang; //x,y,z correspond to roll, pitch, yaw.
374 : // dcm.to_euler(&ang.x, &ang.y, &ang.z); //rpy in radians
375 : // dcm.from_euler(0.0f, 0.0f, ang.z);
376 : // update attitude
377 0 : dcm.rotate(gyro * delta_time);
378 0 : dcm.normalize();
379 :
380 0 : if (hal.scheduler->is_system_initialized()) {
381 : // quadratic translational drag, dominant at higher speeds
382 0 : float speed_sq = velocity_ef.length_squared();
383 0 : if (is_positive(speed_sq)) {
384 0 : Vector3f force = (velocity_ef.normalized() * drag_constant * speed_sq);
385 0 : Vector3f ef_drag_accel = -force / mass;
386 0 : Vector3f bf_drag_accel = dcm.transposed() * ef_drag_accel;
387 0 : accel_body += bf_drag_accel;
388 : }
389 : // linear translational drag so residual velocity decays to zero
390 0 : accel_body -= (dcm.transposed() * velocity_ef) * (drag_lin_constant / mass);
391 :
392 : // add lifting force exactly equal to gravity, for neutral buoyancy (buoyancy in ef)
393 0 : accel_body += dcm.transposed() * Vector3f(0,0,-GRAVITY_MSS);
394 : }
395 :
396 0 : Vector3f accel_earth = dcm * accel_body;
397 0 : accel_earth += Vector3f(0.0f, 0.0f, GRAVITY_MSS); //add gravity
398 0 : velocity_ef += accel_earth * delta_time;
399 0 : position += (velocity_ef * delta_time).todouble(); //update position vector
400 :
401 0 : update_position(); //updates the position from the Vector3f position
402 0 : time_advance();
403 0 : update_mag_field_bf();
404 0 : rate_hz = sitl->loop_rate_hz;
405 :
406 0 : update_battery();
407 0 : }
408 :
409 0 : void Blimp::update_battery()
410 : {
411 0 : battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah);
412 0 : battery_current = 0.0f;
413 :
414 0 : if (!battery_is_empty()) {
415 0 : if (motorblimp) {
416 0 : constexpr float motor_current_scaler_amps = 2.0f;
417 0 : for (uint8_t i=0; i<4; i++) {
418 0 : battery_current += fabsf(mot[i].throttle) * motor_current_scaler_amps;
419 : }
420 : } else {
421 0 : constexpr float fin_current_scaler_amps = 0.01f;
422 0 : for (uint8_t i=0; i<4; i++) {
423 0 : battery_current += fabsf(fin[i].vel) * fin_current_scaler_amps;
424 : }
425 : }
426 : }
427 :
428 0 : battery.consume_energy(battery_current, AP_HAL::micros64());
429 0 : battery_voltage = battery.get_voltage();
430 0 : battery_temperature_degC = battery.get_temperature_degC();
431 0 : }
|
