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: https://github.com/Microsoft/AirSim
17 : */
18 :
19 : #pragma once
20 :
21 : #include "SIM_config.h"
22 :
23 : #if AP_SIM_AIRSIM_ENABLED
24 :
25 : #include <AP_HAL/utility/Socket_native.h>
26 : #include "SIM_Aircraft.h"
27 :
28 : namespace SITL {
29 :
30 : /*
31 : Airsim Simulator
32 : */
33 :
34 : class AirSim : public Aircraft {
35 : public:
36 : AirSim(const char *frame_str);
37 :
38 : /* update model by one time step */
39 : void update(const struct sitl_input &input) override;
40 :
41 : /* static object creator */
42 0 : static Aircraft *create(const char *frame_str) {
43 0 : return NEW_NOTHROW AirSim(frame_str);
44 : }
45 :
46 : /* Create and set in/out socket for Airsim simulator */
47 : void set_interface_ports(const char* address, const int port_in, const int port_out) override;
48 :
49 : private:
50 : enum class OutputType {
51 : Copter = 1,
52 : Rover = 2
53 : } output_type;
54 :
55 : // Control packet for Rover
56 : struct rover_packet {
57 : float throttle; // -1 to 1
58 : float steering; // -1 to 1
59 : };
60 :
61 : // rotor control packet sent by Ardupilot
62 : static const int kArduCopterRotorControlCount = 11;
63 :
64 : struct servo_packet {
65 : uint16_t pwm[kArduCopterRotorControlCount];
66 : };
67 :
68 : // default connection_info_.ip_address
69 : const char *airsim_ip = "127.0.0.1";
70 :
71 : // connection_info_.ip_port
72 : uint16_t airsim_sensor_port = 9003;
73 :
74 : // connection_info_.sitl_ip_port
75 : uint16_t airsim_control_port = 9002;
76 :
77 : SocketAPM_native sock;
78 :
79 : double average_frame_time;
80 : uint64_t frame_counter;
81 : uint64_t last_frame_count;
82 : uint64_t last_timestamp;
83 :
84 : void output_copter(const sitl_input& input);
85 : void output_rover(const sitl_input& input);
86 : // Wrapper function over the above 2 output methods
87 : void output_servos(const sitl_input& input);
88 :
89 : void recv_fdm(const sitl_input& input);
90 : void report_FPS(void);
91 :
92 : bool parse_sensors(const char *json);
93 :
94 : // buffer for parsing pose data in JSON format
95 : uint8_t sensor_buffer[65000];
96 : uint32_t sensor_buffer_len;
97 :
98 : enum data_type {
99 : DATA_UINT64,
100 : DATA_FLOAT,
101 : DATA_DOUBLE,
102 : DATA_VECTOR3F,
103 : DATA_VECTOR3F_ARRAY,
104 : DATA_FLOAT_ARRAY,
105 : };
106 :
107 : struct {
108 : uint64_t timestamp;
109 : struct {
110 : Vector3f angular_velocity;
111 : Vector3f linear_acceleration;
112 : } imu;
113 : struct {
114 : double lat, lon, alt;
115 : } gps;
116 : struct {
117 : float roll, pitch, yaw;
118 : } pose;
119 : struct {
120 : Vector3f world_linear_velocity;
121 : } velocity;
122 : struct {
123 : struct vector3f_array points;
124 : } lidar;
125 : struct {
126 : struct float_array rc_channels;
127 : } rc;
128 : struct {
129 : struct float_array rng_distances;
130 : } rng;
131 : } state;
132 :
133 : // table to aid parsing of JSON sensor data
134 : struct keytable {
135 : const char *section;
136 : const char *key;
137 : void *ptr;
138 : enum data_type type;
139 : } keytable[13] = {
140 : { "", "timestamp", &state.timestamp, DATA_UINT64 },
141 : { "imu", "angular_velocity", &state.imu.angular_velocity, DATA_VECTOR3F },
142 : { "imu", "linear_acceleration", &state.imu.linear_acceleration, DATA_VECTOR3F },
143 : { "gps", "lat", &state.gps.lat, DATA_DOUBLE },
144 : { "gps", "lon", &state.gps.lon, DATA_DOUBLE },
145 : { "gps", "alt", &state.gps.alt, DATA_DOUBLE },
146 : { "pose", "roll", &state.pose.roll, DATA_FLOAT },
147 : { "pose", "pitch", &state.pose.pitch, DATA_FLOAT },
148 : { "pose", "yaw", &state.pose.yaw, DATA_FLOAT },
149 : { "velocity", "world_linear_velocity", &state.velocity.world_linear_velocity, DATA_VECTOR3F },
150 : { "lidar", "point_cloud", &state.lidar.points, DATA_VECTOR3F_ARRAY },
151 : { "rc", "channels", &state.rc.rc_channels, DATA_FLOAT_ARRAY },
152 : { "rng", "distances", &state.rng.rng_distances, DATA_FLOAT_ARRAY },
153 : };
154 : };
155 :
156 : }
157 :
158 : #endif // AP_SIM_AIRSIM_ENABLED
|
