Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_HAL/AP_HAL.h>
       4             : #include <AP_HAL/utility/RingBuffer.h>
       5             : 
       6             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
       7             : 
       8             : #include "SITL_State_common.h"
       9             : 
      10             : #if defined(HAL_BUILD_AP_PERIPH)
      11             : #include "SITL_Periph_State.h"
      12             : #else
      13             : 
      14             : #include <netinet/in.h>
      15             : 
      16             : class HAL_SITL;
      17             : 
      18             : class HALSITL::SITL_State : public SITL_State_Common {
      19             :     friend class HALSITL::Scheduler;
      20             :     friend class HALSITL::Util;
      21             :     friend class HALSITL::GPIO;
      22             : public:
      23             :     void init(int argc, char * const argv[]) override;
      24             : 
      25             :     void loop_hook(void);
      26        6306 :     uint16_t base_port(void) const {
      27        6306 :         return _base_port;
      28             :     }
      29             : 
      30             :     // paths for UART devices
      31             :     const char *_serial_path[9] {
      32             :         "tcp:0:wait",
      33             :         "tcp:2",
      34             :         "tcp:3",
      35             :         "GPS1",
      36             :         "GPS2",
      37             :         "tcp:5",
      38             :         "tcp:6",
      39             :         "tcp:7",
      40             :         "tcp:8",
      41             :     };
      42             :     ObjectArray<struct AP_Param::defaults_table_struct> cmdline_param{100};
      43             : 
      44             :     /* parse a home location string */
      45             :     static bool parse_home(const char *home_str,
      46             :                            Location &loc,
      47             :                            float &yaw_degrees);
      48             : 
      49             :     /* lookup a location in locations.txt */
      50             :     static bool lookup_location(const char *home_str,
      51             :                                 Location &loc,
      52             :                                 float &yaw_degrees);
      53             :     
      54        3076 :     uint8_t get_instance() const { return _instance; }
      55             : 
      56             : private:
      57             :     void _parse_command_line(int argc, char * const argv[]);
      58             :     void _usage(void);
      59             : 
      60             :     /*
      61             :       look up the model/vehicle in @ROMFS/vehicleinfo.json and set
      62             :       defaults_path to the matching comma-separated @ROMFS/... list
      63             :       so AP_Param can load them. No-op if defaults_path is already
      64             :       set or if no embedded JSON is found.
      65             :     */
      66             :     void resolve_defaults_from_romfs(const char *model_str, const char *vehicle_str);
      67             : 
      68             :     /* dump @ROMFS/vehicleinfo.json to stdout (for --list-models) */
      69             :     static void list_models_and_exit(void);
      70             : 
      71             :     void _sitl_setup();
      72             :     void _setup_timer(void);
      73             :     void _setup_adc(void);
      74             : 
      75             :     void set_height_agl(void);
      76             :     void _update_rangefinder();
      77             :     void _set_signal_handlers(void) const;
      78             : 
      79             :     void _update_airspeed(float eas);
      80             :     void _fdm_input_local(void);
      81             :     void _output_to_flightgear(void);
      82             :     void _simulator_servos(struct sitl_input &input);
      83             :     void _fdm_input_step(void);
      84             : 
      85             :     void wait_clock(uint64_t wait_time_usec);
      86             : 
      87             :     // internal state
      88             :     uint8_t _instance;
      89             :     uint16_t _base_port;
      90             :     pid_t _parent_pid;
      91             :     uint32_t _update_count;
      92             : 
      93             :     Scheduler *_scheduler;
      94             : 
      95             :     uint16_t _rcin_port;
      96             :     const char *_rcin_path;
      97             :     uint16_t _fg_view_port;
      98             :     uint16_t _irlock_port;
      99             : 
     100             :     bool _use_fg_view;
     101             : 
     102             :     bool _use_dds_sim_time = false;
     103             :     
     104             :     const char *_fg_address;
     105             : 
     106             :     // simulation model name as given on the command line, for
     107             :     // model-specific flightgear view output
     108             :     const char *_model_str = nullptr;
     109             : 
     110             :     // delay buffer variables
     111             :     static const uint8_t wind_buffer_length = 50;
     112             : 
     113             :     // airspeed sensor delay buffer variables
     114             :     struct readings_wind {
     115             :         uint32_t time;
     116             :         float data;
     117             :     };
     118             :     uint8_t store_index_wind;
     119             :     uint32_t last_store_time_wind;
     120             :     VectorN<readings_wind,wind_buffer_length> buffer_wind;
     121             :     uint32_t time_delta_wind;
     122             :     uint32_t delayed_time_wind;
     123             : 
     124             :     // returns a voltage between 0V to 5V which should appear as the
     125             :     // voltage from the sensor
     126             :     float _sonar_pin_voltage() const;
     127             : 
     128             :     // multicast state
     129             :     int servo_in_fd = -1;
     130             :     struct sockaddr_in mc_dest;
     131             : 
     132             :     // simulated-peripheral lockstep: do not advance the simulation
     133             :     // past a multicast state packet until every registered peripheral
     134             :     // has acknowledged consuming it
     135             :     bool _periph_lockstep;
     136             :     static const uint8_t MAX_MCAST_PERIPHS = 16;
     137             :     // a peripheral silent for this long in wall-clock time is presumed
     138             :     // dead; the harness SIGTERMs them with no notice, so there is no
     139             :     // goodbye to wait for.  Must sit above real starvation (tens of
     140             :     // milliseconds) and below the test framework's own timeouts
     141             :     static const uint64_t PERIPH_EVICT_TIMEOUT_MS = 1000;
     142             :     // how long an evicted peer is held out of the registry, so that
     143             :     // acks still queued from before its death cannot re-register it
     144             :     static const uint64_t PERIPH_REJOIN_COOLDOWN_MS = 500;
     145             :     // how long to block for acks before re-examining the registry for
     146             :     // peripherals which have passed the eviction timeout
     147             :     static const uint8_t PERIPH_ACK_POLL_MS = 5;
     148             :     struct mcast_periph {
     149             :         struct sockaddr_in addr;
     150             :         uint64_t last_ack_us;    // state timestamp the periph has acked
     151             :         uint64_t last_heard_ms;  // wall-clock time we last heard from it
     152             :     } mcast_periphs[MAX_MCAST_PERIPHS];
     153             :     uint8_t num_mcast_periphs;
     154             :     // recently-evicted peers; their stale queued acks must not
     155             :     // re-register them (a dead peripheral's last acks can still be in
     156             :     // the socket queue when it is evicted)
     157             :     struct evicted_periph {
     158             :         struct sockaddr_in addr;
     159             :         uint64_t evicted_ms;
     160             :     } evicted_periphs[MAX_MCAST_PERIPHS];
     161             :     uint8_t num_evicted_periphs;
     162             :     bool _warned_ack_size;  // we have complained about a bad-sized reply
     163             :     void handle_periph_ack(const struct sitl_mcast_ack &ack, ssize_t len, const struct sockaddr_in &src);
     164             :     void wait_periph_acks(uint64_t timestamp_us);
     165             :     static uint64_t wall_millis(void);
     166             : 
     167             :     // send out SITL state as UDP multicast
     168             :     void multicast_state_open(void);
     169             :     void multicast_state_send(void);
     170             :     void multicast_servo_update(struct sitl_input &input);
     171             : 
     172             :     uint16_t mc_servo[SITL_NUM_CHANNELS];
     173             :     void check_servo_input(void);
     174             : };
     175             : 
     176             : #endif // defined(HAL_BUILD_AP_PERIPH)
     177             : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL