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 : StratoBlimp simulator class
17 : */
18 :
19 : #include "SIM_StratoBlimp.h"
20 :
21 : #if AP_SIM_STRATOBLIMP_ENABLED
22 :
23 : #include <AP_Logger/AP_Logger.h>
24 : #include <AP_Motors/AP_Motors.h>
25 :
26 : #include <stdio.h>
27 :
28 : using namespace SITL;
29 :
30 : extern const AP_HAL::HAL& hal;
31 :
32 : // SITL StratoBlimp parameters
33 : const AP_Param::GroupInfo StratoBlimp::var_info[] = {
34 : // @Param: MASS
35 : // @DisplayName: mass
36 : // @Description: mass of blimp not including lifting gas
37 : // @Units: kg
38 : AP_GROUPINFO("MASS", 1, StratoBlimp, mass, 80),
39 :
40 : // @Param: HMASS
41 : // @DisplayName: helium mass
42 : // @Description: mass of lifting gas
43 : // @Units: kg
44 : AP_GROUPINFO("HMASS", 2, StratoBlimp, helium_mass, 13.54),
45 :
46 : // @Param: ARM_LEN
47 : // @DisplayName: arm length
48 : // @Description: distance from center of mass to one motor
49 : // @Units: m
50 : AP_GROUPINFO("ARM_LEN", 3, StratoBlimp, arm_length, 3.6),
51 :
52 : // @Param: MOT_THST
53 : // @DisplayName: motor thrust
54 : // @Description: thrust at max throttle for one motor
55 : // @Units: N
56 : AP_GROUPINFO("MOT_THST", 4, StratoBlimp, motor_thrust, 145),
57 :
58 : // @Param: DRAG_FWD
59 : // @DisplayName: drag in forward direction
60 : // @Description: drag on X axis
61 : AP_GROUPINFO("DRAG_FWD", 5, StratoBlimp, drag_fwd, 0.27),
62 :
63 : // @Param: DRAG_SIDE
64 : // @DisplayName: drag in sidewards direction
65 : // @Description: drag on Y axis
66 : AP_GROUPINFO("DRAG_SIDE", 16, StratoBlimp, drag_side, 0.5),
67 :
68 : // @Param: DRAG_UP
69 : // @DisplayName: drag in upward direction
70 : // @Description: drag on Z axis
71 : AP_GROUPINFO("DRAG_UP", 6, StratoBlimp, drag_up, 0.4),
72 :
73 : // @Param: MOI_YAW
74 : // @DisplayName: moment of inertia in yaw
75 : // @Description: moment of inertia in yaw
76 : AP_GROUPINFO("MOI_YAW", 7, StratoBlimp, moi_yaw, 2800),
77 :
78 : // @Param: MOI_ROLL
79 : // @DisplayName: moment of inertia in roll
80 : // @Description: moment of inertia in roll
81 : AP_GROUPINFO("MOI_ROLL", 8, StratoBlimp, moi_roll, 1400),
82 :
83 : // @Param: MOI_PITCH
84 : // @DisplayName: moment of inertia in pitch
85 : // @Description: moment of inertia in pitch
86 : AP_GROUPINFO("MOI_PITCH", 9, StratoBlimp, moi_pitch, 3050),
87 :
88 : // @Param: ALT_TARG
89 : // @DisplayName: altitude target
90 : // @Description: altitude target
91 : // @Units: m
92 : AP_GROUPINFO("ALT_TARG", 10, StratoBlimp, altitude_target, 20000),
93 :
94 : // @Param: CLMB_RT
95 : // @DisplayName: target climb rate
96 : // @Description: target climb rate
97 : // @Units: m/s
98 : AP_GROUPINFO("CLMB_RT", 11, StratoBlimp, target_climb_rate, 5),
99 :
100 : // @Param: YAW_RT
101 : // @DisplayName: yaw rate
102 : // @Description: maximum yaw rate with full left throttle at target altitude
103 : // @Units: deg/s
104 : AP_GROUPINFO("YAW_RT", 12, StratoBlimp, yaw_rate_max, 60),
105 :
106 : // @Param: MOT_ANG
107 : // @DisplayName: motor angle
108 : // @Description: maximum motor tilt angle
109 : // @Units: deg
110 : AP_GROUPINFO("MOT_ANG", 13, StratoBlimp, motor_angle, 20),
111 :
112 : // @Param: COL
113 : // @DisplayName: center of lift
114 : // @Description: center of lift position above CoG
115 : // @Units: m
116 : AP_GROUPINFO("COL", 14, StratoBlimp, center_of_lift, 2.54),
117 :
118 : // @Param: WVANE
119 : // @DisplayName: weathervaning offset
120 : // @Description: center of drag for weathervaning
121 : // @Units: m
122 : AP_GROUPINFO("WVANE", 15, StratoBlimp, center_of_drag, 0.3),
123 :
124 : // @Param: FLR
125 : // @DisplayName: free lift rate
126 : // @Description: amount of additional lift generated by the helper balloon (for the purpose of ascent), as a proportion of the 'neutral buoyancy' lift
127 : AP_GROUPINFO("FLR", 17, StratoBlimp, free_lift_rate, 0.12),
128 :
129 : AP_GROUPEND
130 : };
131 :
132 0 : StratoBlimp::StratoBlimp(const char *frame_str) :
133 0 : Aircraft(frame_str)
134 : {
135 0 : AP::sitl()->models.stratoblimp_ptr = this;
136 0 : AP_Param::setup_object_defaults(this, var_info);
137 0 : }
138 :
139 : /*
140 : calculate coefficients to match parameters
141 : */
142 0 : void StratoBlimp::calculate_coefficients(void)
143 : {
144 : // calculate yaw drag based on turn rate at the given altitude
145 0 : drag_yaw = 1.0;
146 :
147 : // get full throttle rotational accel for one motor
148 0 : Vector3f body_acc, rot_accel;
149 0 : handle_motor(1, 0, body_acc, rot_accel, -arm_length);
150 :
151 : // get rotational drag at target alt
152 0 : Vector3f vel_bf, g, drag_linear, drag_rotaccel;
153 0 : g.z = radians(yaw_rate_max);
154 :
155 0 : get_drag(vel_bf, g,
156 : altitude_target,
157 : drag_linear, drag_rotaccel);
158 :
159 0 : drag_yaw = rot_accel.z / -drag_rotaccel.z;
160 0 : }
161 :
162 0 : void StratoBlimp::handle_motor(float throttle, float tilt, Vector3f &body_acc, Vector3f &rot_accel, float lateral_position)
163 : {
164 0 : const float angle_rad = radians(motor_angle) * tilt;
165 0 : const float thrust_x = motor_thrust * throttle;
166 0 : const float total_mass = mass + helium_mass;
167 :
168 0 : const Vector3f thrust{cosf(angle_rad)*thrust_x, 0, -sinf(angle_rad)*thrust_x}; // assume constant with pressure alt and linear
169 0 : Vector3f accel = thrust / total_mass;
170 0 : Vector3f pos{0, lateral_position, 0};
171 :
172 0 : Vector3f torque = (pos % thrust);
173 :
174 0 : rot_accel.z += torque.z / moi_yaw;
175 0 : body_acc += accel;
176 0 : }
177 :
178 :
179 : /*
180 : get body frame linear and rotational drag for a given velocity and altitude
181 : */
182 0 : void StratoBlimp::get_drag(const Vector3f &velocity_linear,
183 : const Vector3f &velocity_rot,
184 : float altitude,
185 : Vector3f &drag_linear, Vector3f &drag_rotaccel)
186 : {
187 0 : Vector3f vel_air_bf = velocity_linear;
188 0 : const float drag_x_sign = vel_air_bf.x>0? -1 : 1;
189 0 : const float drag_y_sign = vel_air_bf.y>0? -1 : 1;
190 0 : const float drag_z_sign = vel_air_bf.z>0? -1 : 1;
191 0 : drag_linear.x = 0.5 * drag_x_sign * air_density * sq(vel_air_bf.x) * drag_fwd;
192 0 : drag_linear.y = 0.5 * drag_y_sign * air_density * sq(vel_air_bf.y) * drag_fwd;
193 0 : drag_linear.z = 0.5 * drag_z_sign * air_density * sq(vel_air_bf.z) * drag_up;
194 :
195 0 : drag_rotaccel = -velocity_rot * drag_yaw;
196 :
197 : /*
198 : apply torque from drag
199 : */
200 0 : Vector3f drag_force = drag_linear * mass;
201 0 : Vector3f drag_pos{-center_of_drag, 0, -center_of_lift};
202 0 : Vector3f drag_torque = (drag_pos % drag_force);
203 0 : drag_rotaccel += drag_torque / moi_pitch;
204 0 : }
205 :
206 : /*
207 : get vertical thrust from lift in Newtons
208 : */
209 0 : float StratoBlimp::get_lift(float altitude)
210 : {
211 : // start with neutral buoyancy
212 0 : float lift_accel = GRAVITY_MSS;
213 :
214 : // add lift from helper balloon if still attached
215 0 : if (helper_balloon_attached) {
216 : // helper balloon additional lift amount based on Free Lift Ratio
217 0 : lift_accel += GRAVITY_MSS*free_lift_rate;
218 : // detach helper balloon if the target altitude has been reached
219 0 : if (altitude >= altitude_target) {
220 0 : helper_balloon_attached = false;
221 : }
222 : }
223 0 : return mass * lift_accel;
224 : }
225 :
226 : // calculate rotational and linear accelerations in body frame
227 0 : void StratoBlimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc, Vector3f &rot_accel)
228 : {
229 : //float delta_time = frame_time_us * 1.0e-6f;
230 :
231 0 : if (!hal.scheduler->is_system_initialized()) {
232 0 : return;
233 : }
234 :
235 0 : const float left_tilt = filtered_servo_angle(input, 0);
236 0 : const float right_tilt = filtered_servo_angle(input, 1);
237 :
238 0 : const float left_throttle = filtered_servo_range(input, 2);
239 0 : const float right_throttle = filtered_servo_range(input, 3);
240 0 : const float ground_release = filtered_servo_range(input, 4);
241 :
242 0 : body_acc.zero();
243 0 : rot_accel.zero();
244 :
245 0 : handle_motor(left_throttle, left_tilt, body_acc, rot_accel, -arm_length);
246 0 : handle_motor(right_throttle, right_tilt, body_acc, rot_accel, arm_length);
247 :
248 0 : Vector3f drag_linear, drag_rotaccel;
249 0 : get_drag(velocity_air_bf, gyro,
250 0 : location.alt*0.01,
251 : drag_linear, drag_rotaccel);
252 :
253 0 : body_acc += drag_linear;
254 0 : rot_accel += drag_rotaccel;
255 :
256 0 : if (ground_release > 0.9) {
257 0 : released = true;
258 : }
259 0 : if (released) {
260 0 : Vector3f lift_thrust_ef{0, 0, -get_lift(location.alt*0.01)};
261 0 : Vector3f lift_thrust_bf = dcm.transposed() * lift_thrust_ef;
262 :
263 0 : body_acc += lift_thrust_bf / mass;
264 :
265 : /*
266 : apply righting moment
267 : */
268 0 : Vector3f lift_pos{0, 0, -center_of_lift};
269 0 : Vector3f lift_torque = (lift_pos % lift_thrust_bf);
270 0 : rot_accel += lift_torque / moi_roll;
271 : }
272 : }
273 :
274 : /*
275 : update the airship simulation by one time step
276 : */
277 0 : void StratoBlimp::update(const struct sitl_input &input)
278 : {
279 0 : calculate_coefficients();
280 :
281 0 : float delta_time = frame_time_us * 1.0e-6f;
282 :
283 0 : Vector3f rot_accel = Vector3f(0,0,0);
284 0 : calculate_forces(input, accel_body, rot_accel);
285 :
286 : // update rotational rates in body frame
287 0 : gyro += rot_accel * delta_time;
288 :
289 0 : gyro.x = constrain_float(gyro.x, -radians(2000.0f), radians(2000.0f));
290 0 : gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
291 0 : gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
292 :
293 0 : dcm.rotate(gyro * delta_time);
294 0 : dcm.normalize();
295 :
296 0 : update_dynamics(rot_accel);
297 0 : update_external_payload(input);
298 :
299 : // update lat/lon/altitude
300 0 : update_position();
301 0 : update_wind(input);
302 0 : time_advance();
303 :
304 : // update magnetic field
305 0 : update_mag_field_bf();
306 :
307 0 : update_battery();
308 0 : }
309 :
310 : #endif // AP_SIM_STRATOBLIMP_ENABLED
|
