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 static tether attached to the vehicle and ground
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_TETHER_ENABLED
22 :
23 : #include "SIM_Tether.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 : // TetherSim parameters
34 : const AP_Param::GroupInfo TetherSim::var_info[] = {
35 : // @Param: ENABLE
36 : // @DisplayName: Tether Simulation Enable/Disable
37 : // @Description: Enable or disable the tether simulation
38 : // @Values: 0:Disabled,1:Enabled
39 : // @User: Advanced
40 : AP_GROUPINFO_FLAGS("ENABLE", 1, TetherSim, enable, 0, AP_PARAM_FLAG_ENABLE),
41 :
42 : // @Param: DENSITY
43 : // @DisplayName: Tether Wire Density
44 : // @Description: Linear mass density of the tether wire
45 : // @Range: 0 1
46 : // @User: Advanced
47 : AP_GROUPINFO("DENSITY", 2, TetherSim, tether_linear_density, 0.0167),
48 :
49 : // @Param: LINELEN
50 : // @DisplayName: Tether Maximum Line Length
51 : // @Description: Maximum length of the tether line in meters
52 : // @Units: m
53 : // @Range: 0 100
54 : // @User: Advanced
55 : AP_GROUPINFO("LINELEN", 3, TetherSim, max_line_length, 100.0),
56 :
57 : // @Param: SYSID
58 : // @DisplayName: Tether Simulation MAVLink System ID
59 : // @Description: MAVLink system ID for the tether simulation, used to distinguish it from other systems on the network
60 : // @Range: 0 255
61 : // @User: Advanced
62 : AP_GROUPINFO("SYSID", 4, TetherSim, sys_id, 2),
63 :
64 : // @Param: STUCK
65 : // @DisplayName: Tether Stuck Enable/Disable
66 : // @Description: Enable or disable a stuck tether simulation
67 : // @Values: 0:Disabled,1:Enabled
68 : // @User: Advanced
69 : AP_GROUPINFO("STUCK", 5, TetherSim, tether_stuck, 0),
70 :
71 : // @Param: SPGCNST
72 : // @DisplayName: Tether Spring Constant
73 : // @Description: Spring constant for the tether to simulate elastic forces when stretched beyond its maximum length
74 : // @Range: 0 255
75 : // @User: Advanced
76 : AP_GROUPINFO("SPGCNST", 6, TetherSim, tether_spring_constant, 100),
77 :
78 : // @Param: DMPCNST
79 : // @DisplayName: Tether Damping Constant
80 : // @Description: Damping constant for the tether to simulate resistance based on change in stretch
81 : // @Range: 0 255
82 : // @User: Advanced
83 : AP_GROUPINFO("DMPCNST", 7, TetherSim, tether_damping_constant, 10),
84 :
85 : AP_GROUPEND
86 : };
87 :
88 : // TetherSim handles interaction with main vehicle
89 2096 : TetherSim::TetherSim()
90 : {
91 2096 : AP_Param::setup_object_defaults(this, var_info);
92 2096 : }
93 :
94 :
95 169080479 : void TetherSim::update(const Location& veh_pos)
96 : {
97 169080479 : if (!enable) {
98 168941839 : return;
99 : }
100 :
101 138640 : if (veh_pos.is_zero()) {
102 0 : return;
103 : }
104 :
105 : // initialise fixed tether ground location
106 138640 : const uint32_t now_us = AP_HAL::micros();
107 138640 : if (!initialised) {
108 : // capture EKF origin
109 3 : auto *sitl = AP::sitl();
110 3 : const Location ekf_origin = sitl->state.home;
111 3 : if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
112 1 : return;
113 : }
114 :
115 : // more initialisation
116 2 : last_update_us = now_us;
117 2 : initialised = true;
118 : }
119 :
120 : // calculate dt and update tether forces
121 138639 : const float dt = (now_us - last_update_us)*1.0e-6;
122 138639 : last_update_us = now_us;
123 :
124 138639 : update_tether_force(veh_pos, dt);
125 :
126 : // send tether location to GCS at 5hz
127 : // TO-Do: add provision to make the tether movable
128 138639 : const uint32_t now_ms = AP_HAL::millis();
129 138639 : if (now_ms - last_report_ms >= reporting_period_ms) {
130 1155 : last_report_ms = now_ms;
131 1155 : send_report();
132 1155 : write_log();
133 : }
134 : }
135 :
136 : // get earth-frame forces on the vehicle from the tether
137 : // returns true on success and fills in forces_ef argument, false on failure
138 81459271 : bool TetherSim::get_forces_on_vehicle(Vector3f& forces_ef) const
139 : {
140 81459271 : if (!enable) {
141 81320631 : return false;
142 : }
143 :
144 138640 : forces_ef = veh_forces_ef;
145 138640 : return true;
146 : }
147 :
148 : // send a report to the vehicle control code over MAVLink
149 1155 : void TetherSim::send_report(void)
150 : {
151 1155 : if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
152 2 : ::printf("Tether System connected to %s:%u\n", target_address, (unsigned)target_port);
153 2 : mavlink_connected = true;
154 : }
155 1155 : if (!mavlink_connected) {
156 0 : return;
157 : }
158 :
159 : // get current time
160 1155 : uint32_t now_ms = AP_HAL::millis();
161 :
162 : // send heartbeat at 1hz
163 1155 : const uint8_t component_id = MAV_COMP_ID_USER11;
164 1155 : if (now_ms - last_heartbeat_ms >= 1000) {
165 116 : last_heartbeat_ms = now_ms;
166 :
167 116 : const mavlink_heartbeat_t heartbeat{
168 : custom_mode: 0,
169 : type : MAV_TYPE_GROUND_ROVER,
170 : autopilot : MAV_AUTOPILOT_INVALID,
171 : base_mode: 0,
172 : system_status: 0,
173 : mavlink_version: 0,
174 : };
175 :
176 : mavlink_message_t msg;
177 232 : mavlink_msg_heartbeat_encode_status(
178 116 : sys_id.get(),
179 : component_id,
180 : &mav_status,
181 : &msg,
182 : &heartbeat);
183 : uint8_t buf[300];
184 116 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
185 116 : mav_socket.send(buf, len);
186 : }
187 :
188 : // send a GLOBAL_POSITION_INT messages
189 : {
190 1155 : Location tether_anchor_loc;
191 : int32_t alt_amsl_cm, alt_rel_cm;
192 1155 : if (!get_tether_ground_location(tether_anchor_loc) ||
193 2310 : !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) ||
194 1155 : !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) {
195 233 : return;
196 : }
197 922 : const mavlink_global_position_int_t global_position_int{
198 : time_boot_ms: now_ms,
199 922 : lat: tether_anchor_loc.lat,
200 922 : lon: tether_anchor_loc.lng,
201 922 : alt: alt_amsl_cm * 10, // amsl alt in mm
202 922 : relative_alt: alt_rel_cm * 10, // relative alt in mm
203 : vx: 0, // velocity in cm/s
204 : vy: 0, // velocity in cm/s
205 : vz: 0, // velocity in cm/s
206 : hdg: 0 // heading in centi-degrees
207 922 : };
208 : mavlink_message_t msg;
209 922 : mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int);
210 : uint8_t buf[300];
211 922 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
212 922 : if (len > 0) {
213 922 : mav_socket.send(buf, len);
214 : }
215 : }
216 : }
217 :
218 1155 : void TetherSim::write_log()
219 : {
220 : #if HAL_LOGGING_ENABLED
221 : // write log of tether state
222 : // @LoggerMessage: TETH
223 : // @Description: Tether state
224 : // @Field: TimeUS: Time since system startup
225 : // @Field: Len: Tether length
226 : // @Field: VFN: Force on vehicle in North direction
227 : // @Field: VFE: Force on vehicle in East direction
228 : // @Field: VFD: Force on vehicle in Down direction
229 2310 : AP::logger().WriteStreaming("TETH",
230 : "TimeUS,Len,VFN,VFE,VFD", // labels
231 : "s----", // units
232 : "F----", // multipliers
233 : "Qffff", // format
234 : AP_HAL::micros64(),
235 1155 : (float)tether_length,
236 1155 : (double)veh_forces_ef.x,
237 1155 : (double)veh_forces_ef.y,
238 1155 : (double)veh_forces_ef.z);
239 : #endif
240 1155 : }
241 : // returns true on success and fills in tether_ground_loc argument, false on failure
242 139794 : bool TetherSim::get_tether_ground_location(Location& tether_ground_loc) const
243 : {
244 : // get EKF origin
245 139794 : auto *sitl = AP::sitl();
246 139794 : if (sitl == nullptr) {
247 0 : return false;
248 : }
249 139794 : const Location ekf_origin = sitl->state.home;
250 139794 : if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
251 0 : return false;
252 : }
253 :
254 139794 : tether_ground_loc = ekf_origin;
255 139794 : return true;
256 : }
257 :
258 138639 : void TetherSim::update_tether_force(const Location& veh_pos, float dt)
259 : {
260 :
261 138639 : Location tether_anchor_loc;
262 138639 : if (!get_tether_ground_location(tether_anchor_loc)) {
263 0 : return;
264 : }
265 :
266 : // Step 1: Calculate the vector from the tether anchor to the vehicle
267 138639 : Vector3f tether_vector = veh_pos.get_distance_NED(tether_anchor_loc);
268 138639 : tether_length = tether_vector.length();
269 :
270 : // Step 2: Check if tether is taut (length exceeds maximum allowed length) or stuck
271 138639 : if (tether_length > max_line_length) {
272 :
273 : // Calculate the stretch beyond the maximum length
274 29945 : float stretch = MAX(tether_length - max_line_length, 0.0f);
275 :
276 : // Calculate the stretch rate beyond the maximum length
277 29945 : float stretch_rate = (stretch - prev_stretch)/dt;
278 :
279 : // Apply a spring and damping penalty force proportional to the stretch
280 29945 : float penalty_force_magnitude = tether_spring_constant * stretch + tether_damping_constant * stretch_rate;
281 :
282 : // Direction of force is along the tether, pulling toward the anchor
283 29945 : veh_forces_teth = tether_vector.normalized() * penalty_force_magnitude;
284 :
285 29945 : prev_stretch = stretch;
286 :
287 108694 : } else if (tether_stuck) {
288 :
289 : // Calculate the stretch beyond the maximum length
290 0 : float stretch = MAX(tether_length - tether_not_stuck_length, 0.0f);
291 :
292 : // Calculate the stretch rate beyond the maximum length
293 0 : float stretch_rate = (stretch - prev_stretch)/dt;
294 :
295 : // Apply a spring-like penalty force proportional to the stretch
296 0 : float penalty_force_magnitude = tether_spring_constant * stretch + tether_damping_constant * stretch_rate;
297 :
298 : // Direction of force is directly along the tether, towards the tether anchor point
299 0 : veh_forces_teth = tether_vector.normalized() * penalty_force_magnitude;
300 :
301 0 : prev_stretch = stretch;
302 :
303 : } else {
304 108694 : tether_not_stuck_length = tether_length;
305 108694 : veh_forces_teth.zero();
306 : }
307 :
308 : // Step 3: Calculate the weight of the tether being lifted
309 : // The weight is proportional to the current tether length
310 138639 : const float tether_weight_force = tether_linear_density * tether_length * GRAVITY_MSS;
311 :
312 : // Step 4: Calculate the tension force
313 138639 : Vector3f tension_force_NED = tether_vector.normalized() * tether_weight_force;
314 :
315 : // Step 5: Apply the total force to the vehicle
316 138639 : veh_forces_ef = veh_forces_teth + tension_force_NED;
317 : }
318 :
319 : #endif
|
