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 Gazebo
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_GAZEBO_ENABLED
22 :
23 : #include "SIM_Gazebo.h"
24 :
25 : #include <stdio.h>
26 : #include <errno.h>
27 :
28 : namespace SITL {
29 :
30 0 : Gazebo::Gazebo(const char *frame_str) :
31 : Aircraft(frame_str),
32 0 : last_timestamp(0),
33 0 : socket_sitl{true}
34 : {
35 0 : fprintf(stdout, "Starting SITL Gazebo\n");
36 0 : }
37 :
38 : /*
39 : Create and set in/out socket
40 : */
41 0 : void Gazebo::set_interface_ports(const char* address, const int port_in, const int port_out)
42 : {
43 : // try to bind to a specific port so that if we restart ArduPilot
44 : // Gazebo keeps sending us packets. Not strictly necessary but
45 : // useful for debugging
46 0 : if (!socket_sitl.bind("0.0.0.0", port_in)) {
47 0 : fprintf(stderr, "SITL: socket in bind failed on sim in : %d - %s\n", port_in, strerror(errno));
48 0 : fprintf(stderr, "Aborting launch...\n");
49 0 : exit(1);
50 : }
51 0 : printf("Bind %s:%d for SITL in\n", "127.0.0.1", port_in);
52 0 : socket_sitl.reuseaddress();
53 0 : socket_sitl.set_blocking(false);
54 :
55 0 : _gazebo_address = address;
56 0 : _gazebo_port = port_out;
57 0 : printf("Setting Gazebo interface to %s:%d \n", _gazebo_address, _gazebo_port);
58 0 : }
59 :
60 : /*
61 : decode and send servos
62 : */
63 0 : void Gazebo::send_servos(const struct sitl_input &input)
64 : {
65 : servo_packet pkt;
66 : // should rename servo_command
67 : // 16 because struct sitl_input.servos is 16 large in SIM_Aircraft.h
68 0 : for (unsigned i = 0; i < 16; ++i)
69 : {
70 0 : pkt.motor_speed[i] = (input.servos[i]-1000) / 1000.0f;
71 : }
72 0 : socket_sitl.sendto(&pkt, sizeof(pkt), _gazebo_address, _gazebo_port);
73 0 : }
74 :
75 : /*
76 : receive an update from the FDM
77 : This is a blocking function
78 : */
79 0 : void Gazebo::recv_fdm(const struct sitl_input &input)
80 : {
81 : fdm_packet pkt;
82 :
83 : /*
84 : we re-send the servo packet every 0.1 seconds until we get a
85 : reply. This allows us to cope with some packet loss to the FDM
86 : */
87 0 : while (socket_sitl.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
88 0 : send_servos(input);
89 : // Reset the timestamp after a long disconnection, also catch gazebo reset
90 0 : if (get_wall_time_us() > last_wall_time_us + GAZEBO_TIMEOUT_US) {
91 0 : last_timestamp = 0;
92 : }
93 : }
94 :
95 0 : const double deltat = pkt.timestamp - last_timestamp; // in seconds
96 0 : if (deltat < 0) { // don't use old packet
97 0 : time_now_us += 1;
98 0 : return;
99 : }
100 : // get imu stuff
101 0 : accel_body = Vector3f(static_cast<float>(pkt.imu_linear_acceleration_xyz[0]),
102 0 : static_cast<float>(pkt.imu_linear_acceleration_xyz[1]),
103 0 : static_cast<float>(pkt.imu_linear_acceleration_xyz[2]));
104 :
105 0 : gyro = Vector3f(static_cast<float>(pkt.imu_angular_velocity_rpy[0]),
106 0 : static_cast<float>(pkt.imu_angular_velocity_rpy[1]),
107 0 : static_cast<float>(pkt.imu_angular_velocity_rpy[2]));
108 :
109 : // compute dcm from imu orientation
110 0 : Quaternion quat(static_cast<float>(pkt.imu_orientation_quat[0]),
111 0 : static_cast<float>(pkt.imu_orientation_quat[1]),
112 0 : static_cast<float>(pkt.imu_orientation_quat[2]),
113 0 : static_cast<float>(pkt.imu_orientation_quat[3]));
114 0 : quat.rotation_matrix(dcm);
115 :
116 0 : velocity_ef = Vector3f(static_cast<float>(pkt.velocity_xyz[0]),
117 0 : static_cast<float>(pkt.velocity_xyz[1]),
118 0 : static_cast<float>(pkt.velocity_xyz[2]));
119 :
120 0 : position = Vector3d(pkt.position_xyz[0],
121 : pkt.position_xyz[1],
122 : pkt.position_xyz[2]);
123 0 : position.xy() += origin.get_distance_NE_double(home);
124 :
125 : // auto-adjust to simulation frame rate
126 0 : time_now_us += static_cast<uint64_t>(deltat * 1.0e6);
127 :
128 0 : if (deltat < 0.01 && deltat > 0) {
129 0 : adjust_frame_time(static_cast<float>(1.0/deltat));
130 : }
131 0 : last_timestamp = pkt.timestamp;
132 :
133 : }
134 :
135 : /*
136 : Drain remaining data on the socket to prevent phase lag.
137 : */
138 0 : void Gazebo::drain_sockets()
139 : {
140 0 : const uint16_t buflen = 1024;
141 : char buf[buflen];
142 : ssize_t received;
143 0 : errno = 0;
144 : do {
145 0 : received = socket_sitl.recv(buf, buflen, 0);
146 0 : if (received < 0) {
147 0 : if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
148 0 : fprintf(stderr, "error recv on socket in: %s \n",
149 0 : strerror(errno));
150 : }
151 : } else {
152 : // fprintf(stderr, "received from control socket: %s\n", buf);
153 : }
154 0 : } while (received > 0);
155 :
156 0 : }
157 :
158 : /*
159 : update the Gazebo simulation by one time step
160 : */
161 0 : void Gazebo::update(const struct sitl_input &input)
162 : {
163 0 : send_servos(input);
164 0 : recv_fdm(input);
165 0 : update_battery();
166 0 : update_position();
167 :
168 0 : time_advance();
169 : // update magnetic field
170 0 : update_mag_field_bf();
171 0 : drain_sockets();
172 0 : }
173 :
174 : } // namespace SITL
175 :
176 :
177 : #endif // AP_SIM_GAZEBO_ENABLED
|
