/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* SITL.cpp - software in the loop state */ #include "SITL.h" #if AP_SIM_ENABLED #include #include #include #include #include #include #ifdef SFML_JOYSTICK #ifdef HAVE_SFML_GRAPHICS_HPP #include #elif HAVE_SFML_GRAPHIC_H #include #endif #endif // SFML_JOYSTICK #include "SIM_StratoBlimp.h" #include "SIM_Glider.h" #include "SIM_FlightAxis.h" extern const AP_HAL::HAL& hal; #ifndef SIM_RATE_HZ_DEFAULT #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define SIM_RATE_HZ_DEFAULT 1200 #else #define SIM_RATE_HZ_DEFAULT 400 #endif #endif #if (CONFIG_HAL_BOARD != HAL_BOARD_SITL) // For on-hardware, set allowed relay channels to zero. // Requires user to change the param to allow hadware access. #define SIM_DEFAULT_ENABLED_RELAY_CHANNELS 0 #else // For SITL, set allowed relay channels to the full mask. #define SIM_DEFAULT_ENABLED_RELAY_CHANNELS UINT16_MAX #endif namespace SITL { SIM *SIM::_singleton = nullptr; // table of user settable parameters const AP_Param::GroupInfo SIM::var_info[] = { // @Param: DRIFT_SPEED // @DisplayName: Gyro drift speed // @Description: Gyro drift rate of change in degrees/second/minute AP_GROUPINFO("DRIFT_SPEED", 5, SIM, drift_speed, 0.05f), // @Param: DRIFT_TIME // @DisplayName: Gyro drift time // @Description: Gyro drift duration of one full drift cycle (period in minutes) AP_GROUPINFO("DRIFT_TIME", 6, SIM, drift_time, 5), // @Param: ENGINE_MUL // @DisplayName: Engine failure thrust scaler // @Description: Thrust from Motors in SIM_ENGINE_FAIL will be multiplied by this factor // @Units: ms AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 1), // @Param: WIND_SPD // @DisplayName: Simulated Wind speed // @Description: Allows you to emulate wind in sim // @Units: m/s // @User: Advanced AP_GROUPINFO("WIND_SPD", 9, SIM, wind_speed, 0), // @Param: WIND_DIR // @DisplayName: Direction simulated wind is coming from // @Description: Allows you to set wind direction (true deg) in sim // @Units: deg // @User: Advanced AP_GROUPINFO("WIND_DIR", 10, SIM, wind_direction, 180), // @Param: WIND_TURB // @DisplayName: Simulated Wind variation // @Description: Allows you to emulate random wind variations in sim // @Units: m/s // @User: Advanced AP_GROUPINFO("WIND_TURB", 11, SIM, wind_turbulance, 0), // @Param: WIND_TC // @DisplayName: Wind variation time constant // @Description: this controls the time over which wind changes take effect // @Units: s // @User: Advanced AP_GROUPINFO("WIND_TC", 12, SIM, wind_change_tc, 5), // @Group: SERVO_ // @Path: ./ServoModel.cpp AP_SUBGROUPINFO(servo, "SERVO_", 16, SIM, ServoParams), // @Param: SONAR_ROT // @DisplayName: Sonar rotation // @Description: Sonar rotation from rotations enumeration AP_GROUPINFO("SONAR_ROT", 17, SIM, sonar_rot, Rotation::ROTATION_PITCH_270), // @Param: BATT_VOLTAGE // @DisplayName: Simulated battery voltage // @Description: Simulated battery (constant) voltage // @Units: V // @User: Advanced AP_GROUPINFO("BATT_VOLTAGE", 19, SIM, batt_voltage, 12.6f), // @Param: BATT_CAP_AH // @DisplayName: Simulated battery capacity // @Description: Simulated battery capacity // @Units: Ah // @User: Advanced AP_GROUPINFO("BATT_CAP_AH", 20, SIM, batt_capacity_ah, 0), // @Param: SONAR_GLITCH // @DisplayName: Sonar glitch probablility // @Description: Probablility a sonar glitch would happen // @Range: 0 1 // @User: Advanced AP_GROUPINFO("SONAR_GLITCH", 23, SIM, sonar_glitch, 0), // @Param: SONAR_RND // @DisplayName: Sonar noise factor // @Description: Scaling factor for simulated sonar noise // @User: Advanced AP_GROUPINFO("SONAR_RND", 24, SIM, sonar_noise, 0), // @Param: RC_FAIL // @DisplayName: Simulated RC signal failure // @Description: Allows you to emulate rc failures in sim // @Values: 0:Disabled,1:No RC pusles,2:All Channels neutral except Throttle is 950us // @User: Advanced AP_GROUPINFO("RC_FAIL", 25, SIM, rc_fail, 0), // @Param: FLOAT_EXCEPT // @DisplayName: Generate floating point exceptions // @Description: If set, if a numerical error occurs SITL will die with a floating point exception. // @User: Advanced AP_GROUPINFO("FLOAT_EXCEPT", 28, SIM, float_exception, 1), // @Param: CAN_SRV_MSK // @DisplayName: Mask of CAN servos/ESCs // @Description: The set of actuators controlled externally by CAN SITL AP_Periph // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32 // @User: Advanced AP_GROUPINFO("CAN_SRV_MSK", 29, SIM, can_servo_mask, 0), #if HAL_NUM_CAN_IFACES > 0 // @Param: CAN_TYPE1 // @DisplayName: transport type for first CAN interface // @Description: transport type for first CAN interface // @Values: 0:None,1:MulticastUDP,2:SocketCAN // @User: Advanced AP_GROUPINFO("CAN_TYPE1", 30, SIM, can_transport[0], uint8_t(CANTransport::MulticastUDP)), #endif #if HAL_NUM_CAN_IFACES > 1 // @Param: CAN_TYPE2 // @DisplayName: transport type for second CAN interface // @Description: transport type for second CAN interface // @Values: 0:None,1:MulticastUDP,2:SocketCAN // @User: Advanced AP_GROUPINFO("CAN_TYPE2", 31, SIM, can_transport[1], uint8_t(CANTransport::MulticastUDP)), #endif // @Param: SONAR_SCALE // @DisplayName: Sonar conversion scale // @Description: Sonar conversion scale from distance to voltage // @Units: m/V AP_GROUPINFO("SONAR_SCALE", 32, SIM, sonar_scale, 12.1212f), // @Param: FLOW_ENABLE // @DisplayName: Opflow Enable // @Description: Enable simulated Optical Flow sensor // @Values: 0:Disable,1:Enabled AP_GROUPINFO("FLOW_ENABLE", 33, SIM, flow_enable, 0), // @Param: TERRAIN // @DisplayName: Terrain Enable // @Description: Enable using terrain for height // @Values: 0:Disable,1:Enabled AP_GROUPINFO("TERRAIN", 34, SIM, terrain_enable, 1), // @Param: FLOW_RATE // @DisplayName: Opflow Rate // @Description: Opflow Data Rate // @Units: Hz AP_GROUPINFO("FLOW_RATE", 35, SIM, flow_rate, 10), // @Param: FLOW_DELAY // @DisplayName: Opflow Delay // @Description: Opflow data delay // @Units: ms AP_GROUPINFO("FLOW_DELAY", 36, SIM, flow_delay, 0), // @Param: ADSB_COUNT // @DisplayName: Number of ADSB aircrafts // @Description: Total number of ADSB simulated aircraft AP_GROUPINFO("ADSB_COUNT", 45, SIM, adsb_plane_count, -1), // @Param: ADSB_RADIUS // @DisplayName: ADSB radius stddev of another aircraft // @Description: Simulated standard deviation of radius in ADSB of another aircraft // @Units: m AP_GROUPINFO("ADSB_RADIUS", 46, SIM, adsb_radius_m, 10000), // @Param: ADSB_ALT // @DisplayName: ADSB altitude of another aircraft // @Description: Simulated ADSB altitude of another aircraft // @Units: m AP_GROUPINFO("ADSB_ALT", 47, SIM, adsb_altitude_m, 1000), // @Param: PIN_MASK // @DisplayName: GPIO emulation // @Description: SITL GPIO emulation AP_GROUPINFO("PIN_MASK", 50, SIM, pin_mask, 0), // @Param: ADSB_TX // @DisplayName: ADSB transmit enable // @Description: ADSB transceiever enable and disable // @Values: 0:Transceiever disable, 1:Transceiever enable AP_GROUPINFO("ADSB_TX", 51, SIM, adsb_tx, 0), // @Param: SPEEDUP // @DisplayName: Sim Speedup // @Description: Runs the simulation at multiples of normal speed. Do not use if realtime physics, like RealFlight, is being used // @Range: 1 10 // @User: Advanced AP_GROUPINFO("SPEEDUP", 52, SIM, speedup, -1), // @Param: IMU_POS // @DisplayName: IMU Offsets // @Description: XYZ position of the IMU accelerometer relative to the body frame origin // @Units: m // @Vector3Parameter: 1 AP_GROUPINFO("IMU_POS", 53, SIM, imu_pos_offset, 0), AP_SUBGROUPEXTENSION("", 54, SIM, var_ins), AP_GROUPINFO("SONAR_POS", 55, SIM, rngfnd_pos_offset, 0), // @Param: FLOW_POS // @DisplayName: Opflow Pos // @Description: XYZ position of the optical flow sensor focal point relative to the body frame origin // @Units: m // @Vector3Parameter: 1 AP_GROUPINFO("FLOW_POS", 56, SIM, optflow_pos_offset, 0), // @Param: ENGINE_FAIL // @DisplayName: Engine Fail Mask // @Description: mask of motors which SIM_ENGINE_MUL will be applied to // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8 AP_GROUPINFO("ENGINE_FAIL", 58, SIM, engine_fail, 0), AP_SUBGROUPINFO(models, "", 59, SIM, SIM::ModelParm), AP_SUBGROUPEXTENSION("", 60, SIM, var_mag), #if HAL_SIM_GPS_ENABLED AP_SUBGROUPEXTENSION("", 61, SIM, var_gps), #endif AP_SUBGROUPEXTENSION("", 62, SIM, var_info3), AP_SUBGROUPEXTENSION("", 63, SIM, var_info2), AP_GROUPEND }; // second table of user settable parameters for SITL. const AP_Param::GroupInfo SIM::var_info2[] = { // @Param: TEMP_START // @DisplayName: Start temperature // @Description: Baro start temperature // @Units: degC // @User: Advanced AP_GROUPINFO("TEMP_START", 1, SIM, temp_start, 25), // @Param: TEMP_BRD_OFF // @DisplayName: Baro temperature offset // @Description: Barometer board temperature offset from atmospheric temperature // @Units: degC // @User: Advanced AP_GROUPINFO("TEMP_BRD_OFF", 2, SIM, temp_board_offset, 20), // @Param: TEMP_TCONST // @DisplayName: Warmup time constant // @Description: Barometer warmup temperature time constant // @Units: degC // @User: Advanced AP_GROUPINFO("TEMP_TCONST", 3, SIM, temp_tconst, 30), // @Param: TEMP_BFACTOR // @DisplayName: Baro temperature factor // @Description: A pressure change with temperature that closely matches what has been observed with a ICM-20789 // @User: Advanced AP_GROUPINFO("TEMP_BFACTOR", 4, SIM, temp_baro_factor, 0), // @Param: WIND_DIR_Z // @DisplayName: Simulated wind vertical direction // @Description: Allows you to set vertical wind direction (true deg) in sim. 0 means pure horizontal wind. 90 means pure updraft. // @Units: deg // @User: Advanced AP_GROUPINFO("WIND_DIR_Z", 10, SIM, wind_dir_z, 0), // @Param: WIND_T // @DisplayName: Wind Profile Type // @Description: Selects how wind varies from surface to WIND_T_ALT // @Values: 0:square law,1: none, 2:linear-see WIND_T_COEF // @User: Advanced AP_GROUPINFO("WIND_T" ,15, SIM, wind_type, SIM::WIND_TYPE_SQRT), // @Param: WIND_T_ALT // @DisplayName: Full Wind Altitude // @Description: Altitude at which wind reaches full strength, decaying from full strength as altitude lowers to ground level // @Units: m // @User: Advanced AP_GROUPINFO("WIND_T_ALT" ,16, SIM, wind_type_alt, 60), // @Param: WIND_T_COEF // @DisplayName: Linear Wind Curve Coeff // @Description: For linear wind profile,wind is reduced by (Altitude-WIND_T_ALT) x this value // @User: Advanced AP_GROUPINFO("WIND_T_COEF", 17, SIM, wind_type_coef, 0.01f), // @Param: RC_CHANCOUNT // @DisplayName: RC channel count // @Description: SITL RC channel count AP_GROUPINFO("RC_CHANCOUNT",21, SIM, rc_chancount, 16), // @Group: SPR_ // @Path: ./SIM_Sprayer.cpp AP_SUBGROUPINFO(sprayer_sim, "SPR_", 22, SIM, Sprayer), // @Group: GRPS_ // @Path: ./SIM_Gripper_Servo.cpp AP_SUBGROUPINFO(gripper_sim, "GRPS_", 23, SIM, Gripper_Servo), // @Group: GRPE_ // @Path: ./SIM_Gripper_EPM.cpp AP_SUBGROUPINFO(gripper_epm_sim, "GRPE_", 24, SIM, Gripper_EPM), // @Param: WOW_PIN // @DisplayName: Weight on Wheels Pin // @Description: SITL set this simulated pin to true if vehicle is on ground // @User: Advanced AP_GROUPINFO("WOW_PIN", 25, SIM, wow_pin, -1), // vibration frequencies on each axis AP_GROUPINFO("VIB_FREQ", 26, SIM, vibe_freq, 0), // @Path: ./SIM_Parachute.cpp AP_SUBGROUPINFO(parachute_sim, "PARA_", 27, SIM, Parachute), // @Param: BAUDLIMIT_EN // @DisplayName: Telemetry bandwidth limitting // @Description: SITL enable bandwidth limitting on telemetry ports with non-zero values AP_GROUPINFO("BAUDLIMIT_EN", 28, SIM, telem_baudlimit_enable, 0), // @Group: PLD_ // @Path: ./SIM_Precland.cpp AP_SUBGROUPINFO(precland_sim, "PLD_", 29, SIM, SIM_Precland), // @Param: SHOVE_X // @DisplayName: Acceleration of shove x // @Description: Acceleration of shove to vehicle in x axis // @Units: m/s/s AP_GROUPINFO("SHOVE_X", 30, SIM, shove.x, 0), // @Param: SHOVE_Y // @DisplayName: Acceleration of shove y // @Description: Acceleration of shove to vehicle in y axis // @Units: m/s/s AP_GROUPINFO("SHOVE_Y", 31, SIM, shove.y, 0), // @Param: SHOVE_Z // @DisplayName: Acceleration of shove z // @Description: Acceleration of shove to vehicle in z axis // @Units: m/s/s AP_GROUPINFO("SHOVE_Z", 32, SIM, shove.z, 0), // @Param: SHOVE_TIME // @DisplayName: Time length for shove // @Description: Force to the vehicle over a period of time // @Units: ms AP_GROUPINFO("SHOVE_TIME", 33, SIM, shove.t, 0), // @Param: FLOW_RND // @DisplayName: Opflow noise // @Description: Optical Flow sensor measurement noise // @Units: rad/s AP_GROUPINFO("FLOW_RND", 34, SIM, flow_noise, 0.05f), // @Param: TWIST_X // @DisplayName: Twist x // @Description: Rotational acceleration of twist x axis // @Units: rad/s/s AP_GROUPINFO("TWIST_X", 37, SIM, twist.x, 0), // @Param: TWIST_Y // @DisplayName: Twist y // @Description: Rotational acceleration of twist y axis // @Units: rad/s/s AP_GROUPINFO("TWIST_Y", 38, SIM, twist.y, 0), // @Param: TWIST_Z // @DisplayName: Twist z // @Description: Rotational acceleration of twist z axis // @Units: rad/s/s AP_GROUPINFO("TWIST_Z", 39, SIM, twist.z, 0), // @Param: TWIST_TIME // @DisplayName: Twist time // @Description: Time that twist is applied on the vehicle // @Units: ms AP_GROUPINFO("TWIST_TIME", 40, SIM, twist.t, 0), // @Param: GND_BEHAV // @DisplayName: Ground behavior // @Description: Ground behavior of aircraft (tailsitter, no movement, forward only) AP_GROUPINFO("GND_BEHAV", 41, SIM, gnd_behav, -1), // sailboat wave and tide simulation parameters // @Param: WAVE_ENABLE // @DisplayName: Wave enable // @Description: Wave enable and modes // @Values: 0:disabled, 1: roll and pitch, 2: roll and pitch and heave AP_GROUPINFO("WAVE_ENABLE", 44, SIM, wave.enable, 0.0f), // @Param: WAVE_LENGTH // @DisplayName: Wave length // @Description: Wave length in SITL // @Units: m AP_GROUPINFO("WAVE_LENGTH", 45, SIM, wave.length, 10.0f), // @Param: WAVE_AMP // @DisplayName: Wave amplitude // @Description: Wave amplitude in SITL // @Units: m AP_GROUPINFO("WAVE_AMP", 46, SIM, wave.amp, 0.5f), // @Param: WAVE_DIR // @DisplayName: Wave direction // @Description: Direction wave is coming from // @Units: deg AP_GROUPINFO("WAVE_DIR", 47, SIM, wave.direction, 0.0f), // @Param: WAVE_SPEED // @DisplayName: Wave speed // @Description: Wave speed in SITL // @Units: m/s AP_GROUPINFO("WAVE_SPEED", 48, SIM, wave.speed, 0.5f), // @Param: TIDE_DIR // @DisplayName: Tide direction // @Description: Tide direction wave is coming from // @Units: deg AP_GROUPINFO("TIDE_DIR", 49, SIM, tide.direction, 0.0f), // @Param: TIDE_SPEED // @DisplayName: Tide speed // @Description: Tide speed in simulation // @Units: m/s AP_GROUPINFO("TIDE_SPEED", 50, SIM, tide.speed, 0.0f), // the following coordinates are for CMAC, in Canberra // @Param: OPOS_LAT // @DisplayName: Original Position (Latitude) // @Description: Specifies vehicle's startup latitude // @User: Advanced AP_GROUPINFO("OPOS_LAT", 51, SIM, opos.lat, -35.363261f), // @Param: OPOS_LNG // @DisplayName: Original Position (Longitude) // @Description: Specifies vehicle's startup longitude // @User: Advanced AP_GROUPINFO("OPOS_LNG", 52, SIM, opos.lng, 149.165230f), // @Param: OPOS_ALT // @DisplayName: Original Position (Altitude) // @Description: Specifies vehicle's startup altitude (AMSL) // @User: Advanced AP_GROUPINFO("OPOS_ALT", 53, SIM, opos.alt, 584.0f), // @Param: OPOS_HDG // @DisplayName: Original Position (Heading) // @Description: Specifies vehicle's startup heading (0-360) // @User: Advanced AP_GROUPINFO("OPOS_HDG", 54, SIM, opos.hdg, 353.0f), // @Param: LOOP_DELAY // @DisplayName: Extra delay per main loop // @Description: Extra time delay per main loop // @Units: us AP_GROUPINFO("LOOP_DELAY", 55, SIM, loop_delay, 0), // @Path: ./SIM_Buzzer.cpp AP_SUBGROUPINFO(buzzer_sim, "BZ_", 56, SIM, Buzzer), // @Path: ./SIM_ToneAlarm.cpp AP_SUBGROUPINFO(tonealarm_sim, "TA_", 57, SIM, ToneAlarm), // @Param: EFI_TYPE // @DisplayName: Type of Electronic Fuel Injection // @Description: Different types of Electronic Fuel Injection (EFI) systems // @Values: 0:None,1:MegaSquirt EFI system, 2:Löweheiser EFI system, 8:Hirth engines AP_GROUPINFO("EFI_TYPE", 58, SIM, efi_type, SIM::EFI_TYPE_NONE), // 59 was SAFETY_STATE // @Param: VIB_MOT_HMNC // @DisplayName: Motor harmonics // @Description: Motor harmonics generated in SITL AP_GROUPINFO("VIB_MOT_HMNC", 60, SIM, vibe_motor_harmonics, 1), // @Param: VIB_MOT_MASK // @DisplayName: Motor mask // @Description: Motor mask, allowing external simulators to mark motors AP_GROUPINFO("VIB_MOT_MASK", 5, SIM, vibe_motor_mask, 0), // @Param: VIB_MOT_MAX // @DisplayName: Max motor vibration frequency // @Description: Max frequency to use as baseline for adding motor noise for the gyros and accels // @Units: Hz AP_GROUPINFO("VIB_MOT_MAX", 61, SIM, vibe_motor, 0.0f), // @Param: INS_THR_MIN // @DisplayName: Minimum throttle INS noise // @Description: Minimum throttle for simulated ins noise AP_GROUPINFO("INS_THR_MIN", 62, SIM, ins_noise_throttle_min, 0.1f), // @Param: VIB_MOT_MULT // @DisplayName: Vibration motor scale // @Description: Amplitude scaling of motor noise relative to gyro/accel noise AP_GROUPINFO("VIB_MOT_MULT", 63, SIM, vibe_motor_scale, 1.0f), AP_GROUPEND }; // third table of user settable parameters for SITL. const AP_Param::GroupInfo SIM::var_info3[] = { // @Param: ODOM_ENABLE // @DisplayName: Odometry enable // @Description: SITL odometry enabl // @Values: 0:Disable, 1:Enable AP_GROUPINFO("ODOM_ENABLE", 1, SIM, odom_enable, 0), // @Param: LED_LAYOUT // @DisplayName: LED layout // @Description: LED layout config value AP_GROUPINFO("LED_LAYOUT", 11, SIM, led_layout, 0), // @Param: THML_SCENARI // @DisplayName: Thermal scenarios // @Description: Scenario for thermalling simulation, for soaring AP_GROUPINFO("THML_SCENARI", 12, SIM, thermal_scenario, 0), // @Param: VICON_POS_X // @DisplayName: SITL vicon position on vehicle in Forward direction // @Description: SITL vicon position on vehicle in Forward direction // @Units: m // @Range: 0 10 // @User: Advanced // @Param: VICON_POS_Y // @DisplayName: SITL vicon position on vehicle in Right direction // @Description: SITL vicon position on vehicle in Right direction // @Units: m // @Range: 0 10 // @User: Advanced // @Param: VICON_POS_Z // @DisplayName: SITL vicon position on vehicle in Down direction // @Description: SITL vicon position on vehicle in Down direction // @Units: m // @Range: 0 10 // @User: Advanced AP_GROUPINFO("VICON_POS", 14, SIM, vicon_pos_offset, 0), // Buyoancy for submarines AP_GROUPINFO_FRAME("BUOYANCY", 15, SIM, buoyancy, 1, AP_PARAM_FRAME_SUB), // @Param: VICON_GLIT_X // @DisplayName: SITL vicon position glitch North // @Description: SITL vicon position glitch North // @Units: m // @User: Advanced // @Param: VICON_GLIT_Y // @DisplayName: SITL vicon position glitch East // @Description: SITL vicon position glitch East // @Units: m // @User: Advanced // @Param: VICON_GLIT_Z // @DisplayName: SITL vicon position glitch Down // @Description: SITL vicon position glitch Down // @Units: m // @User: Advanced AP_GROUPINFO("VICON_GLIT", 16, SIM, vicon_glitch, 0), // @Param: VICON_FAIL // @DisplayName: SITL vicon failure // @Description: SITL vicon failure // @Values: 0:Vicon Healthy, 1:Vicon Failed // @User: Advanced AP_GROUPINFO("VICON_FAIL", 17, SIM, vicon_fail, 0), // @Param: VICON_YAW // @DisplayName: SITL vicon yaw angle in earth frame // @Description: SITL vicon yaw angle in earth frame // @Units: deg // @Range: 0 360 // @User: Advanced AP_GROUPINFO("VICON_YAW", 18, SIM, vicon_yaw, 0), // @Param: VICON_YAWERR // @DisplayName: SITL vicon yaw error // @Description: SITL vicon yaw added to reported yaw sent to vehicle // @Units: deg // @Range: -180 180 // @User: Advanced AP_GROUPINFO("VICON_YAWERR", 19, SIM, vicon_yaw_error, 0), // @Param: VICON_TMASK // @DisplayName: SITL vicon type mask // @Description: SITL vicon messages sent // @Bitmask: 0:VISION_POSITION_ESTIMATE, 1:VISION_SPEED_ESTIMATE, 2:VICON_POSITION_ESTIMATE, 3:VISION_POSITION_DELTA, 4:ODOMETRY // @User: Advanced AP_GROUPINFO("VICON_TMASK", 20, SIM, vicon_type_mask, 3), // @Param: VICON_VGLI_X // @DisplayName: SITL vicon velocity glitch North // @Description: SITL vicon velocity glitch North // @Units: m/s // @User: Advanced // @Param: VICON_VGLI_Y // @DisplayName: SITL vicon velocity glitch East // @Description: SITL vicon velocity glitch East // @Units: m/s // @User: Advanced // @Param: VICON_VGLI_Z // @DisplayName: SITL vicon velocity glitch Down // @Description: SITL vicon velocity glitch Down // @Units: m/s // @User: Advanced AP_GROUPINFO("VICON_VGLI", 21, SIM, vicon_vel_glitch, 0), // @Param: RATE_HZ // @DisplayName: Loop rate // @Description: SITL Loop rate // @Units: Hz AP_GROUPINFO("RATE_HZ", 22, SIM, loop_rate_hz, SIM_RATE_HZ_DEFAULT), // @Param: IMU_COUNT // @DisplayName: IMU count // @Description: Number of simulated IMUs to create AP_GROUPINFO("IMU_COUNT", 23, SIM, imu_count, 2), // @Path: ./SIM_FETtecOneWireESC.cpp AP_SUBGROUPINFO(fetteconewireesc_sim, "FTOWESC_", 30, SIM, FETtecOneWireESC), // @Path: ./SIM_RichenPower.cpp AP_SUBGROUPINFO(richenpower_sim, "RICH_", 31, SIM, RichenPower), // @Path: ./SIM_IntelligentEnergy24.cpp AP_SUBGROUPINFO(ie24_sim, "IE24_", 32, SIM, IntelligentEnergy24), // user settable barometer parameters // @Param: BARO_COUNT // @DisplayName: Baro count // @Description: Number of simulated baros to create in SITL // @Range: 0 3 AP_GROUPINFO("BARO_COUNT", 33, SIM, baro_count, 2), // @Group: BARO_ // @Path: ./SITL_Baro.cpp AP_SUBGROUPINFO(baro[0], "BARO_", 34, SIM, BaroParm), #if BARO_MAX_INSTANCES > 1 // @Group: BAR2_ // @Path: ./SITL_Baro.cpp AP_SUBGROUPINFO(baro[1], "BAR2_", 35, SIM, BaroParm), #endif #if BARO_MAX_INSTANCES > 2 // @Group: BAR3_ // @Path: ./SITL_Baro.cpp AP_SUBGROUPINFO(baro[2], "BAR3_", 36, SIM, BaroParm), #endif // @Param: TIME_JITTER // @DisplayName: Loop time jitter // @Description: Upper limit of random jitter in loop time // @Units: us // @User: Advanced AP_GROUPINFO("TIME_JITTER", 37, SIM, loop_time_jitter_us, 0), // @Param: ESC_TELEM // @DisplayName: Simulated ESC Telemetry // @Description: enable perfect simulated ESC telemetry // @User: Advanced AP_GROUPINFO("ESC_TELEM", 40, SIM, esc_telem, 1), // @Param: ESC_ARM_RPM // @DisplayName: ESC RPM when armed // @Description: Simulated RPM when motors are armed // @User: Advanced AP_GROUPINFO("ESC_ARM_RPM", 41, SIM, esc_rpm_armed, 0.0f), // @Param: UART_LOSS // @DisplayName: UART byte loss percentage // @Description: Sets percentage of outgoing byte loss on UARTs // @Units: % // @User: Advanced AP_GROUPINFO("UART_LOSS", 42, SIM, uart_byte_loss_pct, 0), // @Group: ARSPD_ // @Path: ./SITL_Airspeed.cpp AP_SUBGROUPINFO(airspeed[0], "ARSPD_", 50, SIM, AirspeedParm), #if AIRSPEED_MAX_SENSORS > 1 // @Group: ARSPD2_ // @Path: ./SITL_Airspeed.cpp AP_SUBGROUPINFO(airspeed[1], "ARSPD2_", 51, SIM, AirspeedParm), #endif // @Param: ADSB_TYPES // @DisplayName: Simulated ADSB Type mask // @Description: specifies which simulated ADSB types are active // @User: Advanced // @Bitmask: 0:MAVLink,3:SageTechMXS AP_GROUPINFO("ADSB_TYPES", 52, SIM, adsb_types, 1), #ifdef WITH_SITL_OSD // @Param: OSD_COLUMNS // @DisplayName: Simulated OSD number of text columns // @Description: Simulated OSD number of text columns // @Range: 10 100 AP_GROUPINFO("OSD_COLUMNS", 53, SIM, osd_columns, 30), // @Param: OSD_ROWS // @DisplayName: Simulated OSD number of text rows // @Description: Simulated OSD number of text rows // @Range: 10 100 AP_GROUPINFO("OSD_ROWS", 54, SIM, osd_rows, 16), #endif #ifdef SFML_JOYSTICK AP_SUBGROUPEXTENSION("", 63, SIM, var_sfml_joystick), #endif // SFML_JOYSTICK AP_GROUPEND }; #if HAL_SIM_GPS_ENABLED // GPS SITL parameters const AP_Param::GroupInfo SIM::var_gps[] = { // 1 was GPS_DISABLE // 2 was GPS_LAG_MS // 3 was GPS_TYPE // 4 was GPS_BYTELOSS // 5 was GPS_NUMSATS // 6 was GPS_GLITCH // 7 was GPS_HZ // 8 was GPS_DRIFTALT // 9 was GPS_POS // 10 was GPS_NOISE // 11 was GPS_LOCKTIME // 12 was GPS_ALT_OFS // 13 was GPS_HDG // 14 was GPS_ACC // 15 was GPS_VERR // 16 was GPS_JAM // 30 was GPS2_DISABLE // 31 was GPS2_LAG_MS // 32 was GPS2_TYPE // 33 was GPS2_BYTELOSS // 34 was GPS2_NUMSATS // 35 was GPS2_GLITCH // 36 was GPS2_HZ // 37 was GPS2_DRIFTALT // 38 was GPS2_POS // 39 was GPS2_NOISE // 40 was GPS2_LOCKTIME // 41 was GPS2_ALT_OFS // 42 was GPS2_HDG // 43 was GPS2_ACC // 44 was GPS2_VERR // @Param: INIT_LAT_OFS // @DisplayName: Initial Latitude Offset // @Description: GPS initial lat offset from origin AP_GROUPINFO("INIT_LAT_OFS", 45, SIM, gps_init_lat_ofs, 0), // @Param: INIT_LON_OFS // @DisplayName: Initial Longitude Offset // @Description: GPS initial lon offset from origin AP_GROUPINFO("INIT_LON_OFS", 46, SIM, gps_init_lon_ofs, 0), // @Param: INIT_ALT_OFS // @DisplayName: Initial Altitude Offset // @Description: GPS initial alt offset from origin AP_GROUPINFO("INIT_ALT_OFS", 47, SIM, gps_init_alt_ofs, 0), // @Param: GPS_LOG_NUM // @DisplayName: GPS Log Number // @Description: Log number for GPS:update_file() AP_GROUPINFO("GPS_LOG_NUM", 48, SIM, gps_log_num, 0), // 49 was GPS2_JAM #if AP_SIM_MAX_GPS_SENSORS > 0 // @Group: GPS1_ // @Path: ./SIM_GPS.cpp AP_SUBGROUPINFO(gps[0], "GPS1_", 50, SIM, GPSParms), #endif #if AP_SIM_MAX_GPS_SENSORS > 1 // @Group: GPS2_ // @Path: ./SIM_GPS.cpp AP_SUBGROUPINFO(gps[1], "GPS2_", 51, SIM, GPSParms), #endif #if AP_SIM_MAX_GPS_SENSORS > 2 // @Group: GPS3_ // @Path: ./SIM_GPS.cpp AP_SUBGROUPINFO(gps[2], "GPS3_", 52, SIM, GPSParms), #endif #if AP_SIM_MAX_GPS_SENSORS > 3 // @Group: GPS4_ // @Path: ./SIM_GPS.cpp AP_SUBGROUPINFO(gps[3], "GPS4_", 53, SIM, GPSParms), #endif AP_GROUPEND }; #endif // HAL_SIM_GPS_ENABLED // Mag SITL parameters const AP_Param::GroupInfo SIM::var_mag[] = { // @Param: MAG_RND // @DisplayName: Mag motor noise factor // @Description: Scaling factor for simulated vibration from motors // @User: Advanced AP_GROUPINFO("MAG_RND", 1, SIM, mag_noise, 0), AP_GROUPINFO("MAG_MOT", 2, SIM, mag_mot, 0), // @Param: MAG_DELAY // @DisplayName: Mag measurement delay // @Description: Magnetometer measurement delay // @Units: ms // @User: Advanced AP_GROUPINFO("MAG_DELAY", 3, SIM, mag_delay, 0), AP_GROUPINFO("MAG1_OFS", 4, SIM, mag_ofs[0], 0), AP_GROUPINFO("MAG_ALY", 5, SIM, mag_anomaly_ned, 0), // @Param: MAG_ALY_HGT // @DisplayName: Magnetic anomaly height // @Description: Height above ground where anomally strength has decayed to 1/8 of the ground level value // @Units: m // @User: Advanced AP_GROUPINFO("MAG_ALY_HGT", 6, SIM, mag_anomaly_hgt, 1.0f), AP_GROUPINFO("MAG1_DIA", 7, SIM, mag_diag[0], 0), AP_GROUPINFO("MAG1_ODI", 8, SIM, mag_offdiag[0], 0), // @Param: MAG1_ORIENT // @DisplayName: MAG1 Orientation // @Description: MAG1 external compass orientation // @User: Advanced AP_GROUPINFO("MAG1_ORIENT", 9, SIM, mag_orient[0], 0), // @Param: MAG1_SCALING // @DisplayName: MAG1 Scaling factor // @Description: Scale the compass 1 to simulate sensor scale factor errors // @User: Advanced AP_GROUPINFO("MAG1_SCALING", 10, SIM, mag_scaling[0], 1), // @Param: MAG1_DEVID // @DisplayName: MAG1 Device ID // @Description: Device ID of simulated compass 1 // @User: Advanced AP_GROUPINFO("MAG1_DEVID", 11, SIM, mag_devid[0], 97539), // @Param: MAG2_DEVID // @DisplayName: MAG2 Device ID // @Description: Device ID of simulated compass 2 // @User: Advanced AP_GROUPINFO("MAG2_DEVID", 12, SIM, mag_devid[1], 131874), #if MAX_CONNECTED_MAGS > 2 // @Param: MAG3_DEVID // @DisplayName: MAG3 Device ID // @Description: Device ID of simulated compass 3 // @User: Advanced AP_GROUPINFO("MAG3_DEVID", 13, SIM, mag_devid[2], 263178), #endif #if MAX_CONNECTED_MAGS > 3 // @Param: MAG4_DEVID // @DisplayName: MAG2 Device ID // @Description: Device ID of simulated compass 4 // @User: Advanced AP_GROUPINFO("MAG4_DEVID", 14, SIM, mag_devid[3], 97283), #endif #if MAX_CONNECTED_MAGS > 4 // @Param: MAG5_DEVID // @DisplayName: MAG5 Device ID // @Description: Device ID of simulated compass 5 // @User: Advanced AP_GROUPINFO("MAG5_DEVID", 15, SIM, mag_devid[4], 97795), #endif #if MAX_CONNECTED_MAGS > 5 // @Param: MAG6_DEVID // @DisplayName: MAG6 Device ID // @Description: Device ID of simulated compass 6 // @User: Advanced AP_GROUPINFO("MAG6_DEVID", 16, SIM, mag_devid[5], 98051), #endif #if MAX_CONNECTED_MAGS > 6 // @Param: MAG7_DEVID // @DisplayName: MAG7 Device ID // @Description: Device ID of simulated compass 7 // @User: Advanced AP_GROUPINFO("MAG7_DEVID", 17, SIM, mag_devid[6], 0), #endif #if MAX_CONNECTED_MAGS > 7 // @Param: MAG8_DEVID // @DisplayName: MAG8 Device ID // @Description: Device ID of simulated compass 8 // @User: Advanced AP_GROUPINFO("MAG8_DEVID", 18, SIM, mag_devid[7], 0), #endif // @Param: MAG1_FAIL // @DisplayName: MAG1 Failure // @Description: Simulated failure of MAG1 // @Values: 0:Disabled, 1:MAG1 Failure // @User: Advanced AP_GROUPINFO("MAG1_FAIL", 26, SIM, mag_fail[0], 0), #if HAL_COMPASS_MAX_SENSORS > 1 AP_GROUPINFO("MAG2_OFS", 19, SIM, mag_ofs[1], 0), AP_GROUPINFO("MAG2_DIA", 20, SIM, mag_diag[1], 0), AP_GROUPINFO("MAG2_ODI", 21, SIM, mag_offdiag[1], 0), // @Param: MAG2_ORIENT // @DisplayName: MAG2 Orientation // @Description: MAG2 external compass orientation // @User: Advanced AP_GROUPINFO("MAG2_ORIENT", 22, SIM, mag_orient[1], 0), // @Param: MAG2_FAIL // @DisplayName: MAG2 Failure // @Description: Simulated failure of MAG2 // @Values: 0:Disabled, 1:MAG2 Failure // @User: Advanced AP_GROUPINFO("MAG2_FAIL", 27, SIM, mag_fail[1], 0), // @Param: MAG2_SCALING // @DisplayName: MAG2 Scaling factor // @Description: Scale the compass 2 to simulate sensor scale factor errors // @User: Advanced AP_GROUPINFO("MAG2_SCALING", 28, SIM, mag_scaling[1], 1), #endif #if HAL_COMPASS_MAX_SENSORS > 2 AP_GROUPINFO("MAG3_OFS", 23, SIM, mag_ofs[2], 0), AP_GROUPINFO("MAG3_DIA", 24, SIM, mag_diag[2], 0), AP_GROUPINFO("MAG3_ODI", 25, SIM, mag_offdiag[2], 0), // @Param: MAG3_FAIL // @DisplayName: MAG3 Failure // @Description: Simulated failure of MAG3 // @Values: 0:Disabled, 1:MAG3 Failure // @User: Advanced AP_GROUPINFO("MAG3_FAIL", 29, SIM, mag_fail[2], 0), // @Param: MAG3_SCALING // @DisplayName: MAG3 Scaling factor // @Description: Scale the compass 3 to simulate sensor scale factor errors // @User: Advanced AP_GROUPINFO("MAG3_SCALING", 30, SIM, mag_scaling[2], 1), // @Param: MAG3_ORIENT // @DisplayName: MAG3 Orientation // @Description: MAG3 external compass orientation // @User: Advanced AP_GROUPINFO("MAG3_ORIENT", 36, SIM, mag_orient[2], 0), #endif // @Param: MAG_SAVE_IDS // @DisplayName: Save MAG devids on startup // @Description: This forces saving of compass devids on startup so that simulated compasses start as calibrated // @Values: 0:Disabled, 1:Enabled // @User: Advanced AP_GROUPINFO("MAG_SAVE_IDS", 37, SIM, mag_save_ids, 1), AP_GROUPEND }; #ifdef SFML_JOYSTICK const AP_Param::GroupInfo SIM::var_sfml_joystick[] = { AP_GROUPINFO("SF_JS_STICK", 1, SIM, sfml_joystick_id, 0), AP_GROUPINFO("SF_JS_AXIS1", 2, SIM, sfml_joystick_axis[0], sf::Joystick::Axis::X), AP_GROUPINFO("SF_JS_AXIS2", 3, SIM, sfml_joystick_axis[1], sf::Joystick::Axis::Y), AP_GROUPINFO("SF_JS_AXIS3", 4, SIM, sfml_joystick_axis[2], sf::Joystick::Axis::Z), AP_GROUPINFO("SF_JS_AXIS4", 5, SIM, sfml_joystick_axis[3], sf::Joystick::Axis::U), AP_GROUPINFO("SF_JS_AXIS5", 6, SIM, sfml_joystick_axis[4], sf::Joystick::Axis::V), AP_GROUPINFO("SF_JS_AXIS6", 7, SIM, sfml_joystick_axis[5], sf::Joystick::Axis::R), AP_GROUPINFO("SF_JS_AXIS7", 8, SIM, sfml_joystick_axis[6], sf::Joystick::Axis::PovX), AP_GROUPINFO("SF_JS_AXIS8", 9, SIM, sfml_joystick_axis[7], sf::Joystick::Axis::PovY), AP_GROUPEND }; #endif //SFML_JOYSTICK // INS SITL parameters const AP_Param::GroupInfo SIM::var_ins[] = { #if HAL_INS_TEMPERATURE_CAL_ENABLE // @Param: IMUT_START // @DisplayName: IMU temperature start // @Description: Starting IMU temperature of a curve AP_GROUPINFO("IMUT_START", 1, SIM, imu_temp_start, 25), // @Param: IMUT_END // @DisplayName: IMU temperature end // @Description: Ending IMU temperature of a curve AP_GROUPINFO("IMUT_END", 2, SIM, imu_temp_end, 45), // @Param: IMUT_TCONST // @DisplayName: IMU temperature time constant // @Description: IMU temperature time constant of the curve AP_GROUPINFO("IMUT_TCONST", 3, SIM, imu_temp_tconst, 300), // @Param: IMUT_FIXED // @DisplayName: IMU fixed temperature // @Description: IMU fixed temperature by user AP_GROUPINFO("IMUT_FIXED", 4, SIM, imu_temp_fixed, 0), #endif // @Param: ACC1_BIAS // @DisplayName: Accel 1 bias // @Description: bias of simulated accelerometer sensor // @User: Advanced // @Vector3Parameter: 1 AP_GROUPINFO("ACC1_BIAS", 5, SIM, accel_bias[0], 0), #if INS_MAX_INSTANCES > 1 // @Param: ACC2_BIAS // @DisplayName: Accel 2 bias // @CopyFieldsFrom: SIM_ACC1_BIAS // @Vector3Parameter: 1 AP_GROUPINFO("ACC2_BIAS", 6, SIM, accel_bias[1], 0), #endif #if INS_MAX_INSTANCES > 2 // @Param: ACC3_BIAS // @DisplayName: Accel 3 bias // @CopyFieldsFrom: SIM_ACC1_BIAS // @Vector3Parameter: 1 AP_GROUPINFO("ACC3_BIAS", 7, SIM, accel_bias[2], 0), #endif // @Param: GYR1_RND // @DisplayName: Gyro 1 motor noise factor // @Description: scaling factor for simulated vibration from motors // @User: Advanced AP_GROUPINFO("GYR1_RND", 8, SIM, gyro_noise[0], 0), #if INS_MAX_INSTANCES > 1 // @Param: GYR2_RND // @DisplayName: Gyro 2 motor noise factor // @CopyFieldsFrom: SIM_GYR1_RND AP_GROUPINFO("GYR2_RND", 9, SIM, gyro_noise[1], 0), #endif #if INS_MAX_INSTANCES > 2 // @Param: GYR3_RND // @DisplayName: Gyro 3 motor noise factor // @CopyFieldsFrom: SIM_GYR1_RND AP_GROUPINFO("GYR3_RND", 10, SIM, gyro_noise[2], 0), #endif // @Param: ACC1_RND // @DisplayName: Accel 1 motor noise factor // @Description: scaling factor for simulated vibration from motors // @User: Advanced AP_GROUPINFO("ACC1_RND", 11, SIM, accel_noise[0], 0), #if INS_MAX_INSTANCES > 1 // @Param: ACC2_RND // @DisplayName: Accel 2 motor noise factor // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC2_RND", 12, SIM, accel_noise[1], 0), #endif #if INS_MAX_INSTANCES > 2 // @Param: ACC3_RND // @DisplayName: Accel 3 motor noise factor // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC3_RND", 13, SIM, accel_noise[2], 0), #endif // @Param: GYR1_SCALE // @DisplayName: Gyro 1 scaling factor // @Description: scaling factors applied to simulated gyroscope // @User: Advanced // @Vector3Parameter: 1 AP_GROUPINFO("GYR1_SCALE", 14, SIM, gyro_scale[0], 0), #if INS_MAX_INSTANCES > 1 // @Param: GYR2_SCALE // @DisplayName: Gyro 2 scaling factor // @CopyFieldsFrom: SIM_GYR1_SCALE // @Vector3Parameter: 1 AP_GROUPINFO("GYR2_SCALE", 15, SIM, gyro_scale[1], 0), #endif #if INS_MAX_INSTANCES > 2 // @Param: GYR3_SCALE // @DisplayName: Gyro 3 scaling factor // @CopyFieldsFrom: SIM_GYR1_SCALE // @Vector3Parameter: 1 AP_GROUPINFO("GYR3_SCALE", 16, SIM, gyro_scale[2], 0), #endif // @Param: ACCEL1_FAIL // @DisplayName: ACCEL1 Failure // @Description: Simulated failure of ACCEL1 // @Values: 0:Disabled, 1:ACCEL1 Failure // @User: Advanced AP_GROUPINFO("ACCEL1_FAIL", 17, SIM, accel_fail[0], 0), #if INS_MAX_INSTANCES > 1 // @Param: ACCEL2_FAIL // @DisplayName: ACCEL2 Failure // @Description: Simulated failure of ACCEL2 // @Values: 0:Disabled, 1:ACCEL2 Failure // @User: Advanced AP_GROUPINFO("ACCEL2_FAIL", 18, SIM, accel_fail[1], 0), #endif #if INS_MAX_INSTANCES > 2 // @Param: ACCEL3_FAIL // @DisplayName: ACCEL3 Failure // @Description: Simulated failure of ACCEL3 // @Values: 0:Disabled, 1:ACCEL3 Failure // @User: Advanced AP_GROUPINFO("ACCEL3_FAIL", 19, SIM, accel_fail[2], 0), #endif // @Param: GYR_FAIL_MSK // @DisplayName: Gyro Failure Mask // @Description: Determines if the gyro reading updates are stopped when for an IMU simulated failure by ACCELx_FAIL params // @Values: 0:Disabled, 1:Readings stopped // @User: Advanced AP_GROUPINFO("GYR_FAIL_MSK", 20, SIM, gyro_fail_mask, 0), // @Param: ACC_FAIL_MSK // @DisplayName: Accelerometer Failure Mask // @Description: Determines if the acclerometer reading updates are stopped when for an IMU simulated failure by ACCELx_FAIL params // @Values: 0:Disabled, 1:Readings stopped // @User: Advanced AP_GROUPINFO("ACC_FAIL_MSK", 21, SIM, accel_fail_mask, 0), // @Param: ACC1_SCAL // @DisplayName: Accel 1 scaling factor // @Description: scaling factors applied to simulated accelerometer // @User: Advanced // @Vector3Parameter: 1 AP_GROUPINFO("ACC1_SCAL", 22, SIM, accel_scale[0], 0), #if INS_MAX_INSTANCES > 1 // @Param: ACC2_SCAL // @DisplayName: Accel 2 scaling factor // @CopyFieldsFrom: SIM_ACC1_SCAL // @Vector3Parameter: 1 AP_GROUPINFO("ACC2_SCAL", 23, SIM, accel_scale[1], 0), #endif #if INS_MAX_INSTANCES > 2 // @Param: ACC3_SCAL // @DisplayName: Accel 3 scaling factor // @CopyFieldsFrom: SIM_ACC1_SCAL // @Vector3Parameter: 1 AP_GROUPINFO("ACC3_SCAL", 24, SIM, accel_scale[2], 0), #endif AP_GROUPINFO("ACC_TRIM", 25, SIM, accel_trim, 0), #if APM_BUILD_TYPE(APM_BUILD_Rover) // @Param{Rover}: SAIL_TYPE // @DisplayName: Sailboat simulation sail type // @Description: 0: mainsail with sheet, 1: directly actuated wing AP_GROUPINFO("SAIL_TYPE", 26, SIM, sail_type, 0), #endif // @Param: JSON_MASTER // @DisplayName: JSON master instance // @Description: the instance number to take servos from AP_GROUPINFO("JSON_MASTER", 27, SIM, ride_along_master, 0), // @Param: OH_MASK // @DisplayName: SIM-on_hardware Output Enable Mask // @Description: channels which are passed through to actual hardware when running sim on actual hardware AP_GROUPINFO("OH_MASK", 28, SIM, on_hardware_output_enable_mask, 0), #if AP_SIM_INS_FILE_ENABLED // @Param: GYR_FILE_RW // @DisplayName: Gyro data to/from files // @Description: Read and write gyro data to/from files // @Values: 0:Stop writing data, 1:Read data from file, 2:Write data to a file, 3: Read data from file and stop on EOF AP_GROUPINFO("GYR_FILE_RW", 29, SIM, gyro_file_rw, INSFileMode::INS_FILE_NONE), // @Param: ACC_FILE_RW // @DisplayName: Accelerometer data to/from files // @Description: Read and write accelerometer data to/from files // @Values: 0:Stop writing data, 1:Read data from file, 2:Write data to a file, 3: Read data from file and stop on EOF AP_GROUPINFO("ACC_FILE_RW", 30, SIM, accel_file_rw, INSFileMode::INS_FILE_NONE), #endif // @Param: GYR1_BIAS_X // @DisplayName: First Gyro bias on X axis // @Description: First Gyro bias on X axis // @Units: rad/s // @User: Advanced // @Param: GYR1_BIAS_Y // @DisplayName: First Gyro bias on Y axis // @Description: First Gyro bias on Y axis // @Units: rad/s // @User: Advanced // @Param: GYR1_BIAS_Z // @DisplayName: First Gyro bias on Z axis // @Description: First Gyro bias on Z axis // @Units: rad/s // @User: Advanced AP_GROUPINFO("GYR1_BIAS", 31, SIM, gyro_bias[0], 0), #if INS_MAX_INSTANCES > 1 // @Param: GYR2_BIAS_X // @CopyFieldsFrom: SIM_GYR1_BIAS_X // @DisplayName: Second Gyro bias on X axis // @Description: Second Gyro bias on X axis // @Param: GYR2_BIAS_Y // @CopyFieldsFrom: SIM_GYR1_BIAS_Y // @DisplayName: Second Gyro bias on Y axis // @Description: Second Gyro bias on Y axis // @Param: GYR2_BIAS_Z // @CopyFieldsFrom: SIM_GYR1_BIAS_Z // @DisplayName: Second Gyro bias on Z axis // @Description: Second Gyro bias on Z axis AP_GROUPINFO("GYR2_BIAS", 32, SIM, gyro_bias[1], 0), #endif #if INS_MAX_INSTANCES > 2 // @Param: GYR3_BIAS_X // @CopyFieldsFrom: SIM_GYR1_BIAS_X // @DisplayName: Third Gyro bias on X axis // @Description: Third Gyro bias on X axis // @Param: GYR3_BIAS_Y // @CopyFieldsFrom: SIM_GYR1_BIAS_Y // @DisplayName: Third Gyro bias on Y axis // @Description: Third Gyro bias on Y axis // @Param: GYR3_BIAS_Z // @CopyFieldsFrom: SIM_GYR1_BIAS_Z // @DisplayName: Third Gyro bias on Z axis // @Description: Third Gyro bias on Z axis AP_GROUPINFO("GYR3_BIAS", 33, SIM, gyro_bias[2], 0), #endif #if INS_MAX_INSTANCES > 3 // @Param: ACC4_SCAL // @DisplayName: Accel 4 scaling factor // @CopyFieldsFrom: SIM_ACC1_SCAL // @Vector3Parameter: 1 AP_GROUPINFO("ACC4_SCAL", 34, SIM, accel_scale[3], 0), // @Param: ACCEL4_FAIL // @DisplayName: ACCEL4 Failure // @Description: Simulated failure of ACCEL4 // @Values: 0:Disabled, 1:ACCEL4 Failure // @User: Advanced AP_GROUPINFO("ACCEL4_FAIL", 35, SIM, accel_fail[3], 0), // @Param: GYR4_SCALE // @DisplayName: Gyro 4 scaling factor // @CopyFieldsFrom: SIM_GYR1_SCALE // @Vector3Parameter: 1 AP_GROUPINFO("GYR4_SCALE", 36, SIM, gyro_scale[3], 0), // @Param: ACC4_RND // @DisplayName: Accel 4 motor noise factor // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC4_RND", 37, SIM, accel_noise[3], 0), // @Param: GYR4_RND // @DisplayName: Gyro 4 motor noise factor // @CopyFieldsFrom: SIM_GYR1_RND AP_GROUPINFO("GYR4_RND", 38, SIM, gyro_noise[3], 0), // @Param: ACC4_BIAS // @DisplayName: Accel 4 bias // @CopyFieldsFrom: SIM_ACC1_BIAS // @Vector3Parameter: 1 AP_GROUPINFO("ACC4_BIAS", 39, SIM, accel_bias[3], 0), // @Param: GYR4_BIAS_X // @CopyFieldsFrom: SIM_GYR1_BIAS_X // @DisplayName: Fourth Gyro bias on X axis // @Description: Fourth Gyro bias on X axis // @Param: GYR4_BIAS_Y // @CopyFieldsFrom: SIM_GYR1_BIAS_Y // @DisplayName: Fourth Gyro bias on Y axis // @Description: Fourth Gyro bias on Y axis // @Param: GYR4_BIAS_Z // @CopyFieldsFrom: SIM_GYR1_BIAS_Z // @DisplayName: Fourth Gyro bias on Z axis // @Description: Fourth Gyro bias on Z axis AP_GROUPINFO("GYR4_BIAS", 40, SIM, gyro_bias[3], 0), #endif #if INS_MAX_INSTANCES > 4 // @Param: ACC5_SCAL // @DisplayName: Accel 4 scaling factor // @CopyFieldsFrom: SIM_ACC1_SCAL // @Vector3Parameter: 1 AP_GROUPINFO("ACC5_SCAL", 41, SIM, accel_scale[4], 0), // @Param: ACCEL5_FAIL // @DisplayName: ACCEL5 Failure // @Description: Simulated failure of ACCEL5 // @Values: 0:Disabled, 1:ACCEL5 Failure // @User: Advanced AP_GROUPINFO("ACCEL5_FAIL", 42, SIM, accel_fail[4], 0), // @Param: GYR5_SCALE // @DisplayName: Gyro 5 scaling factor // @CopyFieldsFrom: SIM_GYR1_SCALE // @Vector3Parameter: 1 AP_GROUPINFO("GYR5_SCALE", 43, SIM, gyro_scale[4], 0), // @Param: ACC5_RND // @DisplayName: Accel 5 motor noise factor // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC5_RND", 44, SIM, accel_noise[4], 0), // @Param: GYR5_RND // @DisplayName: Gyro 5 motor noise factor // @CopyFieldsFrom: SIM_GYR1_RND AP_GROUPINFO("GYR5_RND", 45, SIM, gyro_noise[4], 0), // @Param: ACC5_BIAS // @DisplayName: Accel 5 bias // @CopyFieldsFrom: SIM_ACC1_BIAS // @Vector3Parameter: 1 AP_GROUPINFO("ACC5_BIAS", 46, SIM, accel_bias[4], 0), // @Param: GYR5_BIAS_X // @CopyFieldsFrom: SIM_GYR1_BIAS_X // @DisplayName: Fifth Gyro bias on X axis // @Description: Fifth Gyro bias on X axis // @Param: GYR5_BIAS_Y // @CopyFieldsFrom: SIM_GYR1_BIAS_Y // @DisplayName: Fifth Gyro bias on Y axis // @Description: Fifth Gyro bias on Y axis // @Param: GYR5_BIAS_Z // @CopyFieldsFrom: SIM_GYR1_BIAS_Z // @DisplayName: Fifth Gyro bias on Z axis // @Description: Fifth Gyro bias on Z axis AP_GROUPINFO("GYR5_BIAS", 47, SIM, gyro_bias[4], 0), #endif // @Param: OH_RELAY_MSK // @DisplayName: SIM-on_hardware Relay Enable Mask // @Description: Allow relay output operation when running SIM-on-hardware AP_GROUPINFO("OH_RELAY_MSK", 48, SIM, on_hardware_relay_enable_mask, SIM_DEFAULT_ENABLED_RELAY_CHANNELS), // @Param: CLAMP_CH // @DisplayName: Simulated Clamp Channel // @Description: If non-zero the vehicle will be clamped in position until the value on this servo channel passes 1800PWM AP_GROUPINFO("CLAMP_CH", 49, SIM, clamp_ch, 0), // the IMUT parameters must be last due to the enable parameters #if HAL_INS_TEMPERATURE_CAL_ENABLE AP_SUBGROUPINFO(imu_tcal[0], "IMUT1_", 61, SIM, AP_InertialSensor_TCal), #if INS_MAX_INSTANCES > 1 AP_SUBGROUPINFO(imu_tcal[1], "IMUT2_", 62, SIM, AP_InertialSensor_TCal), #endif #if INS_MAX_INSTANCES > 2 AP_SUBGROUPINFO(imu_tcal[2], "IMUT3_", 63, SIM, AP_InertialSensor_TCal), #endif #if INS_MAX_INSTANCES > 3 AP_SUBGROUPINFO(imu_tcal[3], "IMUT4_", 60, SIM, AP_InertialSensor_TCal), #endif #if INS_MAX_INSTANCES > 4 AP_SUBGROUPINFO(imu_tcal[4], "IMUT5_", 59, SIM, AP_InertialSensor_TCal), #endif #endif // HAL_INS_TEMPERATURE_CAL_ENABLE AP_GROUPEND }; // user settable parameters for the physics models const AP_Param::GroupInfo SIM::ModelParm::var_info[] = { #if AP_SIM_SHIP_ENABLED // @Group: SHIP_ // @Path: ./SIM_Ship.cpp AP_SUBGROUPINFO(shipsim, "SHIP_", 1, SIM::ModelParm, ShipSim), #endif #if AP_SIM_STRATOBLIMP_ENABLED // @Group: SB_ // @Path: ./SIM_StratoBlimp.cpp AP_SUBGROUPPTR(stratoblimp_ptr, "SB_", 2, SIM::ModelParm, StratoBlimp), #endif #if AP_SIM_GLIDER_ENABLED // @Group: GLD_ // @Path: ./SIM_Glider.cpp AP_SUBGROUPPTR(glider_ptr, "GLD_", 3, SIM::ModelParm, Glider), #endif #if AP_SIM_SLUNGPAYLOAD_ENABLED // @Group: SLUP_ // @Path: ./SIM_SlungPayload.cpp AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim), #endif #if AP_SIM_FLIGHTAXIS_ENABLED // @Group: RFL_ // @Path: ./SIM_FlightAxis.cpp AP_SUBGROUPPTR(flightaxis_ptr, "RFL_", 5, SIM::ModelParm, FlightAxis), #endif AP_GROUPEND }; const Location post_origin { 518752066, 146487830, 0, Location::AltFrame::ABSOLUTE }; /* report SITL state via MAVLink SIMSTATE*/ void SIM::simstate_send(mavlink_channel_t chan) const { if (stop_MAVLink_sim_state) { // Sim only MAVLink messages disabled to give more relaistic data rates return; } float yaw; // convert to same conventions as DCM yaw = state.yawDeg; if (yaw > 180) { yaw -= 360; } mavlink_msg_simstate_send(chan, ToRad(state.rollDeg), ToRad(state.pitchDeg), ToRad(yaw), state.xAccel, state.yAccel, state.zAccel, radians(state.rollRate), radians(state.pitchRate), radians(state.yawRate), state.latitude*1.0e7, state.longitude*1.0e7); } /* report SITL state via MAVLink SIM_STATE */ void SIM::sim_state_send(mavlink_channel_t chan) const { if (stop_MAVLink_sim_state) { // Sim only MAVLink messages disabled to give more relaistic data rates return; } // convert to same conventions as DCM float yaw = state.yawDeg; if (yaw > 180) { yaw -= 360; } mavlink_msg_sim_state_send(chan, state.quaternion.q1, state.quaternion.q2, state.quaternion.q3, state.quaternion.q4, ToRad(state.rollDeg), ToRad(state.pitchDeg), ToRad(yaw), state.xAccel, state.yAccel, state.zAccel, radians(state.rollRate), radians(state.pitchRate), radians(state.yawRate), state.latitude*1.0e7, state.longitude*1.0e7, (float)state.altitude, 0.0, 0.0, state.speedN, state.speedE, state.speedD, (int32_t)(state.latitude*1.0e7), (int32_t)(state.longitude*1.0e7)); } #if HAL_LOGGING_ENABLED /* report SITL state to AP_Logger */ void SIM::Log_Write_SIMSTATE() { float yaw; // convert to same conventions as DCM yaw = state.yawDeg; if (yaw > 180) { yaw -= 360; } struct log_AHRS pkt = { LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG), time_us : AP_HAL::micros64(), roll : (int16_t)(state.rollDeg*100), pitch : (int16_t)(state.pitchDeg*100), yaw : (uint16_t)(wrap_360_cd(yaw*100)), alt : (float)state.altitude, lat : (int32_t)(state.latitude*1.0e7), lng : (int32_t)(state.longitude*1.0e7), q1 : state.quaternion.q1, q2 : state.quaternion.q2, q3 : state.quaternion.q3, q4 : state.quaternion.q4, }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } #endif /* convert a set of roll rates from earth frame to body frame output values are in radians/second */ void SIM::convert_body_frame(double rollDeg, double pitchDeg, double rollRate, double pitchRate, double yawRate, double *p, double *q, double *r) { double phi, theta, phiDot, thetaDot, psiDot; phi = ToRad(rollDeg); theta = ToRad(pitchDeg); phiDot = ToRad(rollRate); thetaDot = ToRad(pitchRate); psiDot = ToRad(yawRate); *p = phiDot - psiDot*sin(theta); *q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta); *r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot; } /* convert angular velocities from body frame to earth frame. all inputs and outputs are in radians/s */ Vector3f SIM::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro) { float p = gyro.x; float q = gyro.y; float r = gyro.z; float phi, theta, psi; dcm.to_euler(&phi, &theta, &psi); float phiDot = p + tanf(theta)*(q*sinf(phi) + r*cosf(phi)); float thetaDot = q*cosf(phi) - r*sinf(phi); if (fabsf(cosf(theta)) < 1.0e-20f) { theta += 1.0e-10f; } float psiDot = (q*sinf(phi) + r*cosf(phi))/cosf(theta); return Vector3f(phiDot, thetaDot, psiDot); } // get the rangefinder reading for the desired rotation, returns -1 for no data float SIM::get_rangefinder(uint8_t instance) { if (instance < ARRAY_SIZE(state.rangefinder_m)) { return state.rangefinder_m[instance]; } return nanf(""); }; float SIM::measure_distance_at_angle_bf(const Location &location, float angle) const { // should we populate state.rangefinder_m[...] from this? Vector2f vehicle_pos_cm; if (!location.get_vector_xy_from_origin_NE(vehicle_pos_cm)) { // should probably use SITL variables... return 0.0f; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL static uint64_t count = 0; if (count == 0) { unlink("/tmp/rayfile.scr"); unlink("/tmp/intersectionsfile.scr"); } count++; // the 1000 here is so the files don't grow unbounded const bool write_debug_files = count < 1000; FILE *rayfile = nullptr; if (write_debug_files) { rayfile = fopen("/tmp/rayfile.scr", "a"); } #endif // cast a ray from location out 200m... Location location2 = location; location2.offset_bearing(wrap_180(angle + state.yawDeg), 200); Vector2f ray_endpos_cm; if (!location2.get_vector_xy_from_origin_NE(ray_endpos_cm)) { // should probably use SITL variables... return 0.0f; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (rayfile != nullptr) { ::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7); fclose(rayfile); } // setup a grid of posts FILE *postfile = nullptr; FILE *intersectionsfile = nullptr; if (write_debug_files) { static bool postfile_written; if (!postfile_written) { ::fprintf(stderr, "Writing /tmp/post-locations.scr\n"); postfile_written = true; postfile = fopen("/tmp/post-locations.scr", "w"); } intersectionsfile = fopen("/tmp/intersections.scr", "a"); } #endif const float radius_cm = 100.0f; float min_dist_cm = 1000000.0; const uint8_t num_post_offset = 10; for (int8_t x=-num_post_offset; x