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             : #pragma once
      17             : 
      18             : #include "SIM_config.h"
      19             : 
      20             : #if AP_SIM_SILENTWINGS_ENABLED
      21             : 
      22             : #include <AP_HAL/utility/Socket_native.h>
      23             : 
      24             : #include "SIM_Aircraft.h"
      25             : 
      26             : namespace SITL {
      27             : 
      28             : /*
      29             :   A wrapper for a simulated Silent Wings aircraft.
      30             : */
      31             : class SilentWings : public Aircraft {
      32             : public:
      33             :     SilentWings(const char *frame_str);
      34             : 
      35             :     /* Updates the aircraft model by one time step */
      36             :     void update(const struct sitl_input &input) override;
      37             : 
      38             :     /* Static object creator */
      39           0 :     static Aircraft *create(const char *frame_str) {
      40           0 :         return NEW_NOTHROW SilentWings(frame_str);
      41             :     }
      42             : 
      43             : private:
      44             : 
      45             :     /* Reply packet sent from SilentWings to ArduPlane */
      46             :     struct PACKED fdm_packet {
      47             :            unsigned int timestamp;          // Millisec  Timestamp
      48             :            double position_latitude;        // Degrees   Position latitude,
      49             :            double position_longitude;       // Degrees            longitude,
      50             :            float  altitude_msl;             // m         Altitude w.r.t. the sea level
      51             :            float  altitude_ground;          // m         Altitude w.r.t. the ground level
      52             :            float  altitude_ground_45;       // m         Ground 45 degrees ahead (NOT IMPLEMENTED YET)
      53             :            float  altitude_ground_forward;  // m         Ground straight ahead (NOT IMPLEMENTED YET)
      54             :            float  roll;                     // Degrees
      55             :            float  pitch;                    // Degrees
      56             :            float  yaw;                      // Degrees
      57             :            float  d_roll;                   // Deg/sec   Roll speed
      58             :            float  d_pitch;                  // Deg/sec   Pitch speed
      59             :            float  d_yaw;                    // Deg/sec   Yaw speed
      60             :            float  vx;                       // m/s       Velocity vector in body-axis
      61             :            float  vy;
      62             :            float  vz;
      63             :            float  vx_wind;                  // m/s       Velocity vector in body-axis, relative to the wind
      64             :            float  vy_wind;
      65             :            float  vz_wind;
      66             :            float  v_eas;                    // m/s       Equivalent (indicated) air speed.
      67             :            float  ax;                       // m/s^2     Acceleration vector in body axis
      68             :            float  ay;
      69             :            float  az;
      70             :            float  angle_of_attack;          // Degrees   Angle of attack
      71             :            float  angle_sideslip;           // Degrees   Sideslip angle
      72             :            float  vario;                    // m/s       Total energy-compensated variometer
      73             :            float  heading;                  // Degrees   Compass heading
      74             :            float  rate_of_turn;             // Deg/sec   Rate of turn
      75             :            float  airpressure;              // Pa        Local air pressure at aircraft altitude)
      76             :            float  density;                  // Air density at aircraft altitude
      77             :            float  temperature;              // Degrees Celcius   Air temperature at aircraft altitude
      78             :     } pkt;
      79             : 
      80             :     struct {
      81             :         uint32_t last_report_ms;
      82             :         uint32_t data_count;
      83             :         uint32_t frame_count;
      84             :     } report;
      85             : 
      86             :     bool recv_fdm(void);
      87             :     void process_packet();
      88             :     bool interim_update();
      89             : 
      90             :     /*  Create and set in/out socket for Silent Wings simulator */
      91             :     void set_interface_ports(const char* address, const int port_in, const int port_out) override;
      92             : 
      93             :     /* Sends control inputs to the Silent Wings. */
      94             :     void send_servos(const struct sitl_input &input);
      95             : 
      96             :     /* Timestamp of the latest data packet received from Silent Wings. */
      97             :     uint32_t last_data_time_ms;
      98             : 
      99             :     /* Timestamp of the first data packet received from Silent Wings. */
     100             :     uint32_t first_pkt_timestamp_ms;
     101             : 
     102             :     /* Indicates whether first_pkt_timestamp_ms has been initialized (i.e., any packets have been received from Silent Wings. */
     103             :     bool inited_first_pkt_timestamp;
     104             : 
     105             :     /* ArduPlane's internal time when the first packet from Silent Wings is received. */
     106             :     uint64_t time_base_us;
     107             : 
     108             :     SocketAPM_native sock;
     109             :     const char *_sw_address = "127.0.0.1";
     110             :     int _port_in = 6060;
     111             :     int _sw_port = 6070;
     112             : 
     113             :     bool home_initialized;
     114             : };
     115             : 
     116             : } // namespace SITL
     117             : 
     118             : #endif  // AP_SIM_SILENTWINGS_ENABLED