AP_HAL: create HAL::SIMState object to hold simulation state

3 years ago · 650ef59be8
4 changed files with 600 additions and 0 deletions
--- a/libraries/AP_HAL/AP_HAL_Namespace.h
+++ b/libraries/AP_HAL/AP_HAL_Namespace.h
@ -65,6 +65,8 @@ namespace AP_HAL {
				@@ -65,6 +65,8 @@ namespace AP_HAL {
        SPIDevice_Type              = -1,
    };

+    class SIMState;
+
    // Must be implemented by the concrete HALs.
    const HAL& get_HAL();
 }
--- a/libraries/AP_HAL/HAL.h
+++ b/libraries/AP_HAL/HAL.h
@ -43,6 +43,9 @@ public:
				@@ -43,6 +43,9 @@ public:
        AP_HAL::Util*       _util,
        AP_HAL::OpticalFlow*_opticalflow,
        AP_HAL::Flash*      _flash,
+#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
+        class AP_HAL::SIMState*   _simstate,
+#endif
        AP_HAL::DSP*        _dsp,
 #if HAL_NUM_CAN_IFACES > 0
        AP_HAL::CANIface* _can_ifaces[HAL_NUM_CAN_IFACES])
@ -73,6 +76,9 @@ public:
				@@ -73,6 +76,9 @@ public:
        util(_util),
        opticalflow(_opticalflow),
        flash(_flash),
+#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
+        simstate(_simstate),
+#endif
        dsp(_dsp)
    {
 #if HAL_NUM_CAN_IFACES > 0
@ -144,4 +150,8 @@ public:
				@@ -144,4 +150,8 @@ public:
    UARTDriver* serial(uint8_t sernum) const;

    static constexpr uint8_t num_serial = 10;
+
+#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
+    AP_HAL::SIMState *simstate;
+#endif
 };
--- a/libraries/AP_HAL/SIMState.cpp
+++ b/libraries/AP_HAL/SIMState.cpp
@ -0,0 +1,354 @@
				@@ -0,0 +1,354 @@
