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 : simulate a slung payload
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_SLUNGPAYLOAD_ENABLED
22 :
23 : #include "SIM_SlungPayload.h"
24 : #include "SITL.h"
25 : #include <stdio.h>
26 : #include "SIM_Aircraft.h"
27 : #include <AP_HAL_SITL/SITL_State.h>
28 : #include <GCS_MAVLink/GCS.h>
29 : #include <AP_Logger/AP_Logger.h>
30 :
31 : using namespace SITL;
32 :
33 : // SlungPayloadSim parameters
34 : const AP_Param::GroupInfo SlungPayloadSim::var_info[] = {
35 : // @Param: ENABLE
36 : // @DisplayName: Slung Payload Sim enable/disable
37 : // @Description: Slung Payload Sim enable/disable
38 : // @Values: 0:Disabled,1:Enabled
39 : // @User: Advanced
40 : AP_GROUPINFO_FLAGS("ENABLE", 1, SlungPayloadSim, enable, 0, AP_PARAM_FLAG_ENABLE),
41 :
42 : // @Param: WEIGHT
43 : // @DisplayName: Slung Payload weight
44 : // @Description: Slung Payload weight in kg
45 : // @Units: kg
46 : // @Range: 0 15
47 : // @User: Advanced
48 : AP_GROUPINFO("WEIGHT", 2, SlungPayloadSim, weight_kg, 1.0),
49 :
50 : // @Param: LINELEN
51 : // @DisplayName: Slung Payload line length
52 : // @Description: Slung Payload line length in meters
53 : // @Units: m
54 : // @Range: 0 100
55 : // @User: Advanced
56 : AP_GROUPINFO("LINELEN", 3, SlungPayloadSim, line_length, 30.0),
57 :
58 : // @Param: DRAG
59 : // @DisplayName: Slung Payload drag coefficient
60 : // @Description: Slung Payload drag coefficient. Higher values increase drag and slow the payload more quickly
61 : // @Units: m
62 : // @Range: 0 10
63 : // @User: Advanced
64 : AP_GROUPINFO("DRAG", 4, SlungPayloadSim, drag_coef, 1),
65 :
66 : // @Param: SYSID
67 : // @DisplayName: Slung Payload MAVLink system ID
68 : // @Description: Slung Payload MAVLink system id to distinguish it from others on the same network
69 : // @Range: 0 255
70 : // @User: Advanced
71 : AP_GROUPINFO("SYSID", 5, SlungPayloadSim, sys_id, 2),
72 :
73 : AP_GROUPEND
74 : };
75 :
76 : // SlungPayloadSim handles interaction with main vehicle
77 2096 : SlungPayloadSim::SlungPayloadSim()
78 : {
79 2096 : AP_Param::setup_object_defaults(this, var_info);
80 2096 : }
81 :
82 : // update the SlungPayloadSim's state using the vehicle's earth-frame position, velocity, acceleration and wind
83 169080479 : void SlungPayloadSim::update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, const Vector3f& wind_ef)
84 : {
85 169080479 : if (!enable) {
86 169080479 : return;
87 : }
88 :
89 : // initialise slung payload location
90 0 : const uint32_t now_us = AP_HAL::micros();
91 0 : if (!initialised) {
92 : // capture EKF origin
93 0 : auto *sitl = AP::sitl();
94 0 : const Location ekf_origin = sitl->state.home;
95 0 : if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
96 0 : return;
97 : }
98 :
99 : // more initialisation
100 0 : last_update_us = now_us;
101 0 : initialised = true;
102 : }
103 :
104 : // calculate dt and update slung payload
105 0 : const float dt = (now_us - last_update_us)*1.0e-6;
106 0 : last_update_us = now_us;
107 0 : update_payload(veh_pos, veh_vel_ef, veh_accel_ef, wind_ef, dt);
108 :
109 : // send payload location to GCS at 5hz
110 0 : const uint32_t now_ms = AP_HAL::millis();
111 0 : if (now_ms - last_report_ms >= reporting_period_ms) {
112 0 : last_report_ms = now_ms;
113 0 : send_report();
114 0 : write_log();
115 : }
116 : }
117 :
118 : // get earth-frame forces on the vehicle from slung payload
119 : // returns true on success and fills in forces_ef argument, false on failure
120 81459271 : bool SlungPayloadSim::get_forces_on_vehicle(Vector3f& forces_ef) const
121 : {
122 81459271 : if (!enable) {
123 81459271 : return false;
124 : }
125 :
126 0 : forces_ef = veh_forces_ef;
127 0 : return true;
128 : }
129 :
130 : // send a report to the vehicle control code over MAVLink
131 0 : void SlungPayloadSim::send_report(void)
132 : {
133 0 : if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
134 0 : ::printf("SlungPayloadSim connected to %s:%u\n", target_address, (unsigned)target_port);
135 0 : mavlink_connected = true;
136 : }
137 0 : if (!mavlink_connected) {
138 0 : return;
139 : }
140 :
141 : // get current time
142 0 : uint32_t now_ms = AP_HAL::millis();
143 :
144 : // send heartbeat at 1hz
145 0 : const uint8_t component_id = MAV_COMP_ID_USER11;
146 0 : if (now_ms - last_heartbeat_ms >= 1000) {
147 0 : last_heartbeat_ms = now_ms;
148 :
149 0 : const mavlink_heartbeat_t heartbeat{
150 : custom_mode: 0,
151 : type : MAV_TYPE_AIRSHIP,
152 : autopilot : MAV_AUTOPILOT_INVALID,
153 : base_mode: 0,
154 : system_status: 0,
155 : mavlink_version: 0,
156 : };
157 :
158 : mavlink_message_t msg;
159 0 : mavlink_msg_heartbeat_encode_status(
160 0 : sys_id.get(),
161 : component_id,
162 : &mav_status,
163 : &msg,
164 : &heartbeat);
165 : uint8_t buf[300];
166 0 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
167 0 : mav_socket.send(buf, len);
168 : }
169 :
170 : // send a GLOBAL_POSITION_INT messages
171 : {
172 0 : Location payload_loc;
173 : int32_t alt_amsl_cm, alt_rel_cm;
174 0 : if (!get_payload_location(payload_loc) ||
175 0 : !payload_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) ||
176 0 : !payload_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) {
177 0 : return;
178 : }
179 0 : const mavlink_global_position_int_t global_position_int{
180 : time_boot_ms: now_ms,
181 0 : lat: payload_loc.lat,
182 0 : lon: payload_loc.lng,
183 0 : alt: alt_amsl_cm * 10, // amsl alt in mm
184 0 : relative_alt: alt_rel_cm * 10, // relative alt in mm
185 0 : vx: int16_t(velocity_NED.x * 100), // velocity in cm/s
186 0 : vy: int16_t(velocity_NED.y * 100), // velocity in cm/s
187 0 : vz: int16_t(velocity_NED.z * 100), // velocity in cm/s
188 : hdg: 0 // heading in centi-degrees
189 0 : };
190 : mavlink_message_t msg;
191 0 : mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int);
192 : uint8_t buf[300];
193 0 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
194 0 : if (len > 0) {
195 0 : mav_socket.send(buf, len);
196 : }
197 : }
198 :
199 : // send ATTITUDE so MissionPlanner can display orientation
200 : {
201 0 : const mavlink_attitude_t attitude{
202 : time_boot_ms: now_ms,
203 : roll: 0,
204 : pitch: 0,
205 : yaw: 0, // heading in radians
206 : rollspeed: 0,
207 : pitchspeed: 0,
208 : yawspeed: 0
209 0 : };
210 : mavlink_message_t msg;
211 0 : mavlink_msg_attitude_encode_status(
212 : sys_id,
213 : component_id,
214 : &mav_status,
215 : &msg,
216 : &attitude);
217 : uint8_t buf[300];
218 0 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
219 0 : if (len > 0) {
220 0 : mav_socket.send(buf, len);
221 : }
222 : }
223 : }
224 :
225 : // write onboard log
226 0 : void SlungPayloadSim::write_log()
227 : {
228 : #if HAL_LOGGING_ENABLED
229 : // write log of slung payload state
230 : // @LoggerMessage: SLUP
231 : // @Description: Slung payload
232 : // @Field: TimeUS: Time since system startup
233 : // @Field: Land: 1 if payload is landed, 0 otherwise
234 : // @Field: Tens: Tension ratio, 1 if line is taut, 0 if slack
235 : // @Field: Len: Line length
236 : // @Field: PN: Payload position as offset from vehicle in North direction
237 : // @Field: PE: Payload position as offset from vehicle in East direction
238 : // @Field: PD: Payload position as offset from vehicle in Down direction
239 : // @Field: VN: Payload velocity in North direction
240 : // @Field: VE: Payload velocity in East direction
241 : // @Field: VD: Payload velocity in Down direction
242 : // @Field: AN: Payload acceleration in North direction
243 : // @Field: AE: Payload acceleration in East direction
244 : // @Field: AD: Payload acceleration in Down direction
245 : // @Field: VFN: Force on vehicle in North direction
246 : // @Field: VFE: Force on vehicle in East direction
247 : // @Field: VFD: Force on vehicle in Down direction
248 0 : AP::logger().WriteStreaming("SLUP",
249 : "TimeUS,Land,Tens,Len,PN,PE,PD,VN,VE,VD,AN,AE,AD,VFN,VFE,VFD", // labels
250 : "s-%mmmmnnnooo---", // units
251 : "F-20000000000000", // multipliers
252 : "Qbffffffffffffff", // format
253 : AP_HAL::micros64(),
254 0 : (uint8_t)landed,
255 0 : (float)tension_ratio,
256 0 : (float)payload_to_veh.length(),
257 0 : (double)-payload_to_veh.x,
258 0 : (double)-payload_to_veh.y,
259 0 : (double)-payload_to_veh.z,
260 0 : (double)velocity_NED.x,
261 0 : (double)velocity_NED.y,
262 0 : (double)velocity_NED.z,
263 0 : (double)accel_NED.x,
264 0 : (double)accel_NED.y,
265 0 : (double)accel_NED.z,
266 0 : (double)veh_forces_ef.x,
267 0 : (double)veh_forces_ef.y,
268 0 : (double)veh_forces_ef.z);
269 : #endif
270 0 : }
271 :
272 : // returns true on success and fills in payload_loc argument, false on failure
273 0 : bool SlungPayloadSim::get_payload_location(Location& payload_loc) const
274 : {
275 : // get EKF origin
276 0 : auto *sitl = AP::sitl();
277 0 : if (sitl == nullptr) {
278 0 : return false;
279 : }
280 0 : const Location ekf_origin = sitl->state.home;
281 0 : if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
282 0 : return false;
283 : }
284 :
285 : // calculate location
286 0 : payload_loc = ekf_origin;
287 0 : payload_loc.offset(position_NED);
288 0 : return true;
289 : }
290 :
291 : // update the slung payloads position, velocity, acceleration
292 : // vehicle position, velocity, acceleration and wind should be in earth-frame NED frame
293 0 : void SlungPayloadSim::update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef,
294 : const Vector3f& wind_ef, float dt)
295 : {
296 : // how we calculate the payload's position, velocity and acceleration
297 : // 1. update the payload's position, velocity using the previous iterations acceleration
298 : // 2. check that the payload does not fall below the terrain
299 : // 3. check if the line is taught and that the payload does not move more than the line length from the vehicle
300 : // 4. calculate gravity and drag forces on the payload
301 : // 5. calculate the tension force between the payload and vehicle including force countering gravity, drag and centripetal force
302 : // 6. update the payload's acceleration using the sum of the above forces
303 :
304 : // initialise position_NED from vehicle position
305 0 : if (position_NED.is_zero()) {
306 0 : if (!veh_pos.is_zero()) {
307 0 : position_NED = veh_pos;
308 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: initialised at %f %f %f", position_NED.x, position_NED.y, position_NED.z);
309 : }
310 0 : return;
311 : }
312 :
313 : // integrate previous iterations acceleration into velocity and position
314 0 : velocity_NED += accel_NED * dt;
315 0 : position_NED += (velocity_NED * dt).todouble();
316 :
317 : // calculate distance from payload to vehicle
318 0 : payload_to_veh = veh_pos - position_NED;
319 0 : float payload_to_veh_length = payload_to_veh.length();
320 :
321 : // update landed state by checking if payload has dropped below terrain
322 0 : Location payload_loc;
323 0 : if (get_payload_location(payload_loc)) {
324 : int32_t alt_terrain_cm;
325 0 : bool landed_orig = landed;
326 0 : if (payload_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_terrain_cm)) {
327 :
328 : // landed if below terrain
329 0 : if (alt_terrain_cm <= 0) {
330 0 : landed = true;
331 :
332 : // raise payload to match terrain
333 0 : position_NED.z += (alt_terrain_cm * 0.01);
334 :
335 : // zero out velocity and acceleration in horizontal and downward direction
336 0 : velocity_NED.xy().zero();
337 0 : velocity_NED.z = MIN(velocity_NED.z, 0);
338 0 : accel_NED.xy().zero();
339 0 : accel_NED.z = MIN(accel_NED.z, 0);
340 :
341 : // zero out forces on vehicle
342 0 : veh_forces_ef.zero();
343 : }
344 :
345 : // not landed if above terrain
346 0 : if (landed && (alt_terrain_cm > 1)) {
347 0 : landed = false;
348 : }
349 : }
350 :
351 : // inform user if landed state has changed
352 0 : if (landed != landed_orig) {
353 0 : if (landed) {
354 : // get payload location again in case it has moved
355 0 : get_payload_location(payload_loc);
356 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: landed lat:%f lon:%f alt:%4.1f",
357 : (double)payload_loc.lat * 1e-7,
358 : (double)payload_loc.lng * 1e-7,
359 : (double)payload_loc.alt * 1e-2);
360 : } else {
361 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: liftoff");
362 : }
363 : }
364 : }
365 :
366 : // calculate forces of gravity
367 0 : Vector3f force_gravity_NED = Vector3f(0.0f, 0.0f, GRAVITY_MSS * weight_kg);
368 :
369 : // tension force on payload (resists gravity, drag, centripetal force)
370 0 : Vector3f tension_force_NED;
371 :
372 : // tension ratio to smooth transition from line being taut to slack
373 0 : tension_ratio = 0;
374 :
375 : // calculate drag force (0.5 * drag_coef * air_density * velocity^2 * surface area)
376 0 : Vector3f force_drag_NED;
377 0 : Vector3f velocity_air_NED = velocity_NED;
378 0 : if (!landed) {
379 0 : velocity_air_NED -= wind_ef;
380 : }
381 0 : if (drag_coef > 0 && !velocity_air_NED.is_zero()) {
382 0 : const float air_density = 1.225; // 1.225 kg/m^3 (standard sea-level density)
383 0 : const float surface_area_m2 = 0.07; // 30cm diameter sphere
384 0 : const float drag_force = 0.5 * drag_coef * air_density * velocity_air_NED.length_squared() * surface_area_m2;
385 0 : force_drag_NED = -velocity_air_NED.normalized() * drag_force;
386 : }
387 :
388 : // sanity check payload distance from vehicle and calculate tension force
389 0 : if (is_positive(payload_to_veh_length)) {
390 :
391 : // calculate unit vector from payload to vehicle
392 0 : const Vector3f payload_to_veh_norm = payload_to_veh.normalized().tofloat();
393 :
394 : // ensure payload is no more than line_length from vehicle
395 0 : if (payload_to_veh_length > line_length) {
396 0 : payload_to_veh *= (line_length / payload_to_veh_length);
397 0 : position_NED = veh_pos - payload_to_veh;
398 : }
399 :
400 : // calculate tension ratio as value between 0 and 1
401 : // tension ratio is 0 when payload-to-vehicle distance is 10cm less than line length
402 : // tension ratio is 1 when payload-to-vehicle distance is equal to line length
403 0 : tension_ratio = constrain_float(1.0 - (line_length - payload_to_veh_length) * 10, 0, 1);
404 :
405 : // calculate tension forces when line is taut
406 0 : if (is_positive(tension_ratio)) {
407 :
408 : // tension resists gravity if vehicle is above payload
409 0 : if (is_negative(payload_to_veh_norm.z)) {
410 0 : tension_force_NED += -force_gravity_NED.projected(payload_to_veh_norm);
411 : }
412 :
413 : // calculate tension force resulting from velocity difference between vehicle and payload
414 : // use time constant to convert velocity to acceleration
415 0 : const float velocity_to_accel_TC = 2.0;
416 0 : Vector3f velocity_diff_NED = (veh_vel_ef - velocity_NED).projected(payload_to_veh_norm);
417 :
418 : // add to tension force if the vehicle is moving faster than the payload
419 0 : if (vectors_same_direction(velocity_diff_NED, payload_to_veh_norm)) {
420 0 : tension_force_NED += velocity_diff_NED / velocity_to_accel_TC * weight_kg;
421 : }
422 :
423 : // tension force resisting payload drag
424 0 : tension_force_NED += -force_drag_NED.projected(payload_to_veh_norm);
425 :
426 : // calculate centripetal force
427 0 : const Vector3f velocity_parallel = velocity_NED.projected(payload_to_veh_norm);
428 0 : const Vector3f velocity_perpendicular = velocity_NED - velocity_parallel;
429 0 : const float tension_force_centripetal = velocity_perpendicular.length_squared() * weight_kg / line_length;
430 0 : const Vector3f tension_force_centripetal_NED = payload_to_veh_norm * tension_force_centripetal;
431 :
432 : // add centripetal force to tension force
433 0 : tension_force_NED += tension_force_centripetal_NED;
434 :
435 : // scale tension force by tension ratio
436 0 : tension_force_NED *= tension_ratio;
437 : }
438 : }
439 :
440 : // force on vehicle is opposite to tension force on payload
441 0 : veh_forces_ef = -tension_force_NED;
442 :
443 : // convert force to acceleration (f=m*a => a=f/m)
444 0 : accel_NED = (force_gravity_NED + force_drag_NED + tension_force_NED) / weight_kg;
445 :
446 : // if slung payload is landed we zero out downward (e.g positive) acceleration
447 0 : if (landed) {
448 0 : accel_NED.z = MIN(accel_NED.z, 0);
449 : // should probably zero out forces_ef vertical component as well?
450 : }
451 : }
452 :
453 : // returns true if the two vectors point in the same direction, false if perpendicular or opposite
454 0 : bool SlungPayloadSim::vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const
455 : {
456 : // check both vectors are non-zero
457 0 : if (v1.is_zero() || v2.is_zero()) {
458 0 : return false;
459 : }
460 0 : return v1.dot(v2) > 0;
461 : }
462 :
463 : #endif
|
