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 ardupilot version of SilentWings
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_SILENTWINGS_ENABLED
22 :
23 : #include "SIM_SilentWings.h"
24 :
25 : #include <stdio.h>
26 : #include <errno.h>
27 :
28 : #include <AP_HAL/AP_HAL.h>
29 :
30 : extern const AP_HAL::HAL& hal;
31 :
32 : using namespace SITL;
33 :
34 : static const struct {
35 : const char *name;
36 : float value;
37 : bool save;
38 : } sim_defaults[] = {
39 : { "AHRS_EKF_TYPE", 10 },
40 : { "INS_GYR_CAL", 0 },
41 : { "EK2_ENABLE", 0 },
42 : { "ARSPD_ENABLE", 1 },
43 : { "ARSPD_USE", 1 },
44 : { "INS_ACC2OFFS_X", 0.001 },
45 : { "INS_ACC2OFFS_Y", 0.001 },
46 : { "INS_ACC2OFFS_Z", 0.001 },
47 : { "INS_ACC2SCAL_X", 1.001 },
48 : { "INS_ACC2SCAL_Y", 1.001 },
49 : { "INS_ACC2SCAL_Z", 1.001 },
50 : { "INS_ACCOFFS_X", 0.001 },
51 : { "INS_ACCOFFS_Y", 0.001 },
52 : { "INS_ACCOFFS_Z", 0.001 },
53 : { "INS_ACCSCAL_X", 1.001 },
54 : { "INS_ACCSCAL_Y", 1.001 },
55 : { "INS_ACCSCAL_Z", 1.001 },
56 : };
57 :
58 0 : SilentWings::SilentWings(const char *frame_str) :
59 : Aircraft(frame_str),
60 0 : last_data_time_ms(0),
61 0 : first_pkt_timestamp_ms(0),
62 0 : inited_first_pkt_timestamp(false),
63 0 : time_base_us(0),
64 0 : sock(true),
65 0 : home_initialized(false)
66 : {
67 : // Force ArduPlane to use sensor data from SilentWings as the actual state,
68 : // without using EKF, i.e., using "fake EKF (type 10)". Disable gyro calibration.
69 : // Set a few other parameters to specific values to keep the calibration checks happy.
70 : // TO DO: fix this. Setting parameters in this way doesn't appear to have any effect.
71 0 : for (uint8_t i = 0; i < ARRAY_SIZE(sim_defaults); i++) {
72 0 : AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
73 0 : if (sim_defaults[i].save) {
74 : enum ap_var_type ptype;
75 0 : AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
76 0 : if (!p->configured()) {
77 0 : p->save();
78 : }
79 : }
80 : }
81 0 : }
82 :
83 :
84 : /*
85 : Create and set in/out socket
86 : */
87 0 : void SilentWings::set_interface_ports(const char* address, const int port_in, const int port_out)
88 : {
89 : // Ignore the specified port_in.
90 : // try to bind to a specific port so that if we restart ArduPilot
91 : // Gazebo keeps sending us packets. Not strictly necessary but
92 : // useful for debugging
93 0 : if (!sock.bind("127.0.0.1", _port_in)) {
94 0 : fprintf(stderr, "SITL: socket in bind failed on sim in : %d - %s\n", _port_in, strerror(errno));
95 0 : fprintf(stderr, "Aborting launch...\n");
96 0 : exit(1);
97 : }
98 0 : printf("Bind %s:%d for SITL in\n", "127.0.0.1", _port_in);
99 0 : sock.reuseaddress();
100 0 : sock.set_blocking(false);
101 :
102 0 : _sw_address = address;
103 : // Ignore the specified port_out.
104 0 : printf("Setting Silent Wings interface to %s:%d \n", _sw_address, _sw_port);
105 0 : }
106 :
107 :
108 : /*
109 : decode and send servos
110 : */
111 0 : void SilentWings::send_servos(const struct sitl_input &input)
112 : {
113 0 : char *buf = nullptr;
114 : // Turn off direct joystick input to the simulator. All joystick commands
115 : // should go through Mission Planner and get properly fused with ArduPlane's
116 : // control inputs when in automatic flight modes.
117 0 : float joystick = 0.0f;
118 0 : float aileron = filtered_servo_angle(input, 0);
119 0 : float elevator = filtered_servo_angle(input, 1);
120 0 : float throttle = filtered_servo_range(input, 2);
121 0 : float rudder = filtered_servo_angle(input, 3);
122 :
123 0 : ssize_t buflen = asprintf(&buf,
124 : "JOY %f\n"
125 : "AIL %f\n"
126 : "ELE %f\n"
127 : "RUD %f\n"
128 : "THR %f\n",
129 0 : joystick, aileron, elevator, rudder, throttle) - 1;
130 :
131 0 : if (buflen < 0) {
132 0 : fprintf(stderr, "Fatal: Failed to allocate enough space for data\n"),
133 0 : exit(1);
134 : }
135 :
136 0 : ssize_t sent = sock.sendto(buf, buflen, _sw_address, _sw_port);
137 0 : free(buf);
138 :
139 0 : if (sent < 0) {
140 0 : fprintf(stderr, "Fatal: Failed to send on control socket\n"),
141 0 : exit(1);
142 : }
143 :
144 0 : if (sent < buflen) {
145 0 : fprintf(stderr, "Failed to send all bytes on control socket\n");
146 : }
147 0 : }
148 :
149 :
150 : /*
151 : Receive an update from the FDM
152 : This is a blocking function
153 : */
154 0 : bool SilentWings::recv_fdm(void)
155 : {
156 : fdm_packet tmp_pkt;
157 0 : memset(&pkt, 0, sizeof(pkt));
158 :
159 0 : ssize_t nread = sock.recv(&tmp_pkt, sizeof(pkt), 0);
160 :
161 : // nread == -1 (255) means no data has arrived
162 0 : if (nread != sizeof(pkt)) {
163 0 : return false;
164 : }
165 :
166 0 : memcpy(&pkt, &tmp_pkt, sizeof(pkt));
167 :
168 : // data received successfully
169 0 : return true;
170 : }
171 :
172 :
173 0 : void SilentWings::process_packet()
174 : {
175 : // pkt.timestamp is the time of day in SilentWings, measured in ms
176 : // since midnight.
177 : // TO DO: check what happens when a flight in SW crosses midnight
178 0 : if (inited_first_pkt_timestamp) {
179 0 : uint64_t tus = (pkt.timestamp - first_pkt_timestamp_ms) * 1.0e3f;
180 0 : time_now_us = time_base_us + tus;
181 : }
182 : else {
183 0 : first_pkt_timestamp_ms = pkt.timestamp;
184 0 : time_base_us = time_now_us;
185 0 : inited_first_pkt_timestamp = true;
186 : }
187 :
188 0 : dcm.from_euler(radians(pkt.roll), radians(pkt.pitch), radians(pkt.yaw));
189 0 : accel_body = Vector3f(pkt.ax * GRAVITY_MSS, pkt.ay * GRAVITY_MSS, pkt.az * GRAVITY_MSS); // This is g-load.
190 0 : gyro = Vector3f(radians(pkt.d_roll), radians(pkt.d_pitch), radians(pkt.d_yaw));
191 : // SilentWings provides velocity in body frame.
192 0 : velocity_ef = dcm * Vector3f(pkt.vx, pkt.vy, pkt.vz);
193 : // SilentWings also provides velocity in body frame w.r.t. the wind, from which we can infer the wind.
194 0 : wind_ef = dcm * (Vector3f(pkt.vx, pkt.vy, pkt.vz) - Vector3f(pkt.vx_wind, pkt.vy_wind, pkt.vz_wind));
195 0 : airspeed = pkt.v_eas;
196 0 : airspeed_pitot = pkt.v_eas;
197 0 : const float alt_abs_m = pkt.altitude_msl;
198 : Location curr_location{
199 0 : int32_t(pkt.position_latitude * 1.0e7),
200 0 : int32_t(pkt.position_longitude * 1.0e7),
201 0 : int32_t(alt_abs_m * 100.0f),
202 : Location::AltFrame::ABSOLUTE
203 0 : };
204 0 : ground_level = alt_abs_m - pkt.altitude_ground;
205 0 : Vector3f posdelta = origin.get_distance_NED(curr_location);
206 0 : position.x = posdelta.x;
207 0 : position.y = posdelta.y;
208 0 : position.z = posdelta.z;
209 0 : update_position();
210 :
211 : // In case Silent Wings' reported location and our location calculated using an offset from the home location diverge, we need
212 : // to reset the home location.
213 0 : if (curr_location.get_distance(location) > 4 || abs(curr_location.alt - location.alt)*0.01f > 2.0f || !home_initialized) {
214 0 : printf("SilentWings home reset dist=%f alt=%.1f/%.1f\n",
215 0 : curr_location.get_distance(location), curr_location.alt*0.01f, location.alt*0.01f);
216 : // reset home location
217 : // Resetting altitude reference point in flight can throw off a bunch
218 : // of important calculations, so let the home altitude always be 0m MSL
219 0 : home = {
220 : curr_location.lat,
221 : curr_location.lng,
222 : 0,
223 : Location::AltFrame::ABSOLUTE,
224 : };
225 0 : origin.lat = home.lat;
226 0 : origin.lng = home.lng;
227 0 : position.x = 0;
228 0 : position.y = 0;
229 0 : position.z = -alt_abs_m;;
230 0 : home_initialized = true;
231 0 : update_position();
232 : }
233 :
234 : // Auto-adjust to Silent Wings' frame rate
235 : // This affects the data rate (without this adjustment, the data rate is
236 : // low no matter what the output_udp_rate in SW's options.dat file is).
237 0 : double deltat = (AP_HAL::millis() - last_data_time_ms) / 1000.0f;
238 :
239 0 : if (deltat < 0.01 && deltat > 0) {
240 0 : adjust_frame_time(1.0/deltat);
241 : }
242 :
243 0 : last_data_time_ms = AP_HAL::millis();
244 :
245 0 : report.data_count++;
246 0 : report.frame_count++;
247 :
248 : if (0) {
249 : printf("Delta: %f Time: %" PRIu64 "\n", deltat, time_now_us);
250 : printf("Accel.x %f\n", accel_body.x);
251 : printf("Accel.y %f\n", accel_body.y);
252 : printf("Accel.z %f\n", accel_body.z);
253 : printf("Gyro.x %f\n", gyro.x);
254 : printf("Gyro.y %f\n", gyro.y);
255 : printf("Gyro.z %f\n", gyro.z);
256 : printf("Pos.x %f\n", position.x);
257 : printf("Pos.y %f\n", position.y);
258 : printf("Pos.z %f\n", position.z);
259 : printf("Roll %f\n", pkt.roll);
260 : printf("Pitch %f\n", pkt.pitch);
261 : printf("Yaw %f\n", pkt.yaw);
262 : }
263 0 : }
264 :
265 : /*
266 : Extrapolates sensor data if Silent Wings hasn't sent us a data packet in a while.
267 : */
268 0 : bool SilentWings::interim_update()
269 : {
270 0 : if (AP_HAL::millis() - last_data_time_ms > 200) {
271 : // don't extrapolate beyond 0.2s
272 0 : return false;
273 : }
274 :
275 0 : float delta_time = frame_time_us * 1e-6f;
276 0 : time_now_us += frame_time_us;
277 0 : extrapolate_sensors(delta_time);
278 0 : update_position();
279 0 : report.frame_count++;
280 0 : return true;
281 : }
282 :
283 :
284 : /*
285 : Update the Silent Wings simulation by one time step.
286 : */
287 0 : void SilentWings::update(const struct sitl_input &input)
288 : {
289 0 : if (recv_fdm()) {
290 0 : process_packet();
291 : // Time has been advanced by process_packet(.)
292 0 : send_servos(input);
293 : }
294 0 : else if (interim_update()) {
295 : // This clause is triggered only if we previously
296 : // received at least one data packet.
297 : // Time has been advanced by interim_update(.)
298 0 : send_servos(input);
299 : }
300 :
301 : // This clause is triggered if and only if we haven't received
302 : // any data packets yet (and therefore didn't attempt
303 : // extrapolating data via interim_update(.) either).
304 0 : if (!inited_first_pkt_timestamp){
305 0 : time_advance();
306 : }
307 : else {
308 0 : if (use_time_sync) {
309 0 : sync_frame_time();
310 : }
311 : }
312 :
313 0 : update_mag_field_bf();
314 0 : update_battery();
315 :
316 0 : uint32_t now = AP_HAL::millis();
317 :
318 0 : if (report.last_report_ms == 0) {
319 0 : report.last_report_ms = now;
320 0 : printf("Resetting last report time to now\n");
321 : }
322 :
323 0 : if (now - report.last_report_ms > 5000) {
324 0 : float dt = (now - report.last_report_ms) * 1.0e-3f;
325 0 : printf("Data rate: %.1f FPS Frame rate: %.1f FPS\n",
326 0 : report.data_count/dt, report.frame_count/dt);
327 0 : report.last_report_ms = now;
328 0 : report.data_count = 0;
329 0 : report.frame_count = 0;
330 : }
331 0 : }
332 :
333 : #endif // AP_SIM_SILENTWINGS_ENABLED
|