+#include "SIMState.h"
+
+#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
+
+/*
+ *  This is a very-much-cut-down AP_HAL_SITL object.  We should make
+ *  PA_HAL_SITL use this object - by moving a lot more code from over
+ *  there into here.
+ */
+
+#include <SITL/SIM_Multicopter.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace AP_HAL;
+
+#include <AP_Terrain/AP_Terrain.h>
+
+void SIMState::update()
+{
+    static bool init_done;
+    if (!init_done) {
+        init_done = true;
+        sitl_model = SITL::MultiCopter::create("+");
+    }
+
+    _fdm_input_step();
+}
+
+/*
+  setup for SITL handling
+ */
+void SIMState::_sitl_setup(const char *home_str)
+{
+    _home_str = home_str;
+
+    printf("Starting SITL input\n");
+
+    // find the barometer object if it exists
+    _barometer = AP_Baro::get_singleton();
+}
+
+
+/*
+  step the FDM by one time step
+ */
+void SIMState::_fdm_input_step(void)
+{
+    fdm_input_local();
+}
+
+/*
+  get FDM input from a local model
+ */
+void SIMState::fdm_input_local(void)
+{
+    struct sitl_input input;
+
+    // construct servos structure for FDM
+    _simulator_servos(input);
+
+    // read servo inputs from ride along flight controllers
+    // ride_along.receive(input);
+
+    // update the model
+    sitl_model->update_model(input);
+
+    // get FDM output from the model
+    if (_sitl == nullptr) {
+        _sitl = AP::sitl();
+    }
+    if (_sitl) {
+        sitl_model->fill_fdm(_sitl->state);
+
+        if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) {
+            for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) {
+                pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000;
+            }
+        }
+    }
+
+    // output JSON state to ride along flight controllers
+    // ride_along.send(_sitl->state,sitl_model->get_position_relhome());
+
+#if HAL_SIM_GIMBAL_ENABLED
+    if (gimbal != nullptr) {
+        gimbal->update();
+    }
+#endif
+#if HAL_SIM_ADSB_ENABLED
+    if (adsb != nullptr) {
+        adsb->update();
+    }
+#endif
+    if (vicon != nullptr) {
+        Quaternion attitude;
+        sitl_model->get_attitude(attitude);
+        vicon->update(sitl_model->get_location(),
+                      sitl_model->get_position_relhome(),
+                      sitl_model->get_velocity_ef(),
+                      attitude);
+    }
+    if (benewake_tf02 != nullptr) {
+        benewake_tf02->update(sitl_model->rangefinder_range());
+    }
+    if (benewake_tf03 != nullptr) {
+        benewake_tf03->update(sitl_model->rangefinder_range());
+    }
+    if (benewake_tfmini != nullptr) {
+        benewake_tfmini->update(sitl_model->rangefinder_range());
+    }
+    if (lightwareserial != nullptr) {
+        lightwareserial->update(sitl_model->rangefinder_range());
+    }
+    if (lightwareserial_binary != nullptr) {
+        lightwareserial_binary->update(sitl_model->rangefinder_range());
+    }
+    if (lanbao != nullptr) {
+        lanbao->update(sitl_model->rangefinder_range());
+    }
+    if (blping != nullptr) {
+        blping->update(sitl_model->rangefinder_range());
+    }
+    if (leddarone != nullptr) {
+        leddarone->update(sitl_model->rangefinder_range());
+    }
+    if (USD1_v0 != nullptr) {
+        USD1_v0->update(sitl_model->rangefinder_range());
+    }
+    if (USD1_v1 != nullptr) {
+        USD1_v1->update(sitl_model->rangefinder_range());
+    }
+    if (maxsonarseriallv != nullptr) {
+        maxsonarseriallv->update(sitl_model->rangefinder_range());
+    }
+    if (wasp != nullptr) {
+        wasp->update(sitl_model->rangefinder_range());
+    }
+    if (nmea != nullptr) {
+        nmea->update(sitl_model->rangefinder_range());
+    }
+    if (rf_mavlink != nullptr) {
+        rf_mavlink->update(sitl_model->rangefinder_range());
+    }
+    if (gyus42v2 != nullptr) {
+        gyus42v2->update(sitl_model->rangefinder_range());
+    }
+    if (efi_ms != nullptr) {
+        efi_ms->update();
+    }
+
+    if (frsky_d != nullptr) {
+        frsky_d->update();
+    }
+    // if (frsky_sport != nullptr) {
+    //     frsky_sport->update();
+    // }
+    // if (frsky_sportpassthrough != nullptr) {
+    //     frsky_sportpassthrough->update();
+    // }
+
+#if AP_SIM_CRSF_ENABLED
+    if (crsf != nullptr) {
+        crsf->update();
+    }
+#endif
+
+#if HAL_SIM_PS_RPLIDARA2_ENABLED
+    if (rplidara2 != nullptr) {
+        rplidara2->update(sitl_model->get_location());
+    }
+#endif
+
+#if HAL_SIM_PS_TERARANGERTOWER_ENABLED
+    if (terarangertower != nullptr) {
+        terarangertower->update(sitl_model->get_location());
+    }
+#endif
+
+#if HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED
+    if (sf45b != nullptr) {
+        sf45b->update(sitl_model->get_location());
+    }
+#endif
+
+    if (vectornav != nullptr) {
+        vectornav->update();
+    }
+
+    if (lord != nullptr) {
+        lord->update();
+    }
+
+#if HAL_SIM_AIS_ENABLED
+    if (ais != nullptr) {
+        ais->update();
+    }
+#endif
+    for (uint8_t i=0; i<ARRAY_SIZE(gps); i++) {
+        if (gps[i] != nullptr) {
+            gps[i]->update();
+        }
+    }
+
+    // update simulation time
+    if (_sitl) {
+        hal.scheduler->stop_clock(_sitl->state.timestamp_us);
+    } else {
+        hal.scheduler->stop_clock(AP_HAL::micros64()+100);
+    }
+
+    set_height_agl();
+
+    _synthetic_clock_mode = true;
+    _update_count++;
+}
+
+/*
+  create sitl_input structure for sending to FDM
+ */
+void SIMState::_simulator_servos(struct sitl_input &input)
+{
+    // output at chosen framerate
+    uint32_t now = AP_HAL::micros();
+    // last_update_usec = now;
+
+    float altitude = _barometer?_barometer->get_altitude():0;
+    float wind_speed = 0;
+    float wind_direction = 0;
+    float wind_dir_z = 0;
+
+    // give 5 seconds to calibrate airspeed sensor at 0 wind speed
+    if (wind_start_delay_micros == 0) {
+        wind_start_delay_micros = now;
+    } else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) {
+        // The EKF does not like step inputs so this LPF keeps it happy.
+        wind_speed =     _sitl->wind_speed_active     = (0.95f*_sitl->wind_speed_active)     + (0.05f*_sitl->wind_speed);
+        wind_direction = _sitl->wind_direction_active = (0.95f*_sitl->wind_direction_active) + (0.05f*_sitl->wind_direction);
+        wind_dir_z =     _sitl->wind_dir_z_active     = (0.95f*_sitl->wind_dir_z_active)     + (0.05f*_sitl->wind_dir_z);
+        
+        // pass wind into simulators using different wind types via param SIM_WIND_T*.
+        switch (_sitl->wind_type) {
+        case SITL::SIM::WIND_TYPE_SQRT:
+            if (altitude < _sitl->wind_type_alt) {
+                wind_speed *= sqrtf(MAX(altitude / _sitl->wind_type_alt, 0));
+            }
+            break;
+
+        case SITL::SIM::WIND_TYPE_COEF:
+            wind_speed += (altitude - _sitl->wind_type_alt) * _sitl->wind_type_coef;
+            break;
+
+        case SITL::SIM::WIND_TYPE_NO_LIMIT:
+        default:
+            break;
+        }
+
+        // never allow negative wind velocity
+        wind_speed = MAX(wind_speed, 0);
+    }
+
+    input.wind.speed = wind_speed;
+    input.wind.direction = wind_direction;
+    input.wind.turbulence = _sitl?_sitl->wind_turbulance:0;
+    input.wind.dir_z = wind_dir_z;
+
+    for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
+        if (pwm_output[i] == 0xFFFF) {
+            input.servos[i] = 0;
+        } else {
+            input.servos[i] = pwm_output[i];
+        }
+    }
+
+    if (_sitl != nullptr) {
+        // FETtec ESC simulation support.  Input signals of 1000-2000
+        // are positive thrust, 0 to 1000 are negative thrust.  Deeper
+        // changes required to support negative thrust - potentially
+        // adding a field to input.
+        if (_sitl != nullptr) {
+            if (_sitl->fetteconewireesc_sim.enabled()) {
+                _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input);
+                for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
+                    if (input.servos[i] != 0 && input.servos[i] < 1000) {
+                        AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
+                    }
+                }
+            }
+        }
+    }
+
+    float voltage = 0;
+    _current = 0;
+    
+    if (_sitl != nullptr) {
+        if (_sitl->state.battery_voltage <= 0) {
+        } else {
+            // FDM provides voltage and current
+            voltage = _sitl->state.battery_voltage;
+            _current = _sitl->state.battery_current;
+        }
+    }
+
+    // assume 3DR power brick
+    voltage_pin_value = ((voltage / 10.1f) / 5.0f) * 1024;
+    current_pin_value = ((_current / 17.0f) / 5.0f) * 1024;
+    // fake battery2 as just a 25% gain on the first one
+    voltage2_pin_value = ((voltage * 0.25f / 10.1f) / 5.0f) * 1024;
+    current2_pin_value = ((_current * 0.25f / 17.0f) / 5.0f) * 1024;
+}
+
+/*
+  set height above the ground in meters
+ */
+void SIMState::set_height_agl(void)
+{
+    static float home_alt = -1;
+
+    if (!_sitl) {
+        // in example program
+        return;
+    }
+
+    if (is_equal(home_alt, -1.0f) && _sitl->state.altitude > 0) {
+        // remember home altitude as first non-zero altitude
+        home_alt = _sitl->state.altitude;
+    }
+
+#if AP_TERRAIN_AVAILABLE
+    if (_sitl != nullptr &&
+        _sitl->terrain_enable) {
+        // get height above terrain from AP_Terrain. This assumes
+        // AP_Terrain is working
+        float terrain_height_amsl;
+        struct Location location;
+        location.lat = _sitl->state.latitude*1.0e7;
+        location.lng = _sitl->state.longitude*1.0e7;
+
+        AP_Terrain *_terrain = AP_Terrain::get_singleton();
+        if (_terrain != nullptr &&
+            _terrain->height_amsl(location, terrain_height_amsl)) {
+            _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl;
+            return;
+        }
+    }
+#endif
+
+    if (_sitl != nullptr) {
+        // fall back to flat earth model
+        _sitl->height_agl = _sitl->state.altitude - home_alt;
+    }
+}
+
+#endif  // AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
--- a/libraries/AP_HAL/SIMState.h
+++ b/libraries/AP_HAL/SIMState.h
@ -0,0 +1,234 @@
				@@ -0,0 +1,234 @@
