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 : Simulator Connector for AirSim
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_AIRSIM_ENABLED
22 :
23 : #include "SIM_AirSim.h"
24 :
25 : #include <stdio.h>
26 : #include <arpa/inet.h>
27 : #include <errno.h>
28 :
29 : #include <AP_HAL/AP_HAL.h>
30 : #include <AP_Logger/AP_Logger.h>
31 : #include <AP_HAL/utility/replace.h>
32 :
33 : #define UDP_TIMEOUT_MS 100
34 :
35 : extern const AP_HAL::HAL& hal;
36 :
37 : using namespace SITL;
38 :
39 0 : AirSim::AirSim(const char *frame_str) :
40 : Aircraft(frame_str),
41 0 : sock(true)
42 : {
43 0 : if (strstr(frame_str, "-copter")) {
44 0 : output_type = OutputType::Copter;
45 0 : } else if (strstr(frame_str, "-rover")) {
46 0 : output_type = OutputType::Rover;
47 : } else {
48 : // default to copter
49 0 : output_type = OutputType::Copter;
50 : }
51 :
52 0 : printf("Starting SITL Airsim type %u\n", (unsigned)output_type);
53 0 : }
54 :
55 : /*
56 : Create & set in/out socket
57 : */
58 0 : void AirSim::set_interface_ports(const char* address, const int port_in, const int port_out)
59 : {
60 : static const char *port_in_addr = "0.0.0.0";
61 :
62 0 : if (!sock.bind(port_in_addr, port_in)) {
63 0 : printf("Unable to bind Airsim sensor_in socket at port %u - Error: %s\n",
64 0 : port_in, strerror(errno));
65 0 : return;
66 : }
67 0 : printf("Bind SITL sensor input at %s:%u\n", port_in_addr, port_in);
68 0 : sock.set_blocking(false);
69 0 : sock.reuseaddress();
70 :
71 0 : airsim_ip = address;
72 0 : airsim_control_port = port_out;
73 0 : airsim_sensor_port = port_in;
74 :
75 0 : printf("AirSim control interface set to %s:%u\n", airsim_ip, airsim_control_port);
76 : }
77 :
78 : /*
79 : Decode and send servos
80 : */
81 0 : void AirSim::output_copter(const sitl_input& input)
82 : {
83 : servo_packet pkt;
84 :
85 0 : for (uint8_t i=0; i<kArduCopterRotorControlCount; i++) {
86 0 : pkt.pwm[i] = input.servos[i];
87 : }
88 :
89 0 : ssize_t send_ret = sock.sendto(&pkt, sizeof(pkt), airsim_ip, airsim_control_port);
90 0 : if (send_ret != sizeof(pkt)) {
91 0 : if (send_ret <= 0) {
92 0 : printf("Unable to send servo output to %s:%u - Error: %s, Return value: %ld\n",
93 0 : airsim_ip, airsim_control_port, strerror(errno), (long)send_ret);
94 : } else {
95 0 : printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)sizeof(pkt));
96 : }
97 : }
98 0 : }
99 :
100 0 : void AirSim::output_rover(const sitl_input& input)
101 : {
102 : rover_packet pkt;
103 :
104 0 : pkt.steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
105 0 : pkt.throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
106 :
107 0 : ssize_t send_ret = sock.sendto(&pkt, sizeof(pkt), airsim_ip, airsim_control_port);
108 0 : if (send_ret != sizeof(pkt)) {
109 0 : if (send_ret <= 0) {
110 0 : printf("Unable to send control output to %s:%u - Error: %s, Return value: %ld\n",
111 0 : airsim_ip, airsim_control_port, strerror(errno), (long)send_ret);
112 : } else {
113 0 : printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)sizeof(pkt));
114 : }
115 : }
116 0 : }
117 :
118 : /*
119 : Wrapper function to send servo output
120 : */
121 0 : void AirSim::output_servos(const sitl_input& input)
122 : {
123 0 : switch (output_type) {
124 0 : case OutputType::Copter:
125 0 : output_copter(input);
126 0 : break;
127 :
128 0 : case OutputType::Rover:
129 0 : output_rover(input);
130 0 : break;
131 : }
132 0 : }
133 :
134 : /*
135 : very simple JSON parser for sensor data
136 : called with pointer to one row of sensor data, nul terminated
137 :
138 : This parser does not do any syntax checking, and is not at all
139 : general purpose
140 : */
141 0 : bool AirSim::parse_sensors(const char *json)
142 : {
143 : // printf("%s\n", json);
144 0 : for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
145 0 : struct keytable &key = keytable[i];
146 :
147 : /* look for section header */
148 0 : const char *p = strstr(json, key.section);
149 0 : if (!p) {
150 : // we don't have this sensor
151 0 : continue;
152 : }
153 0 : p += strlen(key.section)+1;
154 :
155 : // find key inside section
156 0 : p = strstr(p, key.key);
157 0 : if (!p) {
158 0 : printf("Failed to find key %s/%s\n", key.section, key.key);
159 0 : return false;
160 : }
161 :
162 0 : p += strlen(key.key)+3;
163 0 : switch (key.type) {
164 0 : case DATA_UINT64:
165 0 : *((uint64_t *)key.ptr) = strtoul(p, nullptr, 10);
166 0 : break;
167 :
168 0 : case DATA_FLOAT:
169 0 : *((float *)key.ptr) = strtof(p, nullptr);
170 0 : break;
171 :
172 0 : case DATA_DOUBLE:
173 0 : *((double *)key.ptr) = atof(p);
174 0 : break;
175 :
176 0 : case DATA_VECTOR3F: {
177 0 : Vector3f *v = (Vector3f *)key.ptr;
178 0 : if (sscanf(p, "[%f, %f, %f]", &v->x, &v->y, &v->z) != 3) {
179 0 : printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key);
180 0 : return false;
181 : }
182 0 : break;
183 : }
184 :
185 0 : case DATA_VECTOR3F_ARRAY: {
186 : // - array of floats that represent [x,y,z] coordinate for each point hit within the range
187 : // x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
188 : // example: [23.1,0.677024,1.4784,-8.97607135772705,-8.976069450378418,-8.642673492431641e-07,]
189 0 : if (*p++ != '[') {
190 0 : return false;
191 : }
192 0 : uint16_t n = 0;
193 0 : vector3f_array *v = (vector3f_array *)key.ptr;
194 : while (true) {
195 0 : if (n >= v->length) {
196 0 : Vector3f *d = (Vector3f *)realloc(v->data, sizeof(Vector3f)*(n+1));
197 0 : if (d == nullptr) {
198 0 : return false;
199 : }
200 0 : v->data = d;
201 0 : v->length = n+1;
202 : }
203 0 : if (sscanf(p, "%f,%f,%f,", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) {
204 0 : printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n);
205 0 : return false;
206 : }
207 0 : n++;
208 : // Goto 3rd occurence of ,
209 0 : p = strchr(p,',');
210 0 : if (!p) {
211 0 : return false;
212 : }
213 0 : p++;
214 0 : p = strchr(p,',');
215 0 : if (!p) {
216 0 : return false;
217 : }
218 0 : p++;
219 0 : p = strchr(p,',');
220 0 : if (!p) {
221 0 : return false;
222 : }
223 0 : p++;
224 : // Reached end of point cloud
225 0 : if (p[0] == ']') {
226 0 : break;
227 : }
228 0 : }
229 0 : v->length = n;
230 0 : break;
231 : }
232 :
233 0 : case DATA_FLOAT_ARRAY: {
234 : // example: [18.0, 12.694079399108887]
235 0 : if (*p++ != '[') {
236 0 : return false;
237 : }
238 0 : uint16_t n = 0;
239 0 : float_array *v = (float_array *)key.ptr;
240 : while (true) {
241 0 : if (n >= v->length) {
242 0 : float *d = (float *)realloc(v->data, sizeof(float)*(n+1));
243 0 : if (d == nullptr) {
244 0 : return false;
245 : }
246 0 : v->data = d;
247 0 : v->length = n+1;
248 : }
249 0 : v->data[n] = strtof(p, nullptr);
250 0 : n++;
251 0 : p = strchr(p,',');
252 0 : if (!p) {
253 0 : break;
254 : }
255 0 : p++;
256 0 : }
257 0 : v->length = n;
258 0 : break;
259 : }
260 : }
261 : }
262 0 : return true;
263 : }
264 :
265 : /*
266 : Receive new sensor data from simulator
267 : This is a blocking function
268 : */
269 0 : void AirSim::recv_fdm(const sitl_input& input)
270 : {
271 : // Receive sensor packet
272 0 : ssize_t ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
273 0 : uint32_t wait_ms = UDP_TIMEOUT_MS;
274 0 : while (ret <= 0) {
275 : // printf("No sensor message received - %s\n", strerror(errno));
276 0 : ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
277 0 : wait_ms += UDP_TIMEOUT_MS;
278 :
279 : // If no sensor message is received after 1 second, resend servos
280 : // this helps if messages are lost on the way, and both AP & Airsim are waiting for each ther
281 0 : if (wait_ms > 1000) {
282 0 : wait_ms = 0;
283 0 : printf("No sensor message received in last 1s, error - %s, resending servos\n", strerror(errno));
284 0 : output_servos(input);
285 : }
286 : }
287 :
288 : // convert '\n' into nul
289 0 : while (uint8_t *p = (uint8_t *)memchr(&sensor_buffer[sensor_buffer_len], '\n', ret)) {
290 0 : *p = 0;
291 0 : }
292 0 : sensor_buffer_len += ret;
293 :
294 0 : const uint8_t *p2 = (const uint8_t *)memrchr(sensor_buffer, 0, sensor_buffer_len);
295 0 : if (p2 == nullptr || p2 == sensor_buffer) {
296 0 : return;
297 : }
298 0 : const uint8_t *p1 = (const uint8_t *)memrchr(sensor_buffer, 0, p2 - sensor_buffer);
299 0 : if (p1 == nullptr) {
300 0 : return;
301 : }
302 :
303 0 : parse_sensors((const char *)(p1+1));
304 :
305 0 : memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
306 0 : sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
307 :
308 0 : accel_body = state.imu.linear_acceleration;
309 0 : gyro = state.imu.angular_velocity;
310 0 : velocity_ef = state.velocity.world_linear_velocity;
311 :
312 0 : location = {
313 0 : int32_t(state.gps.lat * 1.0e7),
314 0 : int32_t(state.gps.lon * 1.0e7),
315 0 : int32_t(state.gps.alt * 100.0f),
316 : Location::AltFrame::ABSOLUTE
317 : };
318 :
319 0 : position = origin.get_distance_NED_double(location);
320 :
321 0 : dcm.from_euler(state.pose.roll, state.pose.pitch, state.pose.yaw);
322 :
323 0 : if (last_timestamp) {
324 0 : int deltat = state.timestamp - last_timestamp;
325 0 : time_now_us += deltat;
326 :
327 0 : if (deltat > 0 && deltat < 100000) {
328 0 : if (average_frame_time < 1) {
329 0 : average_frame_time = deltat;
330 : }
331 0 : average_frame_time = average_frame_time * 0.98 + deltat * 0.02;
332 : }
333 : }
334 :
335 0 : scanner.points = state.lidar.points;
336 :
337 : // Update RC input, max 12 channels
338 0 : rcin_chan_count = MIN(state.rc.rc_channels.length, 12);
339 0 : for (uint8_t i=0; i < rcin_chan_count; i++) {
340 0 : rcin[i] = state.rc.rc_channels.data[i];
341 : }
342 :
343 : // Update Rangefinder data, max sensors limit as defined
344 0 : uint8_t rng_sensor_count = MIN(state.rng.rng_distances.length, ARRAY_SIZE(rangefinder_m));
345 0 : for (uint8_t i=0; i<rng_sensor_count; i++) {
346 0 : rangefinder_m[i] = state.rng.rng_distances.data[i];
347 : }
348 :
349 : // AirSim does not provide battery information, so use the standard battery model.
350 0 : update_battery();
351 :
352 : #if 0
353 : // @LoggerMessage: ASM1
354 : // @Description: AirSim simulation data
355 : // @Field: TimeUS: Time since system startup
356 : // @Field: TUS: Simulation's timestamp
357 : // @Field: R: Simulation's roll
358 : // @Field: P: Simulation's pitch
359 : // @Field: Y: Simulation's yaw
360 : // @Field: GX: Simulated gyroscope, X-axis
361 : // @Field: GY: Simulated gyroscope, Y-axis
362 : // @Field: GZ: Simulated gyroscope, Z-axis
363 : AP::logger().WriteStreaming("ASM1", "TimeUS,TUS,R,P,Y,GX,GY,GZ",
364 : "QQffffff",
365 : AP_HAL::micros64(),
366 : state.timestamp,
367 : degrees(state.pose.roll),
368 : degrees(state.pose.pitch),
369 : degrees(state.pose.yaw),
370 : degrees(gyro.x),
371 : degrees(gyro.y),
372 : degrees(gyro.z));
373 :
374 : Vector3f velocity_bf = dcm.transposed() * velocity_ef;
375 :
376 : // @LoggerMessage: ASM2
377 : // @Description: More AirSim simulation data
378 : // @Field: TimeUS: Time since system startup
379 : // @Field: AX: simulation's acceleration, X-axis
380 : // @Field: AY: simulation's acceleration, Y-axis
381 : // @Field: AZ: simulation's acceleration, Z-axis
382 : // @Field: VX: simulation's velocity, X-axis
383 : // @Field: VY: simulation's velocity, Y-axis
384 : // @Field: VZ: simulation's velocity, Z-axis
385 : // @Field: PX: simulation's position, X-axis
386 : // @Field: PY: simulation's position, Y-axis
387 : // @Field: PZ: simulation's position, Z-axis
388 : // @Field: Alt: simulation's gps altitude
389 : // @Field: SD: simulation's earth-frame speed-down
390 : AP::logger().WriteStreaming("ASM2", "TimeUS,AX,AY,AZ,VX,VY,VZ,PX,PY,PZ,Alt,SD",
391 : "Qfffffffffff",
392 : AP_HAL::micros64(),
393 : accel_body.x,
394 : accel_body.y,
395 : accel_body.z,
396 : velocity_bf.x,
397 : velocity_bf.y,
398 : velocity_bf.z,
399 : position.x,
400 : position.y,
401 : position.z,
402 : state.gps.alt,
403 : velocity_ef.z);
404 : #endif
405 :
406 0 : last_timestamp = state.timestamp;
407 : }
408 :
409 : /*
410 : update the AirSim simulation by one time step
411 : */
412 0 : void AirSim::update(const sitl_input& input)
413 : {
414 : // Send servos to AirSim
415 0 : output_servos(input);
416 :
417 : // Receive sensor data
418 0 : recv_fdm(input);
419 :
420 : // update magnetic field
421 0 : update_mag_field_bf();
422 :
423 0 : report_FPS();
424 0 : }
425 :
426 : /*
427 : report frame rates
428 : */
429 0 : void AirSim::report_FPS(void)
430 : {
431 0 : if (frame_counter++ % 1000 == 0) {
432 0 : if (last_frame_count != 0) {
433 0 : printf("FPS avg=%.2f\n", 1.0e6/average_frame_time);
434 : }
435 0 : last_frame_count = state.timestamp;
436 : }
437 0 : }
438 :
439 : #endif // AP_SIM_AIRSIM_ENABLED
|
