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 ship takeoff/landing
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_SHIP_ENABLED
22 :
23 : #include "SIM_Ship.h"
24 :
25 : #include "SITL.h"
26 :
27 : #include <stdio.h>
28 :
29 : #include "SIM_Aircraft.h"
30 : #include <AP_HAL_SITL/SITL_State.h>
31 : #include <AP_HAL_SITL/HAL_SITL_Class.h>
32 :
33 : extern const HAL_SITL& hal_sitl;
34 : #include <AP_Terrain/AP_Terrain.h>
35 :
36 : using namespace SITL;
37 :
38 : // SITL Ship parameters
39 : const AP_Param::GroupInfo ShipSim::var_info[] = {
40 : // @Param: ENABLE
41 : // @DisplayName: Ship landing Enable
42 : // @Description: Enable ship landing simulation
43 : // @Values: 0:Disable,1:Enabled
44 : AP_GROUPINFO("ENABLE", 1, ShipSim, enable, 0),
45 : // @Param: SPEED
46 : // @DisplayName: Ship Speed
47 : // @Description: Speed of the ship
48 : // @Units: m/s
49 : AP_GROUPINFO("SPEED", 2, ShipSim, speed, 3),
50 : // @Param: PSIZE
51 : // @DisplayName: Path Size
52 : // @Description: Diameter of the circle the ship is traveling on
53 : // @Units: m
54 : AP_GROUPINFO("PSIZE", 3, ShipSim, path_size, 1000),
55 : // @Param: SYSID
56 : // @DisplayName: System ID
57 : // @Description: System ID of the ship
58 : // @Range: 1 255
59 : AP_GROUPINFO("SYSID", 4, ShipSim, sys_id, 17),
60 : // @Param: DSIZE
61 : // @DisplayName: Deck Size
62 : // @Description: Size of the ship's deck
63 : // @Units: m
64 : AP_GROUPINFO("DSIZE", 5, ShipSim, deck_size, 10),
65 : // @Param: OFS
66 : // @DisplayName: Ship landing pad offset
67 : // @Description: Defines the offset of the ship's landing pad w.r.t. the ship's origin, i.e. where the beacon is placed on the ship
68 : // @Units: m
69 : // @Vector3Parameter: 1
70 : AP_GROUPINFO("OFS", 7, ShipSim, offset, 0),
71 : AP_GROUPEND
72 : };
73 :
74 : /*
75 : update a simulated vehicle
76 : */
77 426150 : void Ship::update(float delta_t)
78 : {
79 : // acceletate over time to keep EKF happy
80 426150 : const float max_accel = 3.0;
81 426150 : const float dspeed_max = max_accel * delta_t;
82 426150 : speed = constrain_float(sim->speed.get(), speed-dspeed_max, speed+dspeed_max);
83 :
84 : // calculate how far around the circle we go
85 426150 : float circumference = M_PI * sim->path_size.get();
86 426150 : float dist = delta_t * speed;
87 426150 : float dangle = (dist / circumference) * 360.0;
88 :
89 426150 : if (delta_t > 0) {
90 426144 : yaw_rate = radians(dangle) / delta_t;
91 : }
92 426150 : heading_deg += dangle;
93 426150 : heading_deg = wrap_360(heading_deg);
94 :
95 426150 : Vector2f dpos(dist, 0);
96 426150 : dpos.rotate(radians(heading_deg));
97 :
98 426150 : position += dpos;
99 426150 : }
100 :
101 2096 : ShipSim::ShipSim()
102 : {
103 2096 : AP_Param::setup_object_defaults(this, var_info);
104 2096 : }
105 :
106 : /*
107 : get the location of the ship
108 : */
109 61426787 : bool ShipSim::get_location(Location &loc) const
110 : {
111 61426787 : if (!enable) {
112 61218322 : return false;
113 : }
114 208465 : if (!home.initialised()) {
115 7 : return false;
116 : }
117 208458 : loc = home;
118 208458 : loc.offset(ship.position.x, ship.position.y);
119 208458 : return true;
120 : }
121 :
122 : /*
123 : get ground speed adjustment if we are landed on the ship
124 : */
125 61425013 : Vector2f ShipSim::get_ground_speed_adjustment(const Location &loc, float &yaw_rate)
126 : {
127 61425013 : Location shiploc;
128 61425013 : if (!get_location(shiploc)) {
129 61218329 : yaw_rate = 0;
130 61218329 : return Vector2f(0,0);
131 : }
132 206684 : if (loc.get_distance(shiploc) > deck_size) {
133 3302 : yaw_rate = 0;
134 3302 : return Vector2f(0,0);
135 : }
136 :
137 : // find center of the circle that the ship is on
138 203382 : Location center = shiploc;
139 203382 : const float path_radius = path_size.get()*0.5;
140 203382 : center.offset_bearing(ship.heading_deg+(ship.yaw_rate>0?90:-90), path_radius);
141 :
142 : // scale speed for ratio of distances
143 203382 : const float p = center.get_distance(loc) / path_radius;
144 203382 : const float scaled_speed = ship.speed * p;
145 :
146 : // work out how far around the circle ahead or behind we are for
147 : // rotating velocity
148 203382 : const float bearing1 = center.get_bearing(loc);
149 203382 : const float bearing2 = center.get_bearing(shiploc);
150 203382 : const float heading = ship.heading_deg + degrees(bearing1-bearing2);
151 :
152 203382 : Vector2f vel(scaled_speed, 0);
153 203382 : vel.rotate(radians(heading));
154 203382 : yaw_rate = ship.yaw_rate;
155 203382 : return vel;
156 : }
157 :
158 : /*
159 : update the ShipSim peripheral state
160 : */
161 178759942 : void ShipSim::update(void)
162 : {
163 178759942 : if (!enable) {
164 178333791 : return;
165 : }
166 :
167 426151 : auto *sitl = AP::sitl();
168 426151 : uint32_t now_us = AP_HAL::micros();
169 :
170 426151 : if (!initialised) {
171 7 : home = sitl->state.home;
172 7 : if (home.lat == 0 && home.lng == 0) {
173 1 : return;
174 : }
175 6 : const Vector3f &ofs = offset.get();
176 6 : home.offset(ofs.x, ofs.y);
177 6 : home.alt -= ofs.z*100;
178 :
179 6 : target_port += 10 * hal_sitl.get_instance();
180 :
181 6 : initialised = true;
182 6 : ::printf("ShipSim home %f %f reporting to %s:%u\n",
183 6 : home.lat*1.0e-7, home.lng*1.0e-7,
184 6 : target_address, (unsigned)target_port);
185 6 : ship.sim = this;
186 6 : last_update_us = now_us;
187 6 : last_report_ms = AP_HAL::millis();
188 : }
189 :
190 426150 : float dt = (now_us - last_update_us)*1.0e-6;
191 426150 : last_update_us = now_us;
192 :
193 426150 : ship.update(dt);
194 :
195 426150 : uint32_t now_ms = AP_HAL::millis();
196 426150 : if (now_ms - last_report_ms >= reporting_period_ms) {
197 1774 : last_report_ms = now_ms;
198 1774 : send_report();
199 : }
200 : }
201 :
202 : /*
203 : send a report to the vehicle control code over MAVLink
204 : */
205 1774 : void ShipSim::send_report(void)
206 : {
207 1774 : if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
208 6 : ::printf("ShipSim connected to %s:%u\n", target_address, (unsigned)target_port);
209 6 : mavlink_connected = true;
210 : }
211 1774 : if (!mavlink_connected) {
212 0 : return;
213 : }
214 :
215 : // Drain and discard anything the vehicle sends us:
216 : {
217 : uint8_t discard[1024];
218 3728 : while (mav_socket.recv(discard, sizeof(discard), 0) > 0) {
219 : }
220 : }
221 :
222 1774 : uint32_t now = AP_HAL::millis();
223 :
224 1774 : const uint8_t component_id = MAV_COMP_ID_USER10;
225 :
226 1774 : if (now - last_heartbeat_ms >= 1000) {
227 356 : last_heartbeat_ms = now;
228 :
229 356 : const mavlink_heartbeat_t heartbeat{
230 : custom_mode: 0,
231 : type : MAV_TYPE_SURFACE_BOAT,
232 : autopilot : MAV_AUTOPILOT_INVALID,
233 : base_mode: 0,
234 : system_status: 0,
235 : mavlink_version: 0,
236 : };
237 :
238 : mavlink_message_t msg;
239 712 : mavlink_msg_heartbeat_encode_status(
240 356 : sys_id.get(),
241 : component_id,
242 : &mav_status,
243 : &msg,
244 : &heartbeat);
245 : uint8_t buf[300];
246 356 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
247 356 : mav_socket.send(buf, len);
248 : }
249 :
250 :
251 : /*
252 : send a GLOBAL_POSITION_INT messages
253 : */
254 1774 : Location loc;
255 1774 : if (!get_location(loc)) {
256 0 : return;
257 : }
258 :
259 1774 : int32_t alt_mm = home.alt * 10; // assume home altitude
260 :
261 : #if AP_TERRAIN_AVAILABLE
262 1774 : auto terrain = AP::terrain();
263 : float height;
264 1774 : if (terrain != nullptr && terrain->enabled() && terrain->height_amsl(loc, height, false)) {
265 1007 : alt_mm = height * 1000;
266 : }
267 : #endif
268 :
269 : { // send position
270 1774 : Vector2f vel(ship.speed, 0);
271 1774 : vel.rotate(radians(ship.heading_deg));
272 :
273 1774 : const mavlink_global_position_int_t global_position_int{
274 : time_boot_ms: now,
275 1774 : lat: loc.lat,
276 1774 : lon: loc.lng,
277 : alt: alt_mm,
278 : relative_alt: 0,
279 1774 : vx: int16_t(vel.x*100),
280 1774 : vy: int16_t(vel.y*100),
281 : vz: 0,
282 1774 : hdg: uint16_t(ship.heading_deg*100)
283 1774 : };
284 : mavlink_message_t msg;
285 1774 : mavlink_msg_global_position_int_encode_status(
286 : sys_id,
287 : component_id,
288 : &mav_status,
289 : &msg,
290 : &global_position_int);
291 : uint8_t buf[300];
292 1774 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
293 1774 : if (len > 0) {
294 1774 : mav_socket.send(buf, len);
295 : }
296 : }
297 :
298 : { // also set ATTITUDE so MissionPlanner can display ship orientation
299 1774 : const mavlink_attitude_t attitude{
300 : time_boot_ms: now,
301 : roll: 0,
302 : pitch: 0,
303 1774 : yaw: float(radians(ship.heading_deg)),
304 : rollspeed: 0,
305 : pitchspeed: 0,
306 1774 : yawspeed: ship.yaw_rate
307 1774 : };
308 : mavlink_message_t msg;
309 1774 : mavlink_msg_attitude_encode_status(
310 : sys_id,
311 : component_id,
312 : &mav_status,
313 : &msg,
314 : &attitude);
315 : uint8_t buf[300];
316 1774 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
317 1774 : if (len > 0) {
318 1774 : mav_socket.send(buf, len);
319 : }
320 : }
321 : }
322 :
323 : #endif // AP_SIM_SHIP_ENABLED
|