+#pragma once
+
+#include <SITL/SITL.h>
+
+#if AP_SIM_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+
+#include <AP_Baro/AP_Baro.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Terrain/AP_Terrain.h>
+#include <SITL/SITL_Input.h>
+#include <SITL/SIM_Gimbal.h>
+#include <SITL/SIM_ADSB.h>
+#include <SITL/SIM_Vicon.h>
+#include <SITL/SIM_RF_Benewake_TF02.h>
+#include <SITL/SIM_RF_Benewake_TF03.h>
+#include <SITL/SIM_RF_Benewake_TFmini.h>
+#include <SITL/SIM_RF_LightWareSerial.h>
+#include <SITL/SIM_RF_LightWareSerialBinary.h>
+#include <SITL/SIM_RF_Lanbao.h>
+#include <SITL/SIM_RF_BLping.h>
+#include <SITL/SIM_RF_LeddarOne.h>
+#include <SITL/SIM_RF_USD1_v0.h>
+#include <SITL/SIM_RF_USD1_v1.h>
+#include <SITL/SIM_RF_MaxsonarSerialLV.h>
+#include <SITL/SIM_RF_Wasp.h>
+#include <SITL/SIM_RF_NMEA.h>
+#include <SITL/SIM_RF_MAVLink.h>
+#include <SITL/SIM_RF_GYUS42v2.h>
+#include <SITL/SIM_VectorNav.h>
+#include <SITL/SIM_LORD.h>
+#include <SITL/SIM_AIS.h>
+#include <SITL/SIM_GPS.h>
+
+#include <SITL/SIM_Frsky_D.h>
+#include <SITL/SIM_CRSF.h>
+#include <SITL/SIM_PS_RPLidarA2.h>
+#include <SITL/SIM_PS_TeraRangerTower.h>
+#include <SITL/SIM_PS_LightWare_SF45B.h>
+
+#include <SITL/SIM_RichenPower.h>
+#include <SITL/SIM_FETtecOneWireESC.h>
+#include <AP_HAL/utility/Socket.h>
+
+#include <AP_HAL/AP_HAL_Namespace.h>
+
+class AP_HAL::SIMState {
+public:
+
+    // simulated airspeed, sonar and battery monitor
+    uint16_t sonar_pin_value;    // pin 0
+    uint16_t airspeed_pin_value; // pin 1
+    uint16_t airspeed_2_pin_value; // pin 2
+    uint16_t voltage_pin_value;  // pin 13
+    uint16_t current_pin_value;  // pin 12
+    uint16_t voltage2_pin_value;  // pin 15
+    uint16_t current2_pin_value;  // pin 14
+
+    void update();
+
+    void set_gps0(SITL::GPS *_gps) { gps[0] = _gps; }
+
+    uint16_t pwm_output[16];  // was SITL_NUM_CHANNELS
+
+private:
+    void _set_param_default(const char *parm);
+    void _sitl_setup(const char *home_str);
+    void _setup_timer(void);
+    void _setup_adc(void);
+
+    void set_height_agl(void);
+    void _set_signal_handlers(void) const;
+
+    void _update_airspeed(float airspeed);
+    void _simulator_servos(struct sitl_input &input);
+    void _fdm_input_step(void);
+    void fdm_input_local(void);
+
+    void wait_clock(uint64_t wait_time_usec);
+
+    uint16_t pwm_input[16];  // was SITL_RC_INPUT_CHANNELS
+
+    // internal state
+    // enum vehicle_type _vehicle;
+    uint8_t _instance;
+    uint16_t _base_port;
+    pid_t _parent_pid;
+    uint32_t _update_count;
+
+    AP_Baro *_barometer;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    SocketAPM _sitl_rc_in{true};
+#endif
+    SITL::SIM *_sitl;
+    uint16_t _rcin_port;
+    uint16_t _fg_view_port;
+    uint16_t _irlock_port;
+    float _current;
+
+    bool _synthetic_clock_mode;
+
+    bool _use_rtscts;
+    bool _use_fg_view;
+    
+    const char *_fg_address;
+
+    // delay buffer variables
+    static const uint8_t wind_buffer_length = 50;
+
+    // airspeed sensor delay buffer variables
+    struct readings_wind {
+        uint32_t time;
+        float data;
+    };
+    uint8_t store_index_wind;
+    uint32_t last_store_time_wind;
+    VectorN<readings_wind,wind_buffer_length> buffer_wind;
+    VectorN<readings_wind,wind_buffer_length> buffer_wind_2;
+    uint32_t time_delta_wind;
+    uint32_t delayed_time_wind;
+    uint32_t wind_start_delay_micros;
+
+    // internal SITL model
+    SITL::Aircraft *sitl_model;
+
+#if HAL_SIM_GIMBAL_ENABLED
+    // simulated gimbal
+    bool enable_gimbal;
+    SITL::Gimbal *gimbal;
+#endif
+
+#if HAL_SIM_ADSB_ENABLED
+    // simulated ADSb
+    SITL::ADSB *adsb;
+#endif
+
+    // simulated vicon system:
+    SITL::Vicon *vicon;
+
+    // simulated Benewake tf02 rangefinder:
+    SITL::RF_Benewake_TF02 *benewake_tf02;
+    // simulated Benewake tf03 rangefinder:
+    SITL::RF_Benewake_TF03 *benewake_tf03;
+    // simulated Benewake tfmini rangefinder:
+    SITL::RF_Benewake_TFmini *benewake_tfmini;
+
+    // simulated LightWareSerial rangefinder - legacy protocol::
+    SITL::RF_LightWareSerial *lightwareserial;
+    // simulated LightWareSerial rangefinder - binary protocol:
+    SITL::RF_LightWareSerialBinary *lightwareserial_binary;
+    // simulated Lanbao rangefinder:
+    SITL::RF_Lanbao *lanbao;
+    // simulated BLping rangefinder:
+    SITL::RF_BLping *blping;
+    // simulated LeddarOne rangefinder:
+    SITL::RF_LeddarOne *leddarone;
+    // simulated USD1 v0 rangefinder:
+    SITL::RF_USD1_v0 *USD1_v0;
+    // simulated USD1 v1 rangefinder:
+    SITL::RF_USD1_v1 *USD1_v1;
+    // simulated MaxsonarSerialLV rangefinder:
+    SITL::RF_MaxsonarSerialLV *maxsonarseriallv;
+    // simulated Wasp rangefinder:
+    SITL::RF_Wasp *wasp;
+    // simulated NMEA rangefinder:
+    SITL::RF_NMEA *nmea;
+    // simulated MAVLink rangefinder:
+    SITL::RF_MAVLink *rf_mavlink;
+    // simulated GYUS42v2 rangefinder:
+    SITL::RF_GYUS42v2 *gyus42v2;
+
+    // simulated Frsky devices
+    SITL::Frsky_D *frsky_d;
+    // SITL::Frsky_SPort *frsky_sport;
+    // SITL::Frsky_SPortPassthrough *frsky_sportpassthrough;
+
+#if HAL_SIM_PS_RPLIDARA2_ENABLED
+    // simulated RPLidarA2:
+    SITL::PS_RPLidarA2 *rplidara2;
+#endif
+
+    // simulated FETtec OneWire ESCs:
+    SITL::FETtecOneWireESC *fetteconewireesc;
+
+#if HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED
+    // simulated SF45B proximity sensor:
+    SITL::PS_LightWare_SF45B *sf45b;
+#endif
+
+#if HAL_SIM_PS_TERARANGERTOWER_ENABLED
+    SITL::PS_TeraRangerTower *terarangertower;
+#endif
+
+#if AP_SIM_CRSF_ENABLED
+    // simulated CRSF devices
+    SITL::CRSF *crsf;
+#endif
+
+    // simulated VectorNav system:
+    SITL::VectorNav *vectornav;
+
+    // simulated LORD Microstrain system
+    SITL::LORD *lord;
+
+#if HAL_SIM_JSON_MASTER_ENABLED
+    // Ride along instances via JSON SITL backend
+    SITL::JSON_Master ride_along;
+#endif
+
+#if HAL_SIM_AIS_ENABLED
+    // simulated AIS stream
+    SITL::AIS *ais;
+#endif
+
+    // simulated EFI MegaSquirt device:
+    SITL::EFI_MegaSquirt *efi_ms;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    // output socket for flightgear viewing
+    SocketAPM fg_socket{true};
+#endif
+
+    const char *defaults_path = HAL_PARAM_DEFAULTS_PATH;
+
+    const char *_home_str;
+
+    // simulated GPS devices
+    SITL::GPS *gps[2];  // constrained by # of parameter sets
+};
+
+#endif // AP_SIM_ENABLED