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 : #pragma once
16 :
17 : #include "SIM_config.h"
18 :
19 : #if AP_SIM_JSON_ENABLED
20 :
21 : #include <AP_HAL/utility/Socket.h>
22 : #include "SIM_Aircraft.h"
23 :
24 : #define SITL_JSON_DEBUG 0
25 :
26 : namespace SITL {
27 :
28 : class JSON : public Aircraft {
29 : public:
30 : JSON(const char *frame_str);
31 :
32 : /* update model by one time step */
33 : void update(const struct sitl_input &input) override;
34 :
35 : /* static object creator */
36 0 : static Aircraft *create(const char *frame_str) {
37 0 : return NEW_NOTHROW JSON(frame_str);
38 : }
39 :
40 : /* Create and set in/out socket for JSON generic simulator */
41 : void set_interface_ports(const char* address, const int port_in, const int port_out) override;
42 :
43 : private:
44 :
45 : struct servo_packet_16 {
46 : uint16_t magic = 18458; // constant magic value
47 : uint16_t frame_rate;
48 : uint32_t frame_count;
49 : uint16_t pwm[16];
50 : };
51 :
52 : struct servo_packet_32 {
53 : uint16_t magic = 29569; // constant magic value
54 : uint16_t frame_rate;
55 : uint32_t frame_count;
56 : uint16_t pwm[32];
57 : };
58 :
59 : // default connection_info_.ip_address
60 : const char *target_ip = "127.0.0.1";
61 :
62 : // default connection_info_.sitl_ip_port
63 : uint16_t control_port = 9002;
64 :
65 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
66 : SocketAPM_native sock;
67 : #else
68 : // sim-on-hardware
69 : SocketAPM sock;
70 : #endif
71 :
72 : uint32_t frame_counter;
73 : double last_timestamp_s;
74 :
75 : void output_servos(const struct sitl_input &input);
76 : void recv_fdm(const struct sitl_input &input);
77 :
78 : uint64_t parse_sensors(const char *json);
79 :
80 : // buffer for parsing pose data in JSON format
81 : uint8_t sensor_buffer[65000];
82 : uint32_t sensor_buffer_len;
83 :
84 : enum data_type {
85 : DATA_UINT64,
86 : DATA_FLOAT,
87 : DATA_DOUBLE,
88 : DATA_VECTOR3F,
89 : DATA_VECTOR3D,
90 : QUATERNION,
91 : BOOLEAN,
92 : };
93 :
94 : struct {
95 : double timestamp_s;
96 : double latitude;
97 : double longitude;
98 : double altitude;
99 : struct {
100 : Vector3f gyro;
101 : Vector3f accel_body;
102 : } imu;
103 : Vector3d position;
104 : Vector3f attitude;
105 : Quaternion quaternion;
106 : Vector3f velocity;
107 : Vector3f velocity_wind;
108 : float rng[6];
109 : float rc[12];
110 : float bat_volt;
111 : float bat_amp;
112 : struct {
113 : float direction;
114 : float speed;
115 : } wind_vane_apparent;
116 : float airspeed;
117 : bool no_time_sync;
118 : bool no_lockstep;
119 : } state;
120 :
121 : // table to aid parsing of JSON sensor data
122 : struct keytable {
123 : const char *section;
124 : const char *key;
125 : void *ptr;
126 : enum data_type type;
127 : bool required;
128 : } keytable[36] {
129 : { "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true },
130 : { "", "latitude", &state.latitude, DATA_DOUBLE, false },
131 : { "", "longitude", &state.longitude, DATA_DOUBLE, false },
132 : { "", "altitude", &state.altitude, DATA_DOUBLE, false },
133 : { "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F, true },
134 : { "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true },
135 : { "", "position", &state.position, DATA_VECTOR3D, false },
136 : { "", "attitude", &state.attitude, DATA_VECTOR3F, false },
137 : { "", "quaternion", &state.quaternion, QUATERNION, false },
138 : { "", "velocity", &state.velocity, DATA_VECTOR3F, true },
139 : { "", "rng_1", &state.rng[0], DATA_FLOAT, false },
140 : { "", "rng_2", &state.rng[1], DATA_FLOAT, false },
141 : { "", "rng_3", &state.rng[2], DATA_FLOAT, false },
142 : { "", "rng_4", &state.rng[3], DATA_FLOAT, false },
143 : { "", "rng_5", &state.rng[4], DATA_FLOAT, false },
144 : { "", "rng_6", &state.rng[5], DATA_FLOAT, false },
145 : {"","velocity_wind", &state.velocity_wind, DATA_VECTOR3F, false},
146 : {"windvane","direction", &state.wind_vane_apparent.direction, DATA_FLOAT, false},
147 : {"windvane","speed", &state.wind_vane_apparent.speed, DATA_FLOAT, false},
148 : {"", "airspeed", &state.airspeed, DATA_FLOAT, false},
149 : {"", "no_time_sync", &state.no_time_sync, BOOLEAN, false},
150 : {"", "no_lockstep", &state.no_lockstep, BOOLEAN, false},
151 : { "rc", "rc_1", &state.rc[0], DATA_FLOAT, false },
152 : { "rc", "rc_2", &state.rc[1], DATA_FLOAT, false },
153 : { "rc", "rc_3", &state.rc[2], DATA_FLOAT, false },
154 : { "rc", "rc_4", &state.rc[3], DATA_FLOAT, false },
155 : { "rc", "rc_5", &state.rc[4], DATA_FLOAT, false },
156 : { "rc", "rc_6", &state.rc[5], DATA_FLOAT, false },
157 : { "rc", "rc_7", &state.rc[6], DATA_FLOAT, false },
158 : { "rc", "rc_8", &state.rc[7], DATA_FLOAT, false },
159 : { "rc", "rc_9", &state.rc[8], DATA_FLOAT, false },
160 : { "rc", "rc_10", &state.rc[9], DATA_FLOAT, false },
161 : { "rc", "rc_11", &state.rc[10], DATA_FLOAT, false },
162 : { "rc", "rc_12", &state.rc[11], DATA_FLOAT, false },
163 : { "battery", "voltage", &state.bat_volt, DATA_FLOAT, false },
164 : { "battery", "current", &state.bat_amp, DATA_FLOAT, false },
165 : };
166 :
167 : // Enum coresponding to the ordering of keys in the keytable.
168 : enum DataKey : uint64_t {
169 : TIMESTAMP = 0x0000000000000001ULL, // 1ULL << 0
170 : LATITUDE = 0x0000000000000002ULL, // 1ULL << 1
171 : LONGITUDE = 0x0000000000000004ULL, // 1ULL << 2
172 : ALTITUDE = 0x0000000000000008ULL, // 1ULL << 3
173 : GYRO = 0x0000000000000010ULL, // 1ULL << 4
174 : ACCEL_BODY = 0x0000000000000020ULL, // 1ULL << 5
175 : POSITION = 0x0000000000000040ULL, // 1ULL << 6
176 : EULER_ATT = 0x0000000000000080ULL, // 1ULL << 7
177 : QUAT_ATT = 0x0000000000000100ULL, // 1ULL << 8
178 : VELOCITY = 0x0000000000000200ULL, // 1ULL << 9
179 : RNG_1 = 0x0000000000000400ULL, // 1ULL << 10
180 : RNG_2 = 0x0000000000000800ULL, // 1ULL << 11
181 : RNG_3 = 0x0000000000001000ULL, // 1ULL << 12
182 : RNG_4 = 0x0000000000002000ULL, // 1ULL << 13
183 : RNG_5 = 0x0000000000004000ULL, // 1ULL << 14
184 : RNG_6 = 0x0000000000008000ULL, // 1ULL << 15
185 : WIND_VEL = 0x0000000000010000ULL, // 1ULL << 16
186 : WIND_DIR = 0x0000000000020000ULL, // 1ULL << 17
187 : WIND_SPD = 0x0000000000040000ULL, // 1ULL << 18
188 : AIRSPEED = 0x0000000000080000ULL, // 1ULL << 19
189 : TIME_SYNC = 0x0000000000100000ULL, // 1ULL << 20
190 : LOCKSTEP = 0x0000000000200000ULL, // 1ULL << 21
191 : RC_1 = 0x0000000000400000ULL, // 1ULL << 22
192 : RC_2 = 0x0000000000800000ULL, // 1ULL << 23
193 : RC_3 = 0x0000000001000000ULL, // 1ULL << 24
194 : RC_4 = 0x0000000002000000ULL, // 1ULL << 25
195 : RC_5 = 0x0000000004000000ULL, // 1ULL << 26
196 : RC_6 = 0x0000000008000000ULL, // 1ULL << 27
197 : RC_7 = 0x0000000010000000ULL, // 1ULL << 28
198 : RC_8 = 0x0000000020000000ULL, // 1ULL << 29
199 : RC_9 = 0x0000000040000000ULL, // 1ULL << 30
200 : RC_10 = 0x0000000080000000ULL, // 1ULL << 31
201 : RC_11 = 0x0000000100000000ULL, // 1ULL << 32
202 : RC_12 = 0x0000000200000000ULL, // 1ULL << 33
203 : BAT_VOLT = 0x0000000400000000ULL, // 1ULL << 34
204 : BAT_AMP = 0x0000000800000000ULL, // 1ULL << 35
205 : };
206 : uint64_t last_received_bitmask;
207 :
208 : #if SITL_JSON_DEBUG
209 : uint32_t last_debug_ms;
210 : #endif
211 :
212 : bool last_no_lockstep;
213 : };
214 :
215 : }
216 :
217 : #endif // AP_SIM_JSON_ENABLED
|
