mirror of https://github.com/ArduPilot/ardupilot
412 lines
11 KiB
C++
412 lines
11 KiB
C++
#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>
|
|
#include <SITL/SIM_Helicopter.h>
|
|
#include <SITL/SIM_SingleCopter.h>
|
|
#include <SITL/SIM_Plane.h>
|
|
#include <SITL/SIM_QuadPlane.h>
|
|
#include <SITL/SIM_Rover.h>
|
|
#include <SITL/SIM_BalanceBot.h>
|
|
#include <SITL/SIM_Sailboat.h>
|
|
#include <SITL/SIM_MotorBoat.h>
|
|
#include <SITL/SIM_Tracker.h>
|
|
#include <SITL/SIM_Submarine.h>
|
|
#include <SITL/SIM_Blimp.h>
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
|
|
#include <AP_Baro/AP_Baro.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
using namespace AP_HAL;
|
|
|
|
#include <AP_Terrain/AP_Terrain.h>
|
|
|
|
#ifndef AP_SIM_FRAME_CLASS
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
|
|
#define AP_SIM_FRAME_CLASS MultiCopter
|
|
#elif APM_BUILD_TYPE(APM_BUILD_Heli)
|
|
#define AP_SIM_FRAME_CLASS Helicopter
|
|
#elif APM_BUILD_TYPE(APM_BUILD_AntennaTracker)
|
|
#define AP_SIM_FRAME_CLASS Tracker
|
|
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
|
#define AP_SIM_FRAME_CLASS Plane
|
|
#elif APM_BUILD_TYPE(APM_BUILD_Rover)
|
|
#define AP_SIM_FRAME_CLASS SimRover
|
|
#elif APM_BUILD_TYPE(APM_BUILD_Blimp)
|
|
#define AP_SIM_FRAME_CLASS Blimp
|
|
#elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
|
|
#define AP_SIM_FRAME_CLASS Submarine
|
|
#endif
|
|
#endif
|
|
|
|
#ifndef AP_SIM_FRAME_STRING
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
|
|
#define AP_SIM_FRAME_STRING "+"
|
|
#elif APM_BUILD_TYPE(APM_BUILD_Heli)
|
|
#define AP_SIM_FRAME_STRING "heli"
|
|
#elif APM_BUILD_TYPE(APM_BUILD_AntennaTracker)
|
|
#define AP_SIM_FRAME_STRING "tracker"
|
|
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
|
#define AP_SIM_FRAME_STRING "plane"
|
|
#elif APM_BUILD_TYPE(APM_BUILD_Rover)
|
|
#define AP_SIM_FRAME_STRING "rover"
|
|
#elif APM_BUILD_TYPE(APM_BUILD_Blimp)
|
|
#define AP_SIM_FRAME_STRING "blimp"
|
|
#elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
|
|
#define AP_SIM_FRAME_STRING "sub"
|
|
#endif
|
|
#endif
|
|
|
|
|
|
void SIMState::update()
|
|
{
|
|
static bool init_done;
|
|
if (!init_done) {
|
|
init_done = true;
|
|
sitl_model = SITL::AP_SIM_FRAME_CLASS::create(AP_SIM_FRAME_STRING);
|
|
}
|
|
|
|
_fdm_input_step();
|
|
}
|
|
|
|
/*
|
|
setup for SITL handling
|
|
*/
|
|
void SIMState::_sitl_setup(const char *home_str)
|
|
{
|
|
_home_str = home_str;
|
|
|
|
printf("Starting SITL input\n");
|
|
}
|
|
|
|
|
|
/*
|
|
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_home();
|
|
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 (nooploop != nullptr) {
|
|
nooploop->update(sitl_model->rangefinder_range());
|
|
}
|
|
if (teraranger_serial != nullptr) {
|
|
teraranger_serial->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 (rds02uf != nullptr) {
|
|
rds02uf->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 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 (microstrain5 != nullptr) {
|
|
microstrain5->update();
|
|
}
|
|
if (inertiallabs != nullptr) {
|
|
inertiallabs->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();
|
|
|
|
// find the barometer object if it exists
|
|
const auto *_barometer = AP_Baro::get_singleton();
|
|
|
|
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;
|
|
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->state.height_agl = _sitl->state.altitude - terrain_height_amsl;
|
|
return;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
if (_sitl != nullptr) {
|
|
// fall back to flat earth model
|
|
_sitl->state.height_agl = _sitl->state.altitude - home_alt;
|
|
}
|
|
}
|
|
|
|
#endif // AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
|