2015-10-22 11:04:23 -03:00
|
|
|
#pragma once
|
2012-06-29 02:06:28 -03:00
|
|
|
|
2019-11-11 00:38:43 -04:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
|
2018-05-23 02:09:57 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
2015-10-22 10:58:33 -03:00
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
2019-01-01 23:14:00 -04:00
|
|
|
#include <AP_Common/Location.h>
|
2012-06-29 02:06:28 -03:00
|
|
|
|
2019-10-07 04:30:22 -03:00
|
|
|
#include "SIM_Buzzer.h"
|
2018-07-31 09:33:23 -03:00
|
|
|
#include "SIM_Gripper_EPM.h"
|
2019-10-08 21:00:53 -03:00
|
|
|
#include "SIM_Gripper_Servo.h"
|
2019-01-08 21:39:44 -04:00
|
|
|
#include "SIM_Parachute.h"
|
2018-08-03 07:52:20 -03:00
|
|
|
#include "SIM_Precland.h"
|
2019-10-08 21:00:53 -03:00
|
|
|
#include "SIM_Sprayer.h"
|
|
|
|
#include "SIM_ToneAlarm.h"
|
2019-11-11 00:38:43 -04:00
|
|
|
#include "SIM_EFI_MegaSquirt.h"
|
2018-07-31 09:33:23 -03:00
|
|
|
|
2015-10-22 10:04:42 -03:00
|
|
|
namespace SITL {
|
|
|
|
|
2018-12-04 00:06:53 -04:00
|
|
|
struct vector3f_array {
|
|
|
|
uint16_t length;
|
|
|
|
Vector3f *data;
|
|
|
|
};
|
|
|
|
|
|
|
|
struct float_array {
|
|
|
|
uint16_t length;
|
|
|
|
float *data;
|
|
|
|
};
|
|
|
|
|
|
|
|
|
2015-11-22 22:25:00 -04:00
|
|
|
struct sitl_fdm {
|
|
|
|
// this is the structure passed between FDM models and the main SITL code
|
2015-05-02 07:29:16 -03:00
|
|
|
uint64_t timestamp_us;
|
2017-05-02 06:35:57 -03:00
|
|
|
Location home;
|
2015-05-04 22:49:54 -03:00
|
|
|
double latitude, longitude; // degrees
|
|
|
|
double altitude; // MSL
|
|
|
|
double heading; // degrees
|
|
|
|
double speedN, speedE, speedD; // m/s
|
|
|
|
double xAccel, yAccel, zAccel; // m/s/s in body frame
|
2015-05-24 19:42:51 -03:00
|
|
|
double rollRate, pitchRate, yawRate; // degrees/s/s in body frame
|
2015-05-04 22:49:54 -03:00
|
|
|
double rollDeg, pitchDeg, yawDeg; // euler angles, degrees
|
2017-04-15 08:20:28 -03:00
|
|
|
Quaternion quaternion;
|
2015-05-04 22:49:54 -03:00
|
|
|
double airspeed; // m/s
|
2015-11-22 22:31:09 -04:00
|
|
|
double battery_voltage; // Volts
|
|
|
|
double battery_current; // Amps
|
|
|
|
double rpm1; // main prop RPM
|
|
|
|
double rpm2; // secondary RPM
|
2016-05-03 23:49:42 -03:00
|
|
|
uint8_t rcin_chan_count;
|
|
|
|
float rcin[8]; // RC input 0..1
|
2017-01-09 05:16:13 -04:00
|
|
|
double range; // rangefinder value
|
2016-06-17 00:46:12 -03:00
|
|
|
Vector3f bodyMagField; // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss.
|
2016-10-16 18:58:59 -03:00
|
|
|
Vector3f angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes
|
2018-12-04 00:06:53 -04:00
|
|
|
|
|
|
|
struct {
|
|
|
|
// data from simulated laser scanner, if available
|
|
|
|
struct vector3f_array points;
|
|
|
|
struct float_array ranges;
|
|
|
|
} scanner;
|
2012-06-29 02:06:28 -03:00
|
|
|
};
|
|
|
|
|
2015-06-29 19:54:46 -03:00
|
|
|
// number of rc output channels
|
2016-07-25 02:45:39 -03:00
|
|
|
#define SITL_NUM_CHANNELS 16
|
2015-06-29 19:54:46 -03:00
|
|
|
|
2015-10-22 10:15:34 -03:00
|
|
|
class SITL {
|
2012-06-29 02:06:28 -03:00
|
|
|
public:
|
2015-05-04 22:49:54 -03:00
|
|
|
|
2012-12-12 17:48:38 -04:00
|
|
|
SITL() {
|
2015-04-20 19:29:27 -03:00
|
|
|
// set a default compass offset
|
|
|
|
mag_ofs.set(Vector3f(5, 13, -18));
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
2017-04-12 03:28:04 -03:00
|
|
|
AP_Param::setup_object_defaults(this, var_info2);
|
2019-02-10 01:01:19 -04:00
|
|
|
if (_singleton != nullptr) {
|
2018-05-02 08:30:59 -03:00
|
|
|
AP_HAL::panic("Too many SITL instances");
|
|
|
|
}
|
2019-02-10 01:01:19 -04:00
|
|
|
_singleton = this;
|
2012-12-12 17:48:38 -04:00
|
|
|
}
|
|
|
|
|
2018-05-02 08:30:59 -03:00
|
|
|
/* Do not allow copies */
|
|
|
|
SITL(const SITL &other) = delete;
|
|
|
|
SITL &operator=(const SITL&) = delete;
|
|
|
|
|
2019-02-10 01:01:19 -04:00
|
|
|
static SITL *_singleton;
|
|
|
|
static SITL *get_singleton() { return _singleton; }
|
2018-05-02 08:30:59 -03:00
|
|
|
|
2018-07-31 07:24:10 -03:00
|
|
|
enum SITL_RCFail {
|
|
|
|
SITL_RCFail_None = 0,
|
|
|
|
SITL_RCFail_NoPulses = 1,
|
|
|
|
SITL_RCFail_Throttle950 = 2,
|
|
|
|
};
|
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
enum GPSType {
|
2013-02-16 05:59:48 -04:00
|
|
|
GPS_TYPE_NONE = 0,
|
|
|
|
GPS_TYPE_UBLOX = 1,
|
|
|
|
GPS_TYPE_MTK = 2,
|
|
|
|
GPS_TYPE_MTK16 = 3,
|
2013-08-13 23:11:46 -03:00
|
|
|
GPS_TYPE_MTK19 = 4,
|
2014-03-31 23:12:15 -03:00
|
|
|
GPS_TYPE_NMEA = 5,
|
2014-04-03 19:48:40 -03:00
|
|
|
GPS_TYPE_SBP = 6,
|
2016-05-16 10:09:06 -03:00
|
|
|
GPS_TYPE_FILE = 7,
|
|
|
|
GPS_TYPE_NOVA = 8,
|
2017-04-04 21:00:44 -03:00
|
|
|
GPS_TYPE_SBP2 = 9,
|
2013-02-16 05:15:57 -04:00
|
|
|
};
|
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
struct sitl_fdm state;
|
|
|
|
|
2015-11-16 00:10:29 -04:00
|
|
|
// loop update rate in Hz
|
|
|
|
uint16_t update_rate_hz;
|
|
|
|
|
|
|
|
// true when motors are active
|
|
|
|
bool motors_on;
|
|
|
|
|
2016-11-19 02:21:39 -04:00
|
|
|
// height above ground
|
|
|
|
float height_agl;
|
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
2017-04-12 03:28:04 -03:00
|
|
|
static const struct AP_Param::GroupInfo var_info2[];
|
2015-05-04 22:49:54 -03:00
|
|
|
|
|
|
|
// noise levels for simulated sensors
|
|
|
|
AP_Float baro_noise; // in metres
|
|
|
|
AP_Float baro_drift; // in metres per second
|
|
|
|
AP_Float baro_glitch; // glitch in meters
|
|
|
|
AP_Float gyro_noise; // in degrees/second
|
2016-01-19 00:28:53 -04:00
|
|
|
AP_Vector3f gyro_scale; // percentage
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Float accel_noise; // in m/s/s
|
2015-07-12 03:26:43 -03:00
|
|
|
AP_Float accel2_noise; // in m/s/s
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Vector3f accel_bias; // in m/s/s
|
2016-12-18 00:53:40 -04:00
|
|
|
AP_Vector3f accel2_bias; // in m/s/s
|
2016-05-10 16:42:08 -03:00
|
|
|
AP_Float arspd_noise; // in m/s
|
2018-03-09 04:08:04 -04:00
|
|
|
AP_Float arspd_fail; // 1st pitot tube failure
|
|
|
|
AP_Float arspd2_fail; // 2nd pitot tube failure
|
|
|
|
AP_Float arspd_fail_pressure; // 1st pitot tube failure pressure
|
|
|
|
AP_Float arspd_fail_pitot_pressure; // 1st pitot tube failure pressure
|
|
|
|
AP_Float arspd2_fail_pressure; // 2nd pitot tube failure pressure
|
|
|
|
AP_Float arspd2_fail_pitot_pressure; // 2nd pitot tube failure pressure
|
2017-01-30 15:04:10 -04:00
|
|
|
AP_Float gps_noise; // amplitude of the gps altitude error
|
2017-07-14 02:00:46 -03:00
|
|
|
AP_Int16 gps_lock_time; // delay in seconds before GPS gets lock
|
2017-09-17 21:24:45 -03:00
|
|
|
AP_Int16 gps_alt_offset; // gps alt error
|
2018-03-25 23:54:05 -03:00
|
|
|
AP_Int8 vicon_observation_history_length; // frame delay for vicon messages
|
2015-07-20 21:56:18 -03:00
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Float mag_noise; // in mag units (earth field is 818)
|
|
|
|
AP_Float mag_error; // in degrees
|
|
|
|
AP_Vector3f mag_mot; // in mag units per amp
|
|
|
|
AP_Vector3f mag_ofs; // in mag units
|
2018-07-16 05:19:38 -03:00
|
|
|
AP_Vector3f mag_diag; // diagonal corrections
|
|
|
|
AP_Vector3f mag_offdiag; // off-diagonal corrections
|
|
|
|
AP_Int8 mag_orient; // external compass orientation
|
2016-10-25 05:54:56 -03:00
|
|
|
AP_Float servo_speed; // servo speed in seconds
|
2012-06-29 02:06:28 -03:00
|
|
|
|
2013-11-26 01:58:30 -04:00
|
|
|
AP_Float sonar_glitch;// probablility between 0-1 that any given sonar sample will read as max distance
|
|
|
|
AP_Float sonar_noise; // in metres
|
2014-08-10 09:36:38 -03:00
|
|
|
AP_Float sonar_scale; // meters per volt
|
2013-11-26 01:42:43 -04:00
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Float drift_speed; // degrees/second/minute
|
|
|
|
AP_Float drift_time; // period in minutes
|
2012-08-17 01:21:43 -03:00
|
|
|
AP_Float engine_mul; // engine multiplier
|
2017-03-20 17:32:54 -03:00
|
|
|
AP_Int8 engine_fail; // engine servo to fail (0-7)
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Int8 gps_disable; // disable simulated GPS
|
|
|
|
AP_Int8 gps2_enable; // enable 2nd simulated GPS
|
|
|
|
AP_Int8 gps_delay; // delay in samples
|
2013-02-16 05:15:57 -04:00
|
|
|
AP_Int8 gps_type; // see enum GPSType
|
2017-04-14 04:57:37 -03:00
|
|
|
AP_Int8 gps2_type; // see enum GPSType
|
2013-02-16 07:00:16 -04:00
|
|
|
AP_Float gps_byteloss;// byte loss as a percent
|
2013-05-06 21:38:36 -03:00
|
|
|
AP_Int8 gps_numsats; // number of visible satellites
|
2017-03-03 22:39:04 -04:00
|
|
|
AP_Vector3f gps_glitch; // glitch offsets in lat, lon and altitude
|
|
|
|
AP_Vector3f gps2_glitch; // glitch offsets in lat, lon and altitude for 2nd GPS
|
2013-10-02 05:44:20 -03:00
|
|
|
AP_Int8 gps_hertz; // GPS update rate in Hz
|
|
|
|
AP_Float batt_voltage; // battery voltage base
|
2013-11-06 21:49:08 -04:00
|
|
|
AP_Float accel_fail; // accelerometer failure value
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Int8 rc_fail; // fail RC input
|
2018-07-23 23:46:11 -03:00
|
|
|
AP_Int8 rc_chancount; // channel count
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Int8 baro_disable; // disable simulated barometer
|
2014-04-21 02:24:45 -03:00
|
|
|
AP_Int8 float_exception; // enable floating point exception checks
|
2015-05-04 22:49:54 -03:00
|
|
|
AP_Int8 flow_enable; // enable simulated optflow
|
|
|
|
AP_Int16 flow_rate; // optflow data rate (Hz)
|
|
|
|
AP_Int8 flow_delay; // optflow data delay
|
|
|
|
AP_Int8 terrain_enable; // enable using terrain for height
|
2018-04-18 00:34:17 -03:00
|
|
|
AP_Int16 pin_mask; // for GPIO emulation
|
2016-09-18 18:45:24 -03:00
|
|
|
AP_Float speedup; // simulation speedup
|
2017-04-30 21:09:34 -03:00
|
|
|
AP_Int8 odom_enable; // enable visual odomotry data
|
2019-01-24 20:26:19 -04:00
|
|
|
AP_Int8 telem_baudlimit_enable; // enable baudrate limiting on links
|
2019-02-14 05:23:25 -04:00
|
|
|
AP_Float flow_noise; // optical flow measurement noise (rad/sec)
|
2019-01-05 06:45:02 -04:00
|
|
|
AP_Int8 baro_count; // number of simulated baros to create
|
2017-05-28 01:08:14 -03:00
|
|
|
AP_Int8 gps_hdg_enabled; // enable the output of a NMEA heading HDT sentence
|
2019-09-17 05:10:06 -03:00
|
|
|
AP_Int32 loop_delay; // extra delay to add to every loop
|
2018-07-31 09:33:23 -03:00
|
|
|
|
2019-11-11 00:38:43 -04:00
|
|
|
// EFI type
|
|
|
|
enum EFIType {
|
|
|
|
EFI_TYPE_NONE = 0,
|
|
|
|
EFI_TYPE_MS = 1,
|
|
|
|
};
|
|
|
|
|
|
|
|
AP_Int8 efi_type;
|
|
|
|
|
2012-08-24 08:22:20 -03:00
|
|
|
// wind control
|
2018-04-30 12:25:19 -03:00
|
|
|
enum WindType {
|
|
|
|
WIND_TYPE_SQRT = 0,
|
|
|
|
WIND_TYPE_NO_LIMIT = 1,
|
|
|
|
WIND_TYPE_COEF = 2,
|
|
|
|
};
|
|
|
|
|
2016-07-15 22:36:59 -03:00
|
|
|
float wind_speed_active;
|
|
|
|
float wind_direction_active;
|
2018-01-30 12:38:36 -04:00
|
|
|
float wind_dir_z_active;
|
2012-08-24 08:22:20 -03:00
|
|
|
AP_Float wind_speed;
|
|
|
|
AP_Float wind_direction;
|
|
|
|
AP_Float wind_turbulance;
|
2015-02-20 17:32:49 -04:00
|
|
|
AP_Float gps_drift_alt;
|
2018-01-30 12:38:36 -04:00
|
|
|
AP_Float wind_dir_z;
|
2018-04-30 12:25:19 -03:00
|
|
|
AP_Int8 wind_type; // enum WindLimitType
|
|
|
|
AP_Float wind_type_alt;
|
|
|
|
AP_Float wind_type_coef;
|
2015-04-13 03:22:57 -03:00
|
|
|
|
|
|
|
AP_Int16 baro_delay; // barometer data delay in ms
|
|
|
|
AP_Int16 mag_delay; // magnetometer data delay in ms
|
|
|
|
AP_Int16 wind_delay; // windspeed data delay in ms
|
|
|
|
|
2016-06-15 00:47:19 -03:00
|
|
|
// ADSB related run-time options
|
|
|
|
AP_Int16 adsb_plane_count;
|
|
|
|
AP_Float adsb_radius_m;
|
|
|
|
AP_Float adsb_altitude_m;
|
2016-08-16 18:16:03 -03:00
|
|
|
AP_Int8 adsb_tx;
|
2016-06-15 00:47:19 -03:00
|
|
|
|
2016-06-17 00:46:12 -03:00
|
|
|
// Earth magnetic field anomaly
|
|
|
|
AP_Vector3f mag_anomaly_ned; // NED anomaly vector at ground level (mGauss)
|
|
|
|
AP_Float mag_anomaly_hgt; // height above ground where anomally strength has decayed to 1/8 of the ground level value (m)
|
|
|
|
|
2016-10-14 20:42:45 -03:00
|
|
|
// Body frame sensor position offsets
|
|
|
|
AP_Vector3f imu_pos_offset; // XYZ position of the IMU accelerometer relative to the body frame origin (m)
|
|
|
|
AP_Vector3f gps_pos_offset; // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
|
|
|
|
AP_Vector3f rngfnd_pos_offset; // XYZ position of the range finder zero range datum relative to the body frame origin (m)
|
|
|
|
AP_Vector3f optflow_pos_offset; // XYZ position of the optical flow sensor focal point relative to the body frame origin (m)
|
|
|
|
|
2017-04-12 03:28:04 -03:00
|
|
|
// temperature control
|
|
|
|
AP_Float temp_start;
|
|
|
|
AP_Float temp_flight;
|
|
|
|
AP_Float temp_tconst;
|
2017-04-12 19:48:50 -03:00
|
|
|
AP_Float temp_baro_factor;
|
2017-04-12 03:28:04 -03:00
|
|
|
|
2017-09-20 00:13:49 -03:00
|
|
|
// differential pressure sensor tube order
|
|
|
|
AP_Int8 arspd_signflip;
|
|
|
|
|
2018-11-08 23:06:30 -04:00
|
|
|
// weight on wheels pin
|
|
|
|
AP_Int8 wow_pin;
|
2019-01-07 05:32:17 -04:00
|
|
|
|
|
|
|
// vibration frequencies in Hz on each axis
|
|
|
|
AP_Vector3f vibe_freq;
|
2017-11-27 06:45:23 -04:00
|
|
|
|
2019-04-17 23:41:00 -03:00
|
|
|
// gyro and accel fail masks
|
|
|
|
AP_Int8 gyro_fail_mask;
|
|
|
|
AP_Int8 accel_fail_mask;
|
|
|
|
|
2017-11-27 06:45:23 -04:00
|
|
|
struct {
|
|
|
|
AP_Float x;
|
|
|
|
AP_Float y;
|
|
|
|
AP_Float z;
|
|
|
|
AP_Int32 t;
|
|
|
|
|
|
|
|
uint32_t start_ms;
|
|
|
|
} shove;
|
|
|
|
|
2019-04-10 22:55:04 -03:00
|
|
|
struct {
|
|
|
|
AP_Float x;
|
|
|
|
AP_Float y;
|
|
|
|
AP_Float z;
|
|
|
|
AP_Int32 t;
|
|
|
|
|
|
|
|
uint32_t start_ms;
|
|
|
|
} twist;
|
|
|
|
|
|
|
|
AP_Int8 gnd_behav;
|
|
|
|
|
2019-05-19 12:03:16 -03:00
|
|
|
struct {
|
|
|
|
AP_Int8 enable; // 0: disabled, 1: roll and pitch, 2: roll, pitch and heave
|
|
|
|
AP_Float length; // m
|
|
|
|
AP_Float amp; // m
|
|
|
|
AP_Float direction; // deg (direction wave is coming from)
|
|
|
|
AP_Float speed; // m/s
|
|
|
|
} wave;
|
|
|
|
|
2019-05-27 20:52:11 -03:00
|
|
|
struct {
|
|
|
|
AP_Float direction; // deg (direction tide is coming from)
|
|
|
|
AP_Float speed; // m/s
|
|
|
|
} tide;
|
|
|
|
|
2019-08-15 03:53:35 -03:00
|
|
|
// original simulated position
|
|
|
|
struct {
|
|
|
|
AP_Float lat;
|
|
|
|
AP_Float lng;
|
|
|
|
AP_Float alt; // metres
|
|
|
|
AP_Float hdg; // 0 to 360
|
|
|
|
} opos;
|
|
|
|
|
2016-11-17 14:05:04 -04:00
|
|
|
uint16_t irlock_port;
|
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
void simstate_send(mavlink_channel_t chan);
|
2012-06-29 02:06:28 -03:00
|
|
|
|
2019-01-17 18:15:50 -04:00
|
|
|
void Log_Write_SIMSTATE();
|
2014-01-03 01:01:18 -04:00
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
// convert a set of roll rates from earth frame to body frame
|
|
|
|
static void convert_body_frame(double rollDeg, double pitchDeg,
|
|
|
|
double rollRate, double pitchRate, double yawRate,
|
|
|
|
double *p, double *q, double *r);
|
2015-05-02 07:29:16 -03:00
|
|
|
|
2015-05-04 22:49:54 -03:00
|
|
|
// convert a set of roll rates from body frame to earth frame
|
2015-05-02 07:29:16 -03:00
|
|
|
static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
|
2018-07-31 09:33:23 -03:00
|
|
|
|
|
|
|
Sprayer sprayer_sim;
|
|
|
|
|
|
|
|
Gripper_Servo gripper_sim;
|
|
|
|
Gripper_EPM gripper_epm_sim;
|
2019-01-08 21:39:44 -04:00
|
|
|
|
|
|
|
Parachute parachute_sim;
|
2019-10-07 04:30:22 -03:00
|
|
|
Buzzer buzzer_sim;
|
2019-10-08 21:00:53 -03:00
|
|
|
ToneAlarm tonealarm_sim;
|
2018-08-03 07:52:20 -03:00
|
|
|
SIM_Precland precland_sim;
|
2019-10-30 07:11:52 -03:00
|
|
|
|
|
|
|
struct {
|
|
|
|
// LED state, for serial LED emulation
|
|
|
|
struct {
|
|
|
|
uint8_t rgb[3];
|
|
|
|
} rgb[16][32];
|
|
|
|
uint8_t num_leds[16];
|
|
|
|
uint32_t send_counter;
|
|
|
|
} led;
|
2019-11-11 00:38:43 -04:00
|
|
|
|
|
|
|
EFI_MegaSquirt efi_ms;
|
2012-06-29 02:06:28 -03:00
|
|
|
};
|
|
|
|
|
2015-10-22 10:04:42 -03:00
|
|
|
} // namespace SITL
|
2018-05-02 08:30:59 -03:00
|
|
|
|
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
SITL::SITL *sitl();
|
|
|
|
};
|
2019-11-11 00:38:43 -04:00
|
|
|
|
|
|
|
#endif // CONFIG_HAL_BOARD
|