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 connection for Webots 2023a
      17             : */
      18             : 
      19             : #include "SIM_config.h"
      20             : 
      21             : #if AP_SIM_WEBOTSPYTHON_ENABLED
      22             : 
      23             : #include "SIM_Webots_Python.h"
      24             : 
      25             : #include <stdio.h>
      26             : #include <errno.h>
      27             : 
      28             : namespace SITL {
      29             : 
      30           0 : WebotsPython::WebotsPython(const char *frame_str) :
      31             :         Aircraft(frame_str),
      32           0 :         last_timestamp(0),
      33           0 :         socket_sitl{true}{
      34             : 
      35             :     // disable time sync and sensor smoothing to allow for faster than realtime simulation
      36           0 :     use_time_sync = false;   
      37           0 :     use_smoothing = false;
      38             : 
      39           0 :     printf("Starting SITL Webots\n");
      40           0 : }
      41             : 
      42             : /*
      43             :   Create and set in/out socket
      44             : */
      45           0 : void WebotsPython::set_interface_ports(const char* address, const int port_in, const int port_out)
      46             : {
      47             :     // try to bind to a specific port so that if we restart ArduPilot
      48             :     // Webots keeps sending us packets. Not strictly necessary but
      49             :     // useful for debugging
      50           0 :     if (!socket_sitl.bind("0.0.0.0", port_in)) {
      51           0 :         fprintf(stderr, "SITL: socket in bind failed on sim in : %d  - %s\n", port_in, strerror(errno));
      52           0 :         fprintf(stderr, "Aborting launch...\n");
      53           0 :         exit(1);
      54             :     }
      55           0 :     printf("Bind %s:%d for SITL in\n", "127.0.0.1", port_in);
      56           0 :     socket_sitl.reuseaddress();
      57           0 :     socket_sitl.set_blocking(false);
      58             : 
      59           0 :     _webots_address = address;
      60           0 :     _webots_port = port_out;
      61           0 :     printf("Setting Webots interface to %s:%d \n", _webots_address, _webots_port);
      62           0 : }
      63             : 
      64             : /*
      65             :   Decode and send SITL outputs to FDM aka Webots (scaled to be 0-1 instead of 1000-2000)
      66             : */
      67           0 : void WebotsPython::send_servos(const struct sitl_input &input)
      68             : {
      69             :     servo_packet pkt;
      70           0 :     for (unsigned i = 0; i < 16; ++i){
      71           0 :       pkt.motor_speed[i] = (input.servos[i]-1000) / 1000.0f;
      72             :     }
      73           0 :     socket_sitl.sendto(&pkt, sizeof(pkt), _webots_address, _webots_port);
      74           0 : }
      75             : 
      76             : /*
      77             :   Receive sensor data from Webots (the Flight Dynamics Model)
      78             : */
      79           0 : void WebotsPython::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 webots reset
      90           0 :         if (get_wall_time_us() > last_wall_time_us + WEBOTS_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 :     dcm.from_euler(static_cast<float>(pkt.imu_orientation_rpy[0]),
     111           0 :                    static_cast<float>(pkt.imu_orientation_rpy[1]),
     112           0 :                    static_cast<float>(pkt.imu_orientation_rpy[2]));
     113             : 
     114           0 :     velocity_ef = Vector3f(static_cast<float>(pkt.velocity_xyz[0]),
     115           0 :                            static_cast<float>(pkt.velocity_xyz[1]),
     116           0 :                            static_cast<float>(pkt.velocity_xyz[2]));
     117             : 
     118           0 :     position = Vector3d(pkt.position_xyz[0],
     119             :                         pkt.position_xyz[1],
     120             :                         pkt.position_xyz[2]);
     121           0 :     position.xy() += origin.get_distance_NE_double(home);
     122             : 
     123             :     // auto-adjust to simulation frame rate
     124           0 :     time_now_us += static_cast<uint64_t>(deltat * 1.0e6);
     125             : 
     126           0 :     if (deltat < 0.01 && deltat > 0) {
     127           0 :         adjust_frame_time(static_cast<float>(1.0/deltat));
     128             :     }
     129           0 :     last_timestamp = pkt.timestamp;
     130             : 
     131             : }
     132             : 
     133             : /*
     134             :   Drain remaining data on the socket to prevent phase lag
     135             :  */
     136           0 : void WebotsPython::drain_sockets()
     137             : {
     138           0 :     const uint16_t buflen = 1024;
     139             :     char buf[buflen];
     140             :     ssize_t received;
     141           0 :     errno = 0;
     142             :     do {
     143           0 :         received = socket_sitl.recv(buf, buflen, 0);
     144           0 :         if (received < 0) {
     145           0 :             if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
     146           0 :                 fprintf(stderr, "error recv on socket in: %s \n",
     147           0 :                         strerror(errno));
     148             :             }
     149             :         } else {
     150             :             // fprintf(stderr, "received from control socket: %s\n", buf);
     151             :         }
     152           0 :     } while (received > 0);
     153             : 
     154           0 : }
     155             : 
     156             : /*
     157             :   Update the Webots simulation by one time step
     158             :  */
     159           0 : void WebotsPython::update(const struct sitl_input &input)
     160             : {
     161           0 :     send_servos(input);
     162           0 :     recv_fdm(input);
     163           0 :     update_battery();
     164           0 :     update_position();
     165             : 
     166           0 :     time_advance();
     167             :     // update magnetic field
     168           0 :     update_mag_field_bf();
     169           0 :     drain_sockets();
     170           0 : }
     171             : 
     172             : }  // namespace SITL
     173             : 
     174             : 
     175             : #endif  // AP_SIM_WEBOTSPYTHON_ENABLED