SITL: added gazebo SITL shim

libraries/SITL/SIM_Gazebo.cpp

@@ -0,0 +1,130 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulator connector for ardupilot version of Gazebo
+*/
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "SIM_Gazebo.h"
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  constructor
+ */
+Gazebo::Gazebo(const char *home_str, const char *frame_str) :
+    Aircraft(home_str, frame_str),
+    last_timestamp(0),
+    sock(true)
+{
+    // try to bind to a specific port so that if we restart ArduPilot
+    // Gazebo keeps sending us packets. Not strictly necessary but
+    // useful for debugging
+    sock.bind("127.0.0.1", 9003);
+
+    sock.reuseaddress();
+    sock.set_blocking(false);
+    fprintf(stdout, "bind\n");
+}
+
+/*
+  decode and send servos
+*/
+void Gazebo::send_servos(const struct sitl_input &input)
+{
+    servo_packet pkt;
+    pkt.motor_speed[0] = (input.servos[0]-1000) / 1000.0f;
+    pkt.motor_speed[1] = (input.servos[1]-1000) / 1000.0f;
+    pkt.motor_speed[2] = (input.servos[2]-1000) / 1000.0f;
+    pkt.motor_speed[3] = (input.servos[3]-1000) / 1000.0f;
+    sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", 9002);
+}
+
+/*
+  receive an update from the FDM
+  This is a blocking function
+ */
+void Gazebo::recv_fdm(const struct sitl_input &input)
+{
+    fdm_packet pkt;
+
+    /*
+      we re-send the servo packet every 0.1 seconds until we get a
+      reply. This allows us to cope with some packet loss to the FDM
+     */
+    while (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
+        send_servos(input);
+    }
+
+    // get imu stuff
+    accel_body = Vector3f(pkt.imu_linear_acceleration_xyz[0],
+                          pkt.imu_linear_acceleration_xyz[1],
+                          pkt.imu_linear_acceleration_xyz[2]);
+
+    gyro = Vector3f(pkt.imu_angular_velocity_rpy[0],
+                    pkt.imu_angular_velocity_rpy[1],
+                    pkt.imu_angular_velocity_rpy[2]);
+
+    // compute dcm from imu orientation
+    Quaternion quat(pkt.imu_orientation_quat[0],
+                    pkt.imu_orientation_quat[1],
+                    pkt.imu_orientation_quat[2],
+                    pkt.imu_orientation_quat[3]);
+    quat.rotation_matrix(dcm);
+
+    double speedN =  pkt.velocity_xyz[0];
+    double speedE =  pkt.velocity_xyz[1];
+    double speedD =  pkt.velocity_xyz[2];
+    velocity_ef = Vector3f(speedN, speedE, speedD);
+
+    position = Vector3f(pkt.position_xyz[0],
+                        pkt.position_xyz[1],
+                        pkt.position_xyz[2]);
+
+
+    // auto-adjust to simulation frame rate
+    double deltat = pkt.timestamp - last_timestamp;
+    time_now_us += deltat * 1.0e6;
+
+    if (deltat < 0.01 && deltat > 0) {
+        adjust_frame_time(1.0/deltat);
+    }
+    last_timestamp = pkt.timestamp;
+
+    /* copied below from iris_ros.py */
+    /*
+    bearing = to_degrees(atan2(position.y, position.x));
+    distance = math.sqrt(self.position.x**2 + self.position.y**2)
+    (self.latitude, self.longitude) = util.gps_newpos(
+      self.home_latitude, self.home_longitude, bearing, distance)
+    self.altitude  = self.home_altitude - self.position.z
+    velocity_body = self.dcm.transposed() * self.velocity
+    self.accelerometer = self.accel_body.copy()
+    */
+}
+
+/*
+  update the Gazebo simulation by one time step
+ */
+void Gazebo::update(const struct sitl_input &input)
+{
+    send_servos(input);
+    recv_fdm(input);
+    update_position();
+}
+#endif // CONFIG_HAL_BOARD

libraries/SITL/SIM_Gazebo.h

@@ -0,0 +1,73 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulator connection for ardupilot version of Gazebo
+*/
+
+#ifndef _SIM_GAZEBO_H
+#define _SIM_GAZEBO_H
+
+#include "SIM_Aircraft.h"
+#include <utility/Socket.h>
+
+/*
+  Gazebo simulator
+ */
+class Gazebo : public Aircraft
+{
+public:
+    Gazebo(const char *home_str, const char *frame_str);
+
+    /* update model by one time step */
+    void update(const struct sitl_input &input);
+
+    /* static object creator */
+    static Aircraft *create(const char *home_str, const char *frame_str) {
+        return new Gazebo(home_str, frame_str);
+    }
+
+private:
+    /*
+      packet sent to Gazebo
+     */
+    struct servo_packet {
+      float motor_speed[4];
+    };
+    
+    /*
+      reply packet sent from Gazebo to ArduPilot
+     */
+    struct fdm_packet {
+      double timestamp;
+      double imu_angular_velocity_rpy[3];
+      double imu_linear_acceleration_xyz[3];
+      double imu_orientation_quat[3];
+      double velocity_xyz[3];
+      double position_xyz[3];
+    };
+
+    void send_servos_heli(const struct sitl_input &input);
+    void send_servos_fixed_wing(const struct sitl_input &input);
+    void recv_fdm(const struct sitl_input &input);
+    void send_servos(const struct sitl_input &input);
+
+    bool heli_servos;
+    double last_timestamp;
+    SocketAPM sock;
+};
+
+
+#endif // _SIM_GAZEBO_H