From 650ef59be852656eb763a9554a93ceb2da6271d7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:15:48 +1100 Subject: [PATCH] AP_HAL: create HAL::SIMState object to hold simulation state --- libraries/AP_HAL/AP_HAL_Namespace.h | 2 + libraries/AP_HAL/HAL.h | 10 + libraries/AP_HAL/SIMState.cpp | 354 ++++++++++++++++++++++++++++ libraries/AP_HAL/SIMState.h | 234 ++++++++++++++++++ 4 files changed, 600 insertions(+) create mode 100644 libraries/AP_HAL/SIMState.cpp create mode 100644 libraries/AP_HAL/SIMState.h diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index e22e631cf5..83946aaf31 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -65,6 +65,8 @@ namespace AP_HAL { SPIDevice_Type = -1, }; + class SIMState; + // Must be implemented by the concrete HALs. const HAL& get_HAL(); } diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index b6e5146c35..7b0887fbe7 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -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: 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: 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 }; diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp new file mode 100644 index 0000000000..960ccfaaf9 --- /dev/null +++ b/libraries/AP_HAL/SIMState.cpp @@ -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 + +extern const AP_HAL::HAL& hal; + +using namespace AP_HAL; + +#include + +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; iupdate(); + } + } + + // 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; ifetteconewireesc_sim.enabled()) { + _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input); + for (uint8_t i=0; istate.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 diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h new file mode 100644 index 0000000000..af572105e0 --- /dev/null +++ b/libraries/AP_HAL/SIMState.h @@ -0,0 +1,234 @@ +#pragma once + +#include + +#if AP_SIM_ENABLED + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +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 buffer_wind; + VectorN 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