Line data    Source code

       1             : #include "SIMState.h"
       2             : 
       3             : #if AP_SIM_ENABLED
       4             : 
       5             : /*
       6             :  *  This is a very-much-cut-down AP_HAL_SITL object.  We should make
       7             :  *  PA_HAL_SITL use this object - by moving a lot more code from over
       8             :  *  there into here.
       9             :  */
      10             : 
      11             : #include <SITL/SIM_Multicopter.h>
      12             : #include <SITL/SIM_Helicopter.h>
      13             : #include <SITL/SIM_SingleCopter.h>
      14             : #include <SITL/SIM_Plane.h>
      15             : #include <SITL/SIM_Glider.h>
      16             : #include <SITL/SIM_QuadPlane.h>
      17             : #include <SITL/SIM_Rover.h>
      18             : #include <SITL/SIM_BalanceBot.h>
      19             : #include <SITL/SIM_Sailboat.h>
      20             : #include <SITL/SIM_MotorBoat.h>
      21             : #include <SITL/SIM_Tracker.h>
      22             : #include <SITL/SIM_Submarine.h>
      23             : #include <SITL/SIM_Blimp.h>
      24             : #include <SITL/SIM_NoVehicle.h>
      25             : #include <SITL/SIM_JSON.h>
      26             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      27             : 
      28             : #include <AP_Baro/AP_Baro.h>
      29             : 
      30             : #include <AP_BoardConfig/AP_BoardConfig.h>
      31             : 
      32             : extern const AP_HAL::HAL& hal;
      33             : 
      34             : using namespace AP_HAL;
      35             : 
      36             : #include <AP_Terrain/AP_Terrain.h>
      37             : 
      38             : #ifndef AP_SIM_FRAME_CLASS
      39             : #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
      40             : #define AP_SIM_FRAME_CLASS MultiCopter
      41             : #elif APM_BUILD_TYPE(APM_BUILD_Heli)
      42             : #define AP_SIM_FRAME_CLASS Helicopter
      43             : #elif APM_BUILD_TYPE(APM_BUILD_AntennaTracker)
      44             : #define AP_SIM_FRAME_CLASS Tracker
      45             : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
      46             : #define AP_SIM_FRAME_CLASS Plane
      47             : #elif APM_BUILD_TYPE(APM_BUILD_Rover)
      48             : #define AP_SIM_FRAME_CLASS SimRover
      49             : #elif APM_BUILD_TYPE(APM_BUILD_Blimp)
      50             : #define AP_SIM_FRAME_CLASS Blimp
      51             : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
      52             : #define AP_SIM_FRAME_CLASS Submarine
      53             : #else
      54             : #define AP_SIM_FRAME_CLASS NoVehicle
      55             : #endif
      56             : #endif
      57             : 
      58             : #ifndef AP_SIM_FRAME_STRING
      59             : #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
      60             : #define AP_SIM_FRAME_STRING "+"
      61             : #elif APM_BUILD_TYPE(APM_BUILD_Heli)
      62             : #define AP_SIM_FRAME_STRING "heli"
      63             : #elif APM_BUILD_TYPE(APM_BUILD_AntennaTracker)
      64             : #define AP_SIM_FRAME_STRING "tracker"
      65             : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
      66             : #define AP_SIM_FRAME_STRING "plane"
      67             : #elif APM_BUILD_TYPE(APM_BUILD_Rover)
      68             : #define AP_SIM_FRAME_STRING "rover"
      69             : #elif APM_BUILD_TYPE(APM_BUILD_Blimp)
      70             : #define AP_SIM_FRAME_STRING "blimp"
      71             : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
      72             : #define AP_SIM_FRAME_STRING "sub"
      73             : #else
      74             : #define AP_SIM_FRAME_STRING ""
      75             : #endif
      76             : #endif
      77             : 
      78             : 
      79             : #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
      80             : void SIMState::update()
      81             : {
      82             :     static bool init_done;
      83             :     if (!init_done) {
      84             :         AP::sitl()->init();
      85             :         init_done = true;
      86             :         sitl_model = SITL::AP_SIM_FRAME_CLASS::create(AP_SIM_FRAME_STRING);
      87             :     }
      88             : 
      89             :     _fdm_input_step();
      90             : }
      91             : 
      92             : /*
      93             :   setup for SITL handling
      94             :  */
      95             : void SIMState::_sitl_setup(const char *home_str)
      96             : {
      97             :     _home_str = home_str;
      98             : 
      99             :     printf("Starting SITL input\n");
     100             : }
     101             : 
     102             : 
     103             : /*
     104             :   step the FDM by one time step
     105             :  */
     106             : void SIMState::_fdm_input_step(void)
     107             : {
     108             :     fdm_input_local();
     109             : }
     110             : 
     111             : /*
     112             :   get FDM input from a local model
     113             :  */
     114             : void SIMState::fdm_input_local(void)
     115             : {
     116             :     struct sitl_input input;
     117             : 
     118             :     // construct servos structure for FDM
     119             :     _simulator_servos(input);
     120             : 
     121             :     // read servo inputs from ride along flight controllers
     122             :     // ride_along.receive(input);
     123             : 
     124             :     // update the model
     125             :     sitl_model->update_home();
     126             :     sitl_model->update_model(input);
     127             : 
     128             :     // get FDM output from the model
     129             :     if (_sitl == nullptr) {
     130             :         _sitl = AP::sitl();
     131             :     }
     132             :     if (_sitl) {
     133             :         sitl_model->fill_fdm(_sitl->state);
     134             :     }
     135             : 
     136             :     // output JSON state to ride along flight controllers
     137             :     // ride_along.send(_sitl->state,sitl_model->get_position_relhome());
     138             : 
     139             : #if AP_SIM_SOLOGIMBAL_ENABLED
     140             :     if (gimbal != nullptr) {
     141             :         gimbal->update();
     142             :     }
     143             : #endif  // AP_SIM_SOLOGIMBAL_ENABLED
     144             : #if AP_SIM_ADSB_ENABLED
     145             :     if (adsb != nullptr) {
     146             :         adsb->update();
     147             :     }
     148             : #endif  // AP_SIM_ADSB_ENABLED
     149             : #if AP_SIM_VICON_ENABLED
     150             :     if (vicon != nullptr) {
     151             :         Quaternion attitude;
     152             :         sitl_model->get_attitude(attitude);
     153             :         vicon->update(sitl_model->get_location(),
     154             :                       sitl_model->get_position_relhome(),
     155             :                       sitl_model->get_velocity_ef(),
     156             :                       attitude);
     157             :     }
     158             : #endif  // AP_SIM_VICON_ENABLED
     159             :     if (benewake_tf02 != nullptr) {
     160             :         benewake_tf02->update(sitl_model->rangefinder_range());
     161             :     }
     162             :     if (benewake_tf03 != nullptr) {
     163             :         benewake_tf03->update(sitl_model->rangefinder_range());
     164             :     }
     165             :     if (benewake_tfmini != nullptr) {
     166             :         benewake_tfmini->update(sitl_model->rangefinder_range());
     167             :     }
     168             :     if (nooploop != nullptr) {
     169             :         nooploop->update(sitl_model->rangefinder_range());
     170             :     }
     171             :     if (teraranger_serial != nullptr) {
     172             :         teraranger_serial->update(sitl_model->rangefinder_range());
     173             :     }
     174             :     if (lightwareserial != nullptr) {
     175             :         lightwareserial->update(sitl_model->rangefinder_range());
     176             :     }
     177             :     if (lightwareserial_binary != nullptr) {
     178             :         lightwareserial_binary->update(sitl_model->rangefinder_range());
     179             :     }
     180             :     if (lanbao != nullptr) {
     181             :         lanbao->update(sitl_model->rangefinder_range());
     182             :     }
     183             :     if (blping != nullptr) {
     184             :         blping->update(sitl_model->rangefinder_range());
     185             :     }
     186             :     if (leddarone != nullptr) {
     187             :         leddarone->update(sitl_model->rangefinder_range());
     188             :     }
     189             :     if (rds02uf != nullptr) {
     190             :         rds02uf->update(sitl_model->rangefinder_range());
     191             :     }
     192             :     if (USD1_v0 != nullptr) {
     193             :         USD1_v0->update(sitl_model->rangefinder_range());
     194             :     }
     195             :     if (USD1_v1 != nullptr) {
     196             :         USD1_v1->update(sitl_model->rangefinder_range());
     197             :     }
     198             :     if (maxsonarseriallv != nullptr) {
     199             :         maxsonarseriallv->update(sitl_model->rangefinder_range());
     200             :     }
     201             :     if (wasp != nullptr) {
     202             :         wasp->update(sitl_model->rangefinder_range());
     203             :     }
     204             :     if (nmea != nullptr) {
     205             :         nmea->update(sitl_model->rangefinder_range());
     206             :     }
     207             :     if (rf_mavlink != nullptr) {
     208             :         rf_mavlink->update(sitl_model->rangefinder_range());
     209             :     }
     210             :     if (gyus42v2 != nullptr) {
     211             :         gyus42v2->update(sitl_model->rangefinder_range());
     212             :     }
     213             :     if (efi_ms != nullptr) {
     214             :         efi_ms->update(*sitl_model);
     215             :     }
     216             : 
     217             :     if (frsky_d != nullptr) {
     218             :         frsky_d->update();
     219             :     }
     220             : 
     221             : #if AP_SIM_CRSF_ENABLED
     222             :     if (crsf != nullptr) {
     223             :         crsf->update();
     224             :     }
     225             : #endif
     226             : 
     227             : #if AP_SIM_PS_RPLIDARA2_ENABLED
     228             :     if (rplidara2 != nullptr) {
     229             :         rplidara2->update(sitl_model->get_location());
     230             :     }
     231             : #endif
     232             : 
     233             : #if AP_SIM_PS_TERARANGERTOWER_ENABLED
     234             :     if (terarangertower != nullptr) {
     235             :         terarangertower->update(sitl_model->get_location());
     236             :     }
     237             : #endif
     238             : 
     239             : #if AP_SIM_PS_LIGHTWARE_SF45B_ENABLED
     240             :     if (sf45b != nullptr) {
     241             :         sf45b->update(sitl_model->get_location());
     242             :     }
     243             : #endif
     244             : 
     245             :     if (vectornav != nullptr) {
     246             :         vectornav->update();
     247             :     }
     248             : 
     249             :     if (microstrain5 != nullptr) {
     250             :         microstrain5->update();
     251             :     }
     252             :     if (inertiallabs != nullptr) {
     253             :         inertiallabs->update();
     254             :     }
     255             : #if AP_SIM_AERON_ENABLED
     256             :     if (aeron != nullptr) {
     257             :         aeron->update();
     258             :     }
     259             : #endif  // AP_SIM_AERON_ENABLED
     260             : 
     261             : #if AP_SIM_AIS_ENABLED
     262             :     if (ais != nullptr) {
     263             :         ais->update();
     264             :     }
     265             : #endif
     266             :     for (uint8_t i=0; i<ARRAY_SIZE(gps); i++) {
     267             :         if (gps[i] != nullptr) {
     268             :             gps[i]->update();
     269             :         }
     270             :     }
     271             : 
     272             :     // update simulation time
     273             :     if (_sitl) {
     274             :         hal.scheduler->stop_clock(_sitl->state.timestamp_us);
     275             :     } else {
     276             :         hal.scheduler->stop_clock(AP_HAL::micros64()+100);
     277             :     }
     278             : 
     279             :     set_height_agl();
     280             : 
     281             :     _update_count++;
     282             : }
     283             : #endif  // CONFIG_HAL_BOARD != HAL_BOARD_SITL
     284             : 
     285             : #if AP_SIM_WIND_SIMULATION_ENABLED
     286   187275482 : void SIMState::update_simulated_wind(struct sitl_input &input)
     287             : {
     288   187275482 :     float wind_speed = 0;
     289   187275482 :     float wind_direction = 0;
     290   187275482 :     float wind_dir_z = 0;
     291             : 
     292   187275482 :     uint32_t now = AP_HAL::micros();
     293             : 
     294   187275482 :     if (_sitl == nullptr)  {
     295        2094 :         _sitl = AP::sitl();
     296             :     }
     297             :     // give 5 seconds to calibrate airspeed sensor at 0 wind speed
     298   187275482 :     if (wind_start_delay_micros == 0) {
     299        2094 :         wind_start_delay_micros = now;
     300   187273388 :     } else if ((now - wind_start_delay_micros) > 5000000) {
     301             :         // The EKF does not like step inputs so this LPF keeps it happy.
     302   175244979 :         uint32_t dt_us = now - last_wind_update_us;
     303   175244979 :         if (dt_us > 1000) {
     304    87622904 :             last_wind_update_us = now;
     305             :             // slew wind based on the configured time constant
     306    87622904 :             const float dt = dt_us * 1.0e-6;
     307    87622904 :             const float tc = MAX(_sitl->wind_change_tc, 0.1);
     308    87622904 :             const float alpha = calc_lowpass_alpha_dt(dt, 1.0/tc);
     309    87622904 :             _sitl->wind_speed_active     += (_sitl->wind_speed - _sitl->wind_speed_active) * alpha;
     310    87622904 :             _sitl->wind_direction_active += (wrap_180(_sitl->wind_direction - _sitl->wind_direction_active)) * alpha;
     311    87622904 :             _sitl->wind_dir_z_active     += (_sitl->wind_dir_z - _sitl->wind_dir_z_active) * alpha;
     312    87622904 :             _sitl->wind_direction_active = wrap_180(_sitl->wind_direction_active);
     313             :         }
     314   175244979 :         wind_speed =     _sitl->wind_speed_active;
     315   175244979 :         wind_direction = _sitl->wind_direction_active;
     316   175244979 :         wind_dir_z =     _sitl->wind_dir_z_active;
     317             : 
     318             :         // pass wind into simulators using different wind types via param SIM_WIND_T*.
     319   175244979 :         const float altitude = _sitl->state.height_agl;
     320   175244979 :         switch (_sitl->wind_type) {
     321   173436382 :         case SITL::SIM::WIND_TYPE_SQRT:
     322   173436382 :             if (altitude < _sitl->wind_type_alt) {
     323   142001243 :                 wind_speed *= sqrtf(MAX(altitude / _sitl->wind_type_alt, 0));
     324             :             }
     325   173436382 :             break;
     326             : 
     327           0 :         case SITL::SIM::WIND_TYPE_COEF:
     328           0 :             wind_speed += (altitude - _sitl->wind_type_alt) * _sitl->wind_type_coef;
     329           0 :             break;
     330             : 
     331     1808597 :         case SITL::SIM::WIND_TYPE_NO_LIMIT:
     332             :         default:
     333     1808597 :             break;
     334             :         }
     335             : 
     336             :         // never allow negative wind velocity
     337   175244979 :         wind_speed = MAX(wind_speed, 0);
     338             :     }
     339             : 
     340   187275482 :     input.wind.speed = wind_speed;
     341   187275482 :     input.wind.direction = wind_direction;
     342   187275482 :     input.wind.turbulence = _sitl->wind_turbulance;
     343   187275482 :     input.wind.dir_z = wind_dir_z;
     344   187275482 : }
     345             : #endif  // AP_SIM_WIND_SIMULATION_ENABLED
     346             : 
     347             : #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
     348             : /*
     349             :   create sitl_input structure for sending to FDM
     350             :  */
     351             : void SIMState::_simulator_servos(struct sitl_input &input)
     352             : {
     353             :     if (_sitl == nullptr) {
     354             :         return;
     355             :     }
     356             : 
     357             :     update_simulated_wind(input);
     358             : 
     359             :     for (uint8_t i=0; i<ARRAY_SIZE(pwm_output); i++) {
     360             :         if (pwm_output[i] == 0xFFFF) {
     361             :             input.servos[i] = 0;
     362             :         } else {
     363             :             input.servos[i] = pwm_output[i];
     364             :         }
     365             :     }
     366             : 
     367             :     {
     368             :         // FETtec ESC simulation support.  Input signals of 1000-2000
     369             :         // are positive thrust, 0 to 1000 are negative thrust.  Deeper
     370             :         // changes required to support negative thrust - potentially
     371             :         // adding a field to input.
     372             :             if (_sitl->fetteconewireesc_sim.enabled()) {
     373             :                 _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input);
     374             :                 for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
     375             :                     if (input.servos[i] != 0 && input.servos[i] < 1000) {
     376             :                         AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
     377             :                     }
     378             :                 }
     379             :             }
     380             :     }
     381             : 
     382             : #if AP_SIM_VOLZ_ENABLED
     383             :     // update simulation input based on data received via "serial" to
     384             :     // Volz servos:
     385             :     if (_sitl->volz_sim.enabled()) {
     386             :         _sitl->volz_sim.update_sitl_input_pwm(input);
     387             :         for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
     388             :             if (input.servos[i] != 0 && input.servos[i] < 1000) {
     389             :                 AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
     390             :             }
     391             :         }
     392             :     }
     393             : #endif
     394             : 
     395             :     float voltage = 0;
     396             :     _current = 0;
     397             : 
     398             :     {
     399             :         if (_sitl->state.battery_voltage <= 0) {
     400             :         } else {
     401             :             // FDM provides voltage and current
     402             :             voltage = _sitl->state.battery_voltage;
     403             :             _current = _sitl->state.battery_current;
     404             :         }
     405             :     }
     406             : 
     407             :     // assume 3DR power brick
     408             :     voltage_pin_value = ((voltage / 10.1f) / 5.0f) * 1024;
     409             :     current_pin_value = ((_current / 17.0f) / 5.0f) * 1024;
     410             :     // fake battery2 as just a 25% gain on the first one
     411             :     voltage2_pin_value = ((voltage * 0.25f / 10.1f) / 5.0f) * 1024;
     412             :     current2_pin_value = ((_current * 0.25f / 17.0f) / 5.0f) * 1024;
     413             : }
     414             : 
     415             : /*
     416             :   set height above the ground in meters
     417             :  */
     418             : void SIMState::set_height_agl(void)
     419             : {
     420             :     static float home_alt = -1;
     421             : 
     422             :     if (!_sitl) {
     423             :         // in example program
     424             :         return;
     425             :     }
     426             : 
     427             :     if (is_equal(home_alt, -1.0f) && _sitl->state.altitude > 0) {
     428             :         // remember home altitude as first non-zero altitude
     429             :         home_alt = _sitl->state.altitude;
     430             :     }
     431             : 
     432             : #if AP_TERRAIN_AVAILABLE
     433             :     if (_sitl != nullptr &&
     434             :         _sitl->terrain_enable) {
     435             :         // get height above terrain from AP_Terrain. This assumes
     436             :         // AP_Terrain is working
     437             :         float terrain_height_amsl;
     438             :         Location location;
     439             :         location.lat = _sitl->state.latitude*1.0e7;
     440             :         location.lng = _sitl->state.longitude*1.0e7;
     441             : 
     442             :         AP_Terrain *_terrain = AP_Terrain::get_singleton();
     443             :         if (_terrain != nullptr &&
     444             :             _terrain->height_amsl(location, terrain_height_amsl)) {
     445             :             _sitl->state.height_agl = _sitl->state.altitude - terrain_height_amsl;
     446             :             return;
     447             :         }
     448             :     }
     449             : #endif
     450             : 
     451             :     if (_sitl != nullptr) {
     452             :         // fall back to flat earth model
     453             :         _sitl->state.height_agl = _sitl->state.altitude - home_alt;
     454             :     }
     455             : }
     456             : 
     457             : #endif  // CONFIG_HAL_BOARD != HAL_BOARD_SITL
     458             : 
     459             : #endif  // AP_SIM_ENABLED