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             :   simulate GPS sensors
      17             : 
      18             :   Usage example:
      19             : param set SERIAL5_PROTOCOL 5
      20             : 
      21             :      sim_vehicle.py -D --console --map -A --serial5=sim:gps:2
      22             : */
      23             : 
      24             : #pragma once
      25             : 
      26             : #include "SIM_config.h"
      27             : 
      28             : #if AP_SIM_GPS_ENABLED
      29             : 
      30             : #include <sys/time.h>
      31             : #include "SIM_SerialDevice.h"
      32             : 
      33             : namespace SITL {
      34             : 
      35             : // for delay simulation:
      36             : struct GPS_Data {
      37             :     uint32_t timestamp_ms;
      38             :     double latitude;
      39             :     double longitude;
      40             :     float altitude;
      41             :     double speedN;
      42             :     double speedE;
      43             :     double speedD;
      44             :     double yaw_deg;
      45             :     double roll_deg;
      46             :     double pitch_deg;
      47             :     bool have_lock;
      48             :     float horizontal_acc;
      49             :     float vertical_acc;
      50             :     float speed_acc;
      51             :     uint8_t num_sats;
      52             :     uint8_t fix_type;
      53             : 
      54             :     // Get course over ground [rad], where 0 = North in WGS-84 coordinate system.
      55             :     // Calculated from 2D velocity.
      56             :     float ground_track_rad() const WARN_IF_UNUSED;
      57             : 
      58             :     // Get 2D speed [m/s] in WGS-84 coordinate system
      59             :     float speed_2d() const WARN_IF_UNUSED;
      60             : };
      61             : 
      62             : 
      63             : class GPS_Backend {
      64             : public:
      65             :     CLASS_NO_COPY(GPS_Backend);
      66             : 
      67             :     GPS_Backend(class GPS &front, uint8_t _instance);
      68          14 :     virtual ~GPS_Backend() {}
      69             : 
      70             :     // 0 baud means "unset" i.e. baud-rate checks should not apply
      71   495556687 :     virtual uint32_t device_baud() const { return 0; }
      72             : 
      73             :     ssize_t write_to_autopilot(const char *p, size_t size) const;
      74             :     ssize_t read_from_autopilot(char *buffer, size_t size) const;
      75             : 
      76             :     // read and process config from autopilot (e.g.)
      77             :     virtual void update_read();
      78             :     // writing fix information to autopilot (e.g.)
      79             :     virtual void publish(const GPS_Data *d) = 0;
      80             : 
      81             :     struct GPS_TOW {
      82             :         // Number of weeks since midnight 5-6 January 1980
      83             :         uint16_t week;
      84             :         // Time since start of the GPS week [mS]
      85             :         uint32_t ms;
      86             :     };
      87             : 
      88             :     static GPS_TOW gps_time();
      89             : 
      90             : protected:
      91             : 
      92             :     uint8_t instance;
      93             :     GPS &front;
      94             : 
      95             :     class SIM *_sitl;
      96             : 
      97             :     static void simulation_timeval(struct timeval *tv);
      98             : };
      99             : 
     100             : class GPS : public SerialDevice {
     101             : public:
     102             : 
     103             :     CLASS_NO_COPY(GPS);
     104             : 
     105             :     enum Type {
     106             :         NONE  =  0,
     107             : #if AP_SIM_GPS_UBLOX_ENABLED
     108             :         UBLOX =  1,
     109             : #endif
     110             : #if AP_SIM_GPS_NMEA_ENABLED
     111             :         NMEA  =  5,
     112             : #endif
     113             : #if AP_SIM_GPS_SBP_ENABLED
     114             :         SBP   =  6,
     115             : #endif
     116             : #if AP_SIM_GPS_FILE_ENABLED
     117             :         FILE  =  7,
     118             : #endif
     119             : #if AP_SIM_GPS_NOVA_ENABLED
     120             :         NOVA  =  8,
     121             : #endif
     122             : #if AP_SIM_GPS_SBP2_ENABLED
     123             :         SBP2  =  9,
     124             : #endif
     125             : #if AP_SIM_GPS_SBF_ENABLED
     126             :         SBF = 10, //matches GPS_TYPE 
     127             : #endif
     128             : #if AP_SIM_GPS_TRIMBLE_ENABLED
     129             :         TRIMBLE  = 11, // matches GPS1_TYPE
     130             : #endif
     131             : #if AP_SIM_GPS_MSP_ENABLED
     132             :         MSP   = 19,
     133             : #endif
     134             :     };
     135             : 
     136             :     GPS(uint8_t _instance);
     137             : 
     138             :     // update state
     139             :     void update();
     140             : 
     141             :     ssize_t write_to_autopilot(const char *p, size_t size) const override;
     142             : 
     143             :     uint32_t device_baud() const override;  // 0 meaning unset
     144             : 
     145             : private:
     146             : 
     147             :     uint8_t instance;
     148             : 
     149             :     // The last time GPS data was written [mS]
     150             :     uint32_t last_write_update_ms;
     151             : 
     152             :     // last 20 samples, allowing for up to 20 samples of delay
     153             :     GPS_Data _gps_history[20];
     154             : 
     155             :     // state of jamming simulation
     156             :     struct {
     157             :         uint32_t last_jam_ms;
     158             :         uint32_t jam_start_ms;
     159             :         uint32_t last_sats_change_ms;
     160             :         uint32_t last_vz_change_ms;
     161             :         uint32_t last_vel_change_ms;
     162             :         uint32_t last_pos_change_ms;
     163             :         uint32_t last_acc_change_ms;
     164             :         double latitude;
     165             :         double longitude;
     166             :     } jamming[2];
     167             : 
     168             :     bool _gps_has_basestation_position;
     169             :     GPS_Data _gps_basestation_data;
     170             : 
     171             :     void simulate_jamming(GPS_Data &d);
     172             : 
     173             :     // get delayed data
     174             :     GPS_Data interpolate_data(const GPS_Data &d, uint32_t delay_ms);
     175             : 
     176             :     uint8_t allocated_type;
     177             :     GPS_Backend *backend;
     178             :     void check_backend_allocation();
     179             : };
     180             : 
     181             : }
     182             : 
     183             : #endif  // AP_SIM_GPS_ENABLED