From e55e74b4d55cabbb222dc7f9941441b0586b1280 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Sat, 31 Aug 2024 15:58:04 -0700 Subject: [PATCH] SITL: updated Xplane model and params --- .gitignore | 1 - Tools/autotest/models/sa_gd2000.parm | 1407 +++++++++++++++++++ Tools/autotest/models/sa_gd2000_xplane.json | 15 + libraries/SITL/SIM_Plane.h | 2 +- libraries/SITL/SIM_XPlane.cpp | 4 +- scripts/droptest.lua | 829 +++++++++++ 6 files changed, 2255 insertions(+), 3 deletions(-) create mode 100644 Tools/autotest/models/sa_gd2000.parm create mode 100644 Tools/autotest/models/sa_gd2000_xplane.json create mode 100644 scripts/droptest.lua diff --git a/.gitignore b/.gitignore index debfef06f055fc..2a54ca25cc82e2 100644 --- a/.gitignore +++ b/.gitignore @@ -138,7 +138,6 @@ way.txt *.wbproj *.wbproj segv_*out -/scripts/ /repl/ /Rover/scripts/ /Rover/repl/ diff --git a/Tools/autotest/models/sa_gd2000.parm b/Tools/autotest/models/sa_gd2000.parm new file mode 100644 index 00000000000000..b987885ae7432d --- /dev/null +++ b/Tools/autotest/models/sa_gd2000.parm @@ -0,0 +1,1407 @@ +ACRO_LOCKING,0 +ACRO_PITCH_RATE,180 +ACRO_ROLL_RATE,180 +ACRO_YAW_RATE,0 +ADSB_TYPE,0 +AFS_ENABLE,0 +AHRS_COMP_BETA,0.1 +AHRS_EKF_TYPE,10 +AHRS_GPS_GAIN,1 +AHRS_GPS_MINSATS,6 +AHRS_GPS_USE,1 +AHRS_OPTIONS,0 +AHRS_ORIENTATION,0 +AHRS_RP_P,0.2 +AHRS_TRIM_X,0.0001 +AHRS_TRIM_Y,0.0001 +AHRS_TRIM_Z,0 +AHRS_WIND_MAX,0 +AHRS_YAW_P,0.2 +AIRSPEED_CRUISE,49.17 +AIRSPEED_MAX,58 +AIRSPEED_MIN,43 +AIRSPEED_STALL,0 +ALT_OFFSET,0 +ARMING_ACCTHRESH,0.75 +ARMING_CHECK,1 +ARMING_MAGTHRESH,100 +ARMING_MIS_ITEMS,0 +ARMING_OPTIONS,0 +ARMING_REQUIRE,1 +ARMING_RUDDER,1 +ARSPD_AUTOCAL,0 +ARSPD_BUS,1 +ARSPD_DEVID,0 +ARSPD_OFF_PCNT,0 +ARSPD_OFFSET,2013.91 +ARSPD_OPTIONS,11 +ARSPD_PIN,1 +ARSPD_PRIMARY,0 +ARSPD_PSI_RANGE,1 +ARSPD_RATIO,2 +ARSPD_SKIP_CAL,1 +ARSPD_TUBE_ORDR,2 +ARSPD_TYPE,2 +ARSPD_USE,1 +ARSPD_WIND_GATE,5 +ARSPD_WIND_MAX,0 +ARSPD_WIND_WARN,0 +ARSPD2_TYPE,0 +ARSPDWIND_DIR,0 +ARSPDWIND_SPD,0 +AUTOTUNE_AXES,7 +AUTOTUNE_LEVEL,6 +AUTOTUNE_OPTIONS,0 +AVD_ENABLE,0 +BARO_ALT_OFFSET,0 +BARO_ALTERR_MAX,2000 +BARO_EXT_BUS,-1 +BARO_FIELD_ELV,0 +BARO_FLTR_RNG,0 +BARO_GND_TEMP,0 +BARO_OPTIONS,0 +BARO_PRIMARY,0 +BARO_PROBE_EXT,0 +BARO1_DEVID,65540 +BARO1_GND_PRESS,101326.4 +BARO1_WCF_ENABLE,0 +BARO2_DEVID,0 +BARO2_GND_PRESS,82593.98 +BARO2_WCF_ENABLE,0 +BARO3_DEVID,0 +BARO3_GND_PRESS,0 +BARO3_WCF_ENABLE,0 +BATT_MONITOR,0 +BATT2_MONITOR,0 +BATT3_MONITOR,0 +BATT4_MONITOR,0 +BATT5_MONITOR,0 +BATT6_MONITOR,0 +BATT7_MONITOR,0 +BATT8_MONITOR,0 +BATT9_MONITOR,0 +BRD_BOOT_DELAY,0 +BRD_OPTIONS,0 +BRD_RTC_TYPES,1 +BRD_RTC_TZ_MIN,0 +BRD_SAFETY_DEFLT,0 +BRD_SAFETY_MASK,0 +BRD_SAFETYOPTION,3 +BRD_SD_FENCE,0 +BRD_SD_MISSION,0 +BRD_SERIAL_NUM,0 +BRD_VBUS_MIN,4.3 +BRD_VSERVO_MIN,0 +BTN_ENABLE,1 +BTN_FUNC1,0 +BTN_FUNC2,0 +BTN_FUNC3,0 +BTN_FUNC4,0 +BTN_OPTIONS1,0 +BTN_OPTIONS2,0 +BTN_OPTIONS3,0 +BTN_OPTIONS4,0 +BTN_PIN1,1 +BTN_PIN2,-1 +BTN_PIN3,-1 +BTN_PIN4,-1 +BTN_REPORT_SEND,10 +CAM_AUTO_ONLY,0 +CAM_MAX_ROLL,0 +CAM_RC_TYPE,0 +CAM1_TYPE,0 +CAM2_TYPE,0 +CAN_D1_PROTOCOL,1 +CAN_D1_PROTOCOL2,0 +CAN_D2_PROTOCOL,1 +CAN_D2_PROTOCOL2,0 +CAN_LOGLEVEL,0 +CAN_P1_DRIVER,0 +CAN_P2_DRIVER,0 +CHUTE_ENABLED,0 +COMPASS_AUTO_ROT,2 +COMPASS_AUTODEC,1 +COMPASS_CAL_FIT,16 +COMPASS_DEC,0.2058348 +COMPASS_DEV_ID,97539 +COMPASS_DEV_ID2,131874 +COMPASS_DEV_ID3,263178 +COMPASS_DEV_ID4,97283 +COMPASS_DEV_ID5,97795 +COMPASS_DEV_ID6,98051 +COMPASS_DEV_ID7,0 +COMPASS_DEV_ID8,0 +COMPASS_DIA_X,1 +COMPASS_DIA_Y,1 +COMPASS_DIA_Z,1 +COMPASS_DIA2_X,1 +COMPASS_DIA2_Y,1 +COMPASS_DIA2_Z,1 +COMPASS_DIA3_X,1 +COMPASS_DIA3_Y,1 +COMPASS_DIA3_Z,1 +COMPASS_DISBLMSK,0 +COMPASS_ENABLE,1 +COMPASS_EXTERN2,0 +COMPASS_EXTERN3,0 +COMPASS_EXTERNAL,1 +COMPASS_FLTR_RNG,0 +COMPASS_LEARN,0 +COMPASS_MOT_X,0 +COMPASS_MOT_Y,0 +COMPASS_MOT_Z,0 +COMPASS_MOT2_X,0 +COMPASS_MOT2_Y,0 +COMPASS_MOT2_Z,0 +COMPASS_MOT3_X,0 +COMPASS_MOT3_Y,0 +COMPASS_MOT3_Z,0 +COMPASS_MOTCT,0 +COMPASS_ODI_X,0 +COMPASS_ODI_Y,0 +COMPASS_ODI_Z,0 +COMPASS_ODI2_X,0 +COMPASS_ODI2_Y,0 +COMPASS_ODI2_Z,0 +COMPASS_ODI3_X,0 +COMPASS_ODI3_Y,0 +COMPASS_ODI3_Z,0 +COMPASS_OFFS_MAX,1800 +COMPASS_OFS_X,5 +COMPASS_OFS_Y,13 +COMPASS_OFS_Z,-18 +COMPASS_OFS2_X,5 +COMPASS_OFS2_Y,13 +COMPASS_OFS2_Z,-18 +COMPASS_OFS3_X,5 +COMPASS_OFS3_Y,13 +COMPASS_OFS3_Z,-18 +COMPASS_OPTIONS,0 +COMPASS_ORIENT,0 +COMPASS_ORIENT2,0 +COMPASS_ORIENT3,0 +COMPASS_PMOT_EN,0 +COMPASS_PRIO1_ID,97539 +COMPASS_PRIO2_ID,131874 +COMPASS_PRIO3_ID,263178 +COMPASS_SCALE,1 +COMPASS_SCALE2,1 +COMPASS_SCALE3,1 +COMPASS_USE,1 +COMPASS_USE2,0 +COMPASS_USE3,0 +CRASH_ACC_THRESH,0 +CRASH_DETECT,0 +CRUISE_ALT_FLOOR,0 +CUST_ROT_ENABLE,0 +DID_ENABLE,0 +DSPOILER_AILMTCH,100 +DSPOILER_CROW_W1,0 +DSPOILER_CROW_W2,0 +DSPOILER_OPTS,3 +DSPOILR_RUD_RATE,100 +EAHRS_TYPE,0 +EFI_TYPE,0 +EK2_ENABLE,0 +EK3_ABIAS_P_NSE,0.02 +EK3_ACC_BIAS_LIM,1 +EK3_ACC_P_NSE,0.35 +EK3_AFFINITY,0 +EK3_ALT_M_NSE,3 +EK3_BCN_DELAY,50 +EK3_BCN_I_GTE,500 +EK3_BCN_M_NSE,1 +EK3_BETA_MASK,0 +EK3_CHECK_SCALE,150 +EK3_DRAG_BCOEF_X,0 +EK3_DRAG_BCOEF_Y,0 +EK3_DRAG_M_NSE,0.5 +EK3_DRAG_MCOEF,0 +EK3_EAS_I_GATE,400 +EK3_EAS_M_NSE,1.4 +EK3_ENABLE,1 +EK3_ERR_THRESH,0.2 +EK3_FLOW_DELAY,10 +EK3_FLOW_I_GATE,500 +EK3_FLOW_M_NSE,0.15 +EK3_FLOW_USE,2 +EK3_GBIAS_P_NSE,0.001 +EK3_GLITCH_RAD,25 +EK3_GND_EFF_DZ,4 +EK3_GPS_CHECK,31 +EK3_GPS_VACC_MAX,0 +EK3_GSF_RST_MAX,2 +EK3_GSF_RUN_MASK,3 +EK3_GSF_USE_MASK,3 +EK3_GYRO_P_NSE,0.015 +EK3_HGT_DELAY,60 +EK3_HGT_I_GATE,500 +EK3_HRT_FILT,2 +EK3_IMU_MASK,3 +EK3_LOG_LEVEL,0 +EK3_MAG_CAL,0 +EK3_MAG_EF_LIM,50 +EK3_MAG_I_GATE,300 +EK3_MAG_M_NSE,0.05 +EK3_MAG_MASK,0 +EK3_MAGB_P_NSE,0.0001 +EK3_MAGE_P_NSE,0.001 +EK3_MAX_FLOW,2.5 +EK3_NOAID_M_NSE,10 +EK3_OGN_HGT_MASK,0 +EK3_OGNM_TEST_SF,2 +EK3_OPTIONS,0 +EK3_POS_I_GATE,500 +EK3_POSNE_M_NSE,0.5 +EK3_PRIMARY,0 +EK3_RNG_I_GATE,500 +EK3_RNG_M_NSE,0.5 +EK3_RNG_USE_HGT,-1 +EK3_RNG_USE_SPD,2 +EK3_SRC_OPTIONS,1 +EK3_SRC1_POSXY,3 +EK3_SRC1_POSZ,1 +EK3_SRC1_VELXY,3 +EK3_SRC1_VELZ,3 +EK3_SRC1_YAW,1 +EK3_SRC2_POSXY,0 +EK3_SRC2_POSZ,1 +EK3_SRC2_VELXY,0 +EK3_SRC2_VELZ,0 +EK3_SRC2_YAW,0 +EK3_SRC3_POSXY,0 +EK3_SRC3_POSZ,1 +EK3_SRC3_VELXY,0 +EK3_SRC3_VELZ,0 +EK3_SRC3_YAW,0 +EK3_TAU_OUTPUT,25 +EK3_TERR_GRAD,0.1 +EK3_VEL_I_GATE,500 +EK3_VELD_M_NSE,0.7 +EK3_VELNE_M_NSE,0.5 +EK3_VIS_VERR_MAX,0.9 +EK3_VIS_VERR_MIN,0.1 +EK3_WENC_VERR,0.1 +EK3_WIND_P_NSE,0.1 +EK3_WIND_PSCALE,1 +EK3_YAW_I_GATE,300 +EK3_YAW_M_NSE,0.5 +ESC_TLM_MAV_OFS,0 +FBWB_CLIMB_RATE,2 +FBWB_ELEV_REV,0 +FENCE_ACTION,1 +FENCE_ALT_MAX,100 +FENCE_ALT_MIN,-10 +FENCE_AUTOENABLE,0 +FENCE_ENABLE,0 +FENCE_MARGIN,2 +FENCE_OPTIONS,1 +FENCE_RADIUS,300 +FENCE_RET_ALT,0 +FENCE_RET_RALLY,0 +FENCE_TOTAL,0 +FENCE_TYPE,4 +FFT_ENABLE,0 +FILT1_TYPE,0 +FILT2_TYPE,0 +FILT3_TYPE,0 +FILT4_TYPE,0 +FILT5_TYPE,0 +FILT6_TYPE,0 +FILT7_TYPE,0 +FILT8_TYPE,0 +FLAP_1_PERCNT,0 +FLAP_1_SPEED,0 +FLAP_2_PERCNT,0 +FLAP_2_SPEED,0 +FLAP_SLEWRATE,75 +FLIGHT_OPTIONS,0 +FLOW_TYPE,0 +FLTMODE_CH,0 +FLTMODE_GCSBLOCK,0 +FLTMODE1,0 +FLTMODE2,0 +FLTMODE3,5 +FLTMODE4,5 +FLTMODE5,10 +FLTMODE6,10 +FOLL_ENABLE,0 +FRSKY_DNLINK_ID,27 +FRSKY_DNLINK1_ID,20 +FRSKY_DNLINK2_ID,7 +FRSKY_OPTIONS,0 +FRSKY_UPLINK_ID,13 +FS_EKF_THRESH,0.8 +FS_GCS_ENABL,0 +FS_LONG_ACTN,0 +FS_LONG_TIMEOUT,10 +FS_SHORT_ACTN,0 +FS_SHORT_TIMEOUT,4 +FWD_BAT_IDX,0 +FWD_BAT_VOLT_MAX,0 +FWD_BAT_VOLT_MIN,0 +GCS_PID_MASK,1 +GEN_TYPE,0 +GLIDE_SLOPE_MIN,15 +GLIDE_SLOPE_THR,5 +GPS_AUTO_CONFIG,1 +GPS_AUTO_SWITCH,1 +GPS_BLEND_MASK,5 +GPS_DRV_OPTIONS,0 +GPS_INJECT_TO,127 +GPS_MIN_ELEV,-100 +GPS_NAVFILTER,8 +GPS_PRIMARY,0 +GPS_RAW_DATA,0 +GPS_SAVE_CFG,2 +GPS_SBAS_MODE,2 +GPS_SBP_LOGMASK,-256 +GPS1_CAN_NODEID,0 +GPS1_CAN_OVRIDE,0 +GPS1_COM_PORT,1 +GPS1_DELAY_MS,0 +GPS1_GNSS_MODE,0 +GPS1_MB_TYPE,0 +GPS1_POS_X,0 +GPS1_POS_Y,0 +GPS1_POS_Z,0 +GPS1_RATE_MS,200 +GPS1_TYPE,1 +GPS2_TYPE,0 +GRIP_ENABLE,0 +GROUND_STEER_ALT,0 +GROUND_STEER_DPS,90 +GUIDED_D,0 +GUIDED_D_FF,0 +GUIDED_FF,0 +GUIDED_FLTD,5 +GUIDED_FLTE,5 +GUIDED_FLTT,5 +GUIDED_I,0 +GUIDED_IMAX,10 +GUIDED_NEF,0 +GUIDED_NTF,0 +GUIDED_P,5000 +GUIDED_PDMX,0 +GUIDED_SMAX,0 +HOME_RESET_ALT,0 +ICE_ENABLE,0 +INITIAL_MODE,0 +INS_ACC_BODYFIX,2 +INS_ACC_ID,2753028 +INS_ACC1_CALTEMP,-300 +INS_ACC2_CALTEMP,-300 +INS_ACC2_ID,2753036 +INS_ACC2OFFS_X,0.2499572 +INS_ACC2OFFS_Y,0.250202 +INS_ACC2OFFS_Z,0.008914948 +INS_ACC2SCAL_X,1 +INS_ACC2SCAL_Y,1 +INS_ACC2SCAL_Z,1 +INS_ACC3_CALTEMP,-300 +INS_ACC3_ID,0 +INS_ACC3OFFS_X,0 +INS_ACC3OFFS_Y,0 +INS_ACC3OFFS_Z,0 +INS_ACC3SCAL_X,0 +INS_ACC3SCAL_Y,0 +INS_ACC3SCAL_Z,0 +INS_ACCEL_FILTER,20 +INS_ACCOFFS_X,0.250558 +INS_ACCOFFS_Y,0.2494919 +INS_ACCOFFS_Z,0.009168625 +INS_ACCSCAL_X,1 +INS_ACCSCAL_Y,1 +INS_ACCSCAL_Z,1 +INS_ENABLE_MASK,127 +INS_FAST_SAMPLE,1 +INS_GYR_CAL,0 +INS_GYR_ID,2752772 +INS_GYR1_CALTEMP,-300 +INS_GYR2_CALTEMP,-300 +INS_GYR2_ID,2752780 +INS_GYR2OFFS_X,0.0002404106 +INS_GYR2OFFS_Y,0.0002287887 +INS_GYR2OFFS_Z,0.0009802132 +INS_GYR3_CALTEMP,-300 +INS_GYR3_ID,0 +INS_GYR3OFFS_X,0 +INS_GYR3OFFS_Y,0 +INS_GYR3OFFS_Z,0 +INS_GYRO_FILTER,20 +INS_GYRO_RATE,0 +INS_GYROFFS_X,-3.276218E-05 +INS_GYROFFS_Y,0.0001654653 +INS_GYROFFS_Z,0.001230893 +INS_HNTC2_ENABLE,0 +INS_HNTCH_ENABLE,0 +INS_LOG_BAT_CNT,1024 +INS_LOG_BAT_LGCT,32 +INS_LOG_BAT_LGIN,20 +INS_LOG_BAT_MASK,0 +INS_LOG_BAT_OPT,0 +INS_POS1_X,0 +INS_POS1_Y,0 +INS_POS1_Z,0 +INS_POS2_X,0 +INS_POS2_Y,0 +INS_POS2_Z,0 +INS_POS3_X,0 +INS_POS3_Y,0 +INS_POS3_Z,0 +INS_RAW_LOG_OPT,0 +INS_STILL_THRESH,0.1 +INS_TCAL_OPTIONS,0 +INS_TCAL1_ENABLE,0 +INS_TCAL2_ENABLE,0 +INS_TCAL3_ENABLE,0 +INS_TRIM_OPTION,1 +INS_USE,1 +INS_USE2,1 +INS_USE3,1 +KDE_NPOLE,14 +KFF_RDDRMIX,0.5 +KFF_THR2PTCH,0 +LAND_ABORT_DEG,0 +LAND_ABORT_THR,0 +LAND_DISARMDELAY,20 +LAND_FLAP_PERCNT,0 +LAND_FLARE_AIM,50 +LAND_FLARE_ALT,20 +LAND_FLARE_SEC,3 +LAND_OPTIONS,0 +LAND_PF_ALT,35 +LAND_PF_ARSPD,40 +LAND_PF_SEC,5 +LAND_PITCH_DEG,12 +LAND_SLOPE_RCALC,2 +LAND_THEN_NEUTRL,0 +LAND_THR_SLEW,0 +LAND_TYPE,0 +LAND_WIND_COMP,50 +LEVEL_ROLL_LIMIT,5 +LGR_ENABLE,0 +LIM_ROLL_AUTO,40 +LOG_BACKEND_TYPE,1 +LOG_BITMASK,65535 +LOG_BLK_RATEMAX,0 +LOG_DARM_RATEMAX,0 +LOG_DISARMED,1 +LOG_FILE_BUFSIZE,200 +LOG_FILE_DSRMROT,1 +LOG_FILE_MB_FREE,500 +LOG_FILE_RATEMAX,0 +LOG_FILE_TIMEOUT,5 +LOG_MAV_BUFSIZE,8 +LOG_MAV_RATEMAX,0 +LOG_MAX_FILES,500 +LOG_REPLAY,0 +MAN_EXPO_PITCH,0 +MAN_EXPO_ROLL,0 +MAN_EXPO_RUDDER,0 +MANUAL_RCMASK,0 +MIN_GROUNDSPEED,0 +MIS_OPTIONS,0 +MIS_RESTART,0 +MIS_TOTAL,0 +MIXING_GAIN,1 +MIXING_OFFSET,0 +MNT1_TYPE,0 +MNT2_TYPE,0 +MSP_OPTIONS,0 +MSP_OSD_NCELLS,0 +NAVL1_DAMPING,0.75 +NAVL1_LIM_BANK,0 +NAVL1_PERIOD,20 +NAVL1_XTRACK_I,0.02 +NET_ENABLE,0 +NET_P1_TYPE,0 +NET_P2_TYPE,0 +NET_P3_TYPE,0 +NET_P4_TYPE,0 +NMEA_MSG_EN,3 +NMEA_RATE_MS,100 +NTF_BUZZ_ON_LVL,1 +NTF_BUZZ_PIN,0 +NTF_BUZZ_TYPES,5 +NTF_BUZZ_VOLUME,100 +NTF_DISPLAY_TYPE,0 +NTF_LED_BRIGHT,3 +NTF_LED_LEN,1 +NTF_LED_OVERRIDE,0 +NTF_LED_TYPES,123079 +ONESHOT_MASK,0 +OSD_TYPE,0 +PLND_ENABLED,0 +PTCH_LIM_MAX_DEG,20 +PTCH_LIM_MIN_DEG,-20 +PTCH_RATE_D,0 +PTCH_RATE_D_FF,0 +PTCH_RATE_FF,0.414 +PTCH_RATE_FLTD,12 +PTCH_RATE_FLTE,0 +PTCH_RATE_FLTT,3 +PTCH_RATE_I,0.28 +PTCH_RATE_IMAX,1 +PTCH_RATE_NEF,0 +PTCH_RATE_NTF,0 +PTCH_RATE_P,0.3 +PTCH_RATE_PDMX,0 +PTCH_RATE_SMAX,150 +PTCH_TRIM_DEG,0 +PTCH2SRV_RLL,0.5 +PTCH2SRV_RMAX_DN,20 +PTCH2SRV_RMAX_UP,20 +PTCH2SRV_TCONST,0.7 +Q_ENABLE,0 +RALLY_INCL_HOME,0 +RALLY_LIMIT_KM,5 +RALLY_TOTAL,0 +RC_OPTIONS,32 +RC_OVERRIDE_TIME,3 +RC_PROTOCOLS,1 +RC1_DZ,30 +RC1_MAX,2000 +RC1_MIN,1000 +RC1_OPTION,0 +RC1_REVERSED,0 +RC1_TRIM,1500 +RC10_DZ,0 +RC10_MAX,1900 +RC10_MIN,1100 +RC10_OPTION,0 +RC10_REVERSED,0 +RC10_TRIM,1500 +RC11_DZ,0 +RC11_MAX,1900 +RC11_MIN,1100 +RC11_OPTION,0 +RC11_REVERSED,0 +RC11_TRIM,1500 +RC12_DZ,0 +RC12_MAX,1900 +RC12_MIN,1100 +RC12_OPTION,0 +RC12_REVERSED,0 +RC12_TRIM,1500 +RC13_DZ,0 +RC13_MAX,1900 +RC13_MIN,1100 +RC13_OPTION,0 +RC13_REVERSED,0 +RC13_TRIM,1500 +RC14_DZ,0 +RC14_MAX,1900 +RC14_MIN,1100 +RC14_OPTION,0 +RC14_REVERSED,0 +RC14_TRIM,1500 +RC15_DZ,0 +RC15_MAX,1900 +RC15_MIN,1100 +RC15_OPTION,0 +RC15_REVERSED,0 +RC15_TRIM,1500 +RC16_DZ,0 +RC16_MAX,1900 +RC16_MIN,1100 +RC16_OPTION,0 +RC16_REVERSED,0 +RC16_TRIM,1500 +RC2_DZ,30 +RC2_MAX,2000 +RC2_MIN,1000 +RC2_OPTION,0 +RC2_REVERSED,0 +RC2_TRIM,1500 +RC3_DZ,30 +RC3_MAX,2005 +RC3_MIN,1111 +RC3_OPTION,0 +RC3_REVERSED,0 +RC3_TRIM,1111 +RC4_DZ,30 +RC4_MAX,2005 +RC4_MIN,1000 +RC4_OPTION,0 +RC4_REVERSED,0 +RC4_TRIM,1500 +RC5_DZ,0 +RC5_MAX,2005 +RC5_MIN,1499 +RC5_OPTION,0 +RC5_REVERSED,0 +RC5_TRIM,2004 +RC6_DZ,0 +RC6_MAX,2000 +RC6_MIN,1000 +RC6_OPTION,0 +RC6_REVERSED,0 +RC6_TRIM,1500 +RC7_DZ,0 +RC7_MAX,2000 +RC7_MIN,1000 +RC7_OPTION,0 +RC7_REVERSED,0 +RC7_TRIM,1500 +RC8_DZ,0 +RC8_MAX,1800 +RC8_MIN,999 +RC8_OPTION,0 +RC8_REVERSED,0 +RC8_TRIM,1800 +RC9_DZ,0 +RC9_MAX,2000 +RC9_MIN,1000 +RC9_OPTION,0 +RC9_REVERSED,0 +RC9_TRIM,1000 +RCMAP_PITCH,2 +RCMAP_ROLL,1 +RCMAP_THROTTLE,3 +RCMAP_YAW,4 +RELAY1_DEFAULT,0 +RELAY1_FUNCTION,1 +RELAY1_INVERTED,0 +RELAY1_PIN,13 +RELAY2_FUNCTION,0 +RELAY3_FUNCTION,0 +RELAY4_FUNCTION,0 +RELAY5_FUNCTION,0 +RELAY6_FUNCTION,0 +RLL_RATE_D,0 +RLL_RATE_D_FF,0 +RLL_RATE_FF,0.770 +RLL_RATE_FLTD,12 +RLL_RATE_FLTE,0 +RLL_RATE_FLTT,3 +RLL_RATE_I,0.18 +RLL_RATE_IMAX,0.89 +RLL_RATE_NEF,0 +RLL_RATE_NTF,0 +RLL_RATE_P,0.05 +RLL_RATE_PDMX,0 +RLL_RATE_SMAX,150 +RLL2SRV_RMAX,40 +RLL2SRV_TCONST,1 +RNGFND_LANDING,1 +RNGFND1_ADDR,0 +RNGFND1_FUNCTION,0 +RNGFND1_GNDCLEAR,10 +RNGFND1_MAX_CM,10000 +RNGFND1_MIN_CM,10 +RNGFND1_OFFSET,0 +RNGFND1_ORIENT,25 +RNGFND1_PIN,0 +RNGFND1_POS_X,0 +RNGFND1_POS_Y,0 +RNGFND1_POS_Z,0 +RNGFND1_PWRRNG,0 +RNGFND1_RMETRIC,1 +RNGFND1_SCALING,20 +RNGFND1_STOP_PIN,-1 +RNGFND1_TYPE,1 +RNGFND2_TYPE,0 +RNGFND3_TYPE,0 +RNGFND4_TYPE,0 +RNGFND5_TYPE,0 +RNGFND6_TYPE,0 +RNGFND7_TYPE,0 +RNGFND8_TYPE,0 +RNGFND9_TYPE,0 +RNGFNDA_TYPE,0 +ROLL_LIMIT_DEG,45 +RPM1_TYPE,0 +RPM2_TYPE,0 +RSSI_TYPE,0 +RTL_ALTITUDE,100 +RTL_AUTOLAND,2 +RTL_CLIMB_MIN,0 +RTL_RADIUS,0 +RUDD_DT_GAIN,10 +RUDDER_ONLY,0 +SCALING_SPEED,49 +SCHED_DEBUG,0 +SCHED_LOOP_RATE,300 +SCHED_OPTIONS,0 +SCR_DEBUG_OPTS,0 +SCR_DIR_DISABLE,0 +SCR_ENABLE,1 +SCR_HEAP_SIZE,120000 +SCR_LD_CHECKSUM,-1 +SCR_RUN_CHECKSUM,-1 +SCR_SDEV_EN,0 +SCR_THD_PRIORITY,0 +SCR_USER1,1 +SCR_USER2,8 +SCR_USER3,0 +SCR_USER4,0 +SCR_USER5,0 +SCR_USER6,0 +SCR_VM_I_COUNT,400000 +SERIAL_PASS1,0 +SERIAL_PASS2,-1 +SERIAL_PASSTIMO,15 +SERIAL0_BAUD,115 +SERIAL0_PROTOCOL,2 +SERIAL1_BAUD,57 +SERIAL1_OPTIONS,0 +SERIAL1_PROTOCOL,2 +SERIAL2_BAUD,57 +SERIAL2_OPTIONS,0 +SERIAL2_PROTOCOL,2 +SERIAL3_BAUD,230 +SERIAL3_OPTIONS,0 +SERIAL3_PROTOCOL,5 +SERIAL4_BAUD,230 +SERIAL4_OPTIONS,0 +SERIAL4_PROTOCOL,5 +SERIAL5_BAUD,57 +SERIAL5_OPTIONS,0 +SERIAL5_PROTOCOL,-1 +SERIAL6_BAUD,57 +SERIAL6_OPTIONS,0 +SERIAL6_PROTOCOL,-1 +SERIAL7_BAUD,57 +SERIAL7_OPTIONS,0 +SERIAL7_PROTOCOL,-1 +SERVO_32_ENABLE,0 +SERVO_AUTO_TRIM,0 +SERVO_DSHOT_ESC,0 +SERVO_DSHOT_RATE,0 +SERVO_FTW_MASK,0 +SERVO_FTW_POLES,14 +SERVO_FTW_RVMASK,0 +SERVO_GPIO_MASK,0 +SERVO_RATE,50 +SERVO_RC_FS_MSK,0 +SERVO_ROB_POSMAX,4095 +SERVO_ROB_POSMIN,0 +SERVO_SBUS_RATE,50 +SERVO_VOLZ_MASK,0 +SERVO_VOLZ_RANGE,200 +SERVO1_FUNCTION,19 +SERVO1_MAX,2000 +SERVO1_MIN,1000 +SERVO1_REVERSED,0 +SERVO1_TRIM,1500 +SERVO10_FUNCTION,0 +SERVO10_MAX,1900 +SERVO10_MIN,1100 +SERVO10_REVERSED,0 +SERVO10_TRIM,1500 +SERVO11_FUNCTION,0 +SERVO11_MAX,1900 +SERVO11_MIN,1100 +SERVO11_REVERSED,0 +SERVO11_TRIM,1500 +SERVO12_FUNCTION,0 +SERVO12_MAX,1900 +SERVO12_MIN,1100 +SERVO12_REVERSED,0 +SERVO12_TRIM,1500 +SERVO13_FUNCTION,0 +SERVO13_MAX,1900 +SERVO13_MIN,1100 +SERVO13_REVERSED,0 +SERVO13_TRIM,1500 +SERVO14_FUNCTION,0 +SERVO14_MAX,1900 +SERVO14_MIN,1100 +SERVO14_REVERSED,0 +SERVO14_TRIM,1500 +SERVO15_FUNCTION,0 +SERVO15_MAX,1900 +SERVO15_MIN,1100 +SERVO15_REVERSED,0 +SERVO15_TRIM,1500 +SERVO16_FUNCTION,0 +SERVO16_MAX,1900 +SERVO16_MIN,1100 +SERVO16_REVERSED,0 +SERVO16_TRIM,1500 +SERVO2_FUNCTION,4 +SERVO2_MAX,2000 +SERVO2_MIN,1000 +SERVO2_REVERSED,1 +SERVO2_TRIM,1500 +SERVO3_FUNCTION,19 +SERVO3_MAX,2000 +SERVO3_MIN,1000 +SERVO3_REVERSED,1 +SERVO3_TRIM,1500 +SERVO4_FUNCTION,4 +SERVO4_MAX,2000 +SERVO4_MIN,1000 +SERVO4_REVERSED,1 +SERVO4_TRIM,1500 +SERVO5_FUNCTION,0 +SERVO5_MAX,2000 +SERVO5_MIN,1000 +SERVO5_REVERSED,0 +SERVO5_TRIM,1500 +SERVO6_FUNCTION,0 +SERVO6_MAX,2000 +SERVO6_MIN,1000 +SERVO6_REVERSED,1 +SERVO6_TRIM,1500 +SERVO7_FUNCTION,0 +SERVO7_MAX,2000 +SERVO7_MIN,1000 +SERVO7_REVERSED,0 +SERVO7_TRIM,1500 +SERVO8_FUNCTION,0 +SERVO8_MAX,2000 +SERVO8_MIN,1000 +SERVO8_REVERSED,0 +SERVO8_TRIM,1500 +SERVO9_FUNCTION,0 +SERVO9_MAX,1900 +SERVO9_MIN,1100 +SERVO9_REVERSED,0 +SERVO9_TRIM,1500 +SIM_ACC_FAIL_MSK,0 +SIM_ACC_FILE_RW,0 +SIM_ACC_TRIM_X,0 +SIM_ACC_TRIM_Y,0 +SIM_ACC_TRIM_Z,0 +SIM_ACC1_BIAS_X,0 +SIM_ACC1_BIAS_Y,0 +SIM_ACC1_BIAS_Z,0 +SIM_ACC1_RND,0 +SIM_ACC1_SCAL_X,0 +SIM_ACC1_SCAL_Y,0 +SIM_ACC1_SCAL_Z,0 +SIM_ACC2_BIAS_X,0 +SIM_ACC2_BIAS_Y,0 +SIM_ACC2_BIAS_Z,0 +SIM_ACC2_RND,0 +SIM_ACC2_SCAL_X,0 +SIM_ACC2_SCAL_Y,0 +SIM_ACC2_SCAL_Z,0 +SIM_ACC3_BIAS_X,0 +SIM_ACC3_BIAS_Y,0 +SIM_ACC3_BIAS_Z,0 +SIM_ACC3_RND,0 +SIM_ACC3_SCAL_X,0 +SIM_ACC3_SCAL_Y,0 +SIM_ACC3_SCAL_Z,0 +SIM_ACCEL1_FAIL,0 +SIM_ACCEL2_FAIL,0 +SIM_ACCEL3_FAIL,0 +SIM_ADSB_ALT,1000 +SIM_ADSB_COUNT,-1 +SIM_ADSB_RADIUS,10000 +SIM_ADSB_TX,0 +SIM_ADSB_TYPES,1 +SIM_ARSPD_FAIL,0 +SIM_ARSPD_FAILP,0 +SIM_ARSPD_OFS,2013 +SIM_ARSPD_PITOT,0 +SIM_ARSPD_RATIO,1.99 +SIM_ARSPD_RND,2 +SIM_ARSPD_SIGN,0 +SIM_ARSPD2_FAIL,0 +SIM_ARSPD2_FAILP,0 +SIM_ARSPD2_OFS,2013 +SIM_ARSPD2_PITOT,0 +SIM_ARSPD2_RATIO,1.99 +SIM_ARSPD2_RND,2 +SIM_ARSPD2_SIGN,0 +SIM_BAR2_DELAY,0 +SIM_BAR2_DISABLE,0 +SIM_BAR2_DRIFT,0 +SIM_BAR2_FREEZE,0 +SIM_BAR2_GLITCH,0 +SIM_BAR2_RND,0.2 +SIM_BAR2_WCF_BAK,0 +SIM_BAR2_WCF_DN,0 +SIM_BAR2_WCF_FWD,0 +SIM_BAR2_WCF_LFT,0 +SIM_BAR2_WCF_RGT,0 +SIM_BAR2_WCF_UP,0 +SIM_BAR3_DELAY,0 +SIM_BAR3_DISABLE,0 +SIM_BAR3_DRIFT,0 +SIM_BAR3_FREEZE,0 +SIM_BAR3_GLITCH,0 +SIM_BAR3_RND,0.2 +SIM_BAR3_WCF_BAK,0 +SIM_BAR3_WCF_DN,0 +SIM_BAR3_WCF_FWD,0 +SIM_BAR3_WCF_LFT,0 +SIM_BAR3_WCF_RGT,0 +SIM_BAR3_WCF_UP,0 +SIM_BARO_COUNT,1 +SIM_BARO_DELAY,0 +SIM_BARO_DISABLE,0 +SIM_BARO_DRIFT,0 +SIM_BARO_FREEZE,0 +SIM_BARO_GLITCH,0 +SIM_BARO_RND,0.2 +SIM_BARO_WCF_BAK,0 +SIM_BARO_WCF_DN,0 +SIM_BARO_WCF_FWD,0 +SIM_BARO_WCF_LFT,0 +SIM_BARO_WCF_RGT,0 +SIM_BARO_WCF_UP,0 +SIM_BATT_CAP_AH,0 +SIM_BATT_VOLTAGE,12.6 +SIM_BAUDLIMIT_EN,0 +SIM_CAN_SRV_MSK,0 +SIM_CAN_TYPE1,1 +SIM_CAN_TYPE2,1 +SIM_CLAMP_CH,0 +SIM_DRIFT_SPEED,0.05 +SIM_DRIFT_TIME,5 +SIM_EFI_TYPE,0 +SIM_ENGINE_FAIL,0 +SIM_ENGINE_MUL,1 +SIM_ESC_ARM_RPM,0 +SIM_ESC_TELEM,1 +SIM_FLOAT_EXCEPT,1 +SIM_FLOW_DELAY,0 +SIM_FLOW_ENABLE,0 +SIM_FLOW_POS_X,0 +SIM_FLOW_POS_Y,0 +SIM_FLOW_POS_Z,0 +SIM_FLOW_RATE,10 +SIM_FLOW_RND,0.05 +SIM_FTOWESC_ENA,0 +SIM_FTOWESC_POW,4095 +SIM_GND_BEHAV,-1 +SIM_GPS_ACC,0.3 +SIM_GPS_ALT_OFS,0 +SIM_GPS_BYTELOSS,0 +SIM_GPS_DISABLE,0 +SIM_GPS_DRIFTALT,0 +SIM_GPS_GLITCH_X,0 +SIM_GPS_GLITCH_Y,0 +SIM_GPS_GLITCH_Z,0 +SIM_GPS_HDG,0 +SIM_GPS_HZ,5 +SIM_GPS_JAM,0 +SIM_GPS_LAG_MS,100 +SIM_GPS_LOCKTIME,0 +SIM_GPS_LOG_NUM,0 +SIM_GPS_NOISE,0 +SIM_GPS_NUMSATS,10 +SIM_GPS_POS_X,0 +SIM_GPS_POS_Y,0 +SIM_GPS_POS_Z,0 +SIM_GPS_TYPE,1 +SIM_GPS_VERR_X,0 +SIM_GPS_VERR_Y,0 +SIM_GPS_VERR_Z,0 +SIM_GPS2_ACC,0.3 +SIM_GPS2_ALT_OFS,0 +SIM_GPS2_BYTELOS,0 +SIM_GPS2_DISABLE,1 +SIM_GPS2_DRFTALT,0 +SIM_GPS2_GLTCH_X,0 +SIM_GPS2_GLTCH_Y,0 +SIM_GPS2_GLTCH_Z,0 +SIM_GPS2_HDG,0 +SIM_GPS2_HZ,5 +SIM_GPS2_JAM,0 +SIM_GPS2_LAG_MS,100 +SIM_GPS2_LCKTIME,0 +SIM_GPS2_NOISE,0 +SIM_GPS2_NUMSATS,10 +SIM_GPS2_POS_X,0 +SIM_GPS2_POS_Y,0 +SIM_GPS2_POS_Z,0 +SIM_GPS2_TYPE,1 +SIM_GPS2_VERR_X,0 +SIM_GPS2_VERR_Y,0 +SIM_GPS2_VERR_Z,0 +SIM_GRPE_ENABLE,0 +SIM_GRPE_PIN,-1 +SIM_GRPS_ENABLE,0 +SIM_GRPS_GRAB,2000 +SIM_GRPS_PIN,-1 +SIM_GRPS_RELEASE,1000 +SIM_GRPS_REVERSE,0 +SIM_GYR_FAIL_MSK,0 +SIM_GYR_FILE_RW,0 +SIM_GYR1_BIAS_X,0 +SIM_GYR1_BIAS_Y,0 +SIM_GYR1_BIAS_Z,0 +SIM_GYR1_RND,0 +SIM_GYR1_SCALE_X,0 +SIM_GYR1_SCALE_Y,0 +SIM_GYR1_SCALE_Z,0 +SIM_GYR2_BIAS_X,0 +SIM_GYR2_BIAS_Y,0 +SIM_GYR2_BIAS_Z,0 +SIM_GYR2_RND,0 +SIM_GYR2_SCALE_X,0 +SIM_GYR2_SCALE_Y,0 +SIM_GYR2_SCALE_Z,0 +SIM_GYR3_BIAS_X,0 +SIM_GYR3_BIAS_Y,0 +SIM_GYR3_BIAS_Z,0 +SIM_GYR3_RND,0 +SIM_GYR3_SCALE_X,0 +SIM_GYR3_SCALE_Y,0 +SIM_GYR3_SCALE_Z,0 +SIM_IE24_ENABLE,0 +SIM_IE24_ERROR,0 +SIM_IE24_STATE,-1 +SIM_IMU_COUNT,2 +SIM_IMU_POS_X,0 +SIM_IMU_POS_Y,0 +SIM_IMU_POS_Z,0 +SIM_IMUT_END,45 +SIM_IMUT_FIXED,0 +SIM_IMUT_START,25 +SIM_IMUT_TCONST,300 +SIM_IMUT1_ENABLE,0 +SIM_IMUT2_ENABLE,0 +SIM_IMUT3_ENABLE,0 +SIM_INIT_ALT_OFS,0 +SIM_INIT_LAT_OFS,0 +SIM_INIT_LON_OFS,0 +SIM_INS_THR_MIN,0.1 +SIM_JSON_MASTER,0 +SIM_LED_LAYOUT,0 +SIM_LOOP_DELAY,0 +SIM_MAG_ALY_HGT,1 +SIM_MAG_ALY_X,0 +SIM_MAG_ALY_Y,0 +SIM_MAG_ALY_Z,0 +SIM_MAG_DELAY,0 +SIM_MAG_MOT_X,0 +SIM_MAG_MOT_Y,0 +SIM_MAG_MOT_Z,0 +SIM_MAG_RND,0 +SIM_MAG_SAVE_IDS,1 +SIM_MAG1_DEVID,97539 +SIM_MAG1_DIA_X,0 +SIM_MAG1_DIA_Y,0 +SIM_MAG1_DIA_Z,0 +SIM_MAG1_FAIL,0 +SIM_MAG1_ODI_X,0 +SIM_MAG1_ODI_Y,0 +SIM_MAG1_ODI_Z,0 +SIM_MAG1_OFS_X,5 +SIM_MAG1_OFS_Y,13 +SIM_MAG1_OFS_Z,-18 +SIM_MAG1_ORIENT,0 +SIM_MAG1_SCALING,1 +SIM_MAG2_DEVID,131874 +SIM_MAG2_DIA_X,0 +SIM_MAG2_DIA_Y,0 +SIM_MAG2_DIA_Z,0 +SIM_MAG2_FAIL,0 +SIM_MAG2_ODI_X,0 +SIM_MAG2_ODI_Y,0 +SIM_MAG2_ODI_Z,0 +SIM_MAG2_OFS_X,5 +SIM_MAG2_OFS_Y,13 +SIM_MAG2_OFS_Z,-18 +SIM_MAG2_ORIENT,0 +SIM_MAG2_SCALING,1 +SIM_MAG3_DEVID,263178 +SIM_MAG3_DIA_X,0 +SIM_MAG3_DIA_Y,0 +SIM_MAG3_DIA_Z,0 +SIM_MAG3_FAIL,0 +SIM_MAG3_ODI_X,0 +SIM_MAG3_ODI_Y,0 +SIM_MAG3_ODI_Z,0 +SIM_MAG3_OFS_X,5 +SIM_MAG3_OFS_Y,13 +SIM_MAG3_OFS_Z,-18 +SIM_MAG3_ORIENT,0 +SIM_MAG3_SCALING,1 +SIM_MAG4_DEVID,97283 +SIM_MAG5_DEVID,97795 +SIM_MAG6_DEVID,98051 +SIM_MAG7_DEVID,0 +SIM_MAG8_DEVID,0 +SIM_ODOM_ENABLE,0 +SIM_OH_MASK,0 +SIM_OH_RELAY_MSK,-1 +SIM_OPOS_ALT,584 +SIM_OPOS_HDG,353 +SIM_OPOS_LAT,-35.36326 +SIM_OPOS_LNG,149.1652 +SIM_PARA_ENABLE,0 +SIM_PARA_PIN,-1 +SIM_PIN_MASK,2 +SIM_PLD_ALT_LMT,15 +SIM_PLD_DIST_LMT,10 +SIM_PLD_ENABLE,0 +SIM_PLD_HEIGHT,0 +SIM_PLD_LAT,0 +SIM_PLD_LON,0 +SIM_PLD_OPTIONS,0 +SIM_PLD_ORIENT,24 +SIM_PLD_RATE,100 +SIM_PLD_SHIP,0 +SIM_PLD_TYPE,0 +SIM_PLD_YAW,0 +SIM_RATE_HZ,1200 +SIM_RC_CHANCOUNT,16 +SIM_RC_FAIL,0 +SIM_RICH_CTRL,-1 +SIM_RICH_ENABLE,0 +SIM_SERVO_DELAY,0 +SIM_SERVO_FILTER,0 +SIM_SERVO_SPEED,0.15 +SIM_SHIP_DSIZE,10 +SIM_SHIP_ENABLE,0 +SIM_SHIP_OFS_X,0 +SIM_SHIP_OFS_Y,0 +SIM_SHIP_OFS_Z,0 +SIM_SHIP_PSIZE,1000 +SIM_SHIP_SPEED,3 +SIM_SHIP_SYSID,17 +SIM_SHOVE_TIME,0 +SIM_SHOVE_X,0 +SIM_SHOVE_Y,0 +SIM_SHOVE_Z,0 +SIM_SLUP_ENABLE,0 +SIM_SONAR_GLITCH,0 +SIM_SONAR_POS_X,0 +SIM_SONAR_POS_Y,0 +SIM_SONAR_POS_Z,0 +SIM_SONAR_RND,0 +SIM_SONAR_ROT,25 +SIM_SONAR_SCALE,12.1212 +SIM_SPEEDUP,1 +SIM_SPR_ENABLE,0 +SIM_SPR_PUMP,-1 +SIM_SPR_SPIN,-1 +SIM_TA_ENABLE,1 +SIM_TEMP_BFACTOR,0 +SIM_TEMP_BRD_OFF,20 +SIM_TEMP_START,25 +SIM_TEMP_TCONST,30 +SIM_TERRAIN,1 +SIM_THML_SCENARI,0 +SIM_TIDE_DIR,0 +SIM_TIDE_SPEED,0 +SIM_TIME_JITTER,0 +SIM_TWIST_TIME,0 +SIM_TWIST_X,0 +SIM_TWIST_Y,0 +SIM_TWIST_Z,0 +SIM_UART_LOSS,0 +SIM_VIB_FREQ_X,0 +SIM_VIB_FREQ_Y,0 +SIM_VIB_FREQ_Z,0 +SIM_VIB_MOT_HMNC,1 +SIM_VIB_MOT_MASK,0 +SIM_VIB_MOT_MAX,0 +SIM_VIB_MOT_MULT,1 +SIM_VICON_FAIL,0 +SIM_VICON_GLIT_X,0 +SIM_VICON_GLIT_Y,0 +SIM_VICON_GLIT_Z,0 +SIM_VICON_POS_X,0 +SIM_VICON_POS_Y,0 +SIM_VICON_POS_Z,0 +SIM_VICON_TMASK,3 +SIM_VICON_VGLI_X,0 +SIM_VICON_VGLI_Y,0 +SIM_VICON_VGLI_Z,0 +SIM_VICON_YAW,0 +SIM_VICON_YAWERR,0 +SIM_WAVE_AMP,0.5 +SIM_WAVE_DIR,0 +SIM_WAVE_ENABLE,0 +SIM_WAVE_LENGTH,10 +SIM_WAVE_SPEED,0.5 +SIM_WIND_DIR,180 +SIM_WIND_DIR_Z,0 +SIM_WIND_SPD,0 +SIM_WIND_T,0 +SIM_WIND_T_ALT,60 +SIM_WIND_T_COEF,0.01 +SIM_WIND_TC,5 +SIM_WIND_TURB,0 +SIM_WOW_PIN,-1 +SOAR_ENABLE,0 +SR0_ADSB,4 +SR0_EXT_STAT,4 +SR0_EXTRA1,4 +SR0_EXTRA2,4 +SR0_EXTRA3,4 +SR0_PARAMS,10 +SR0_POSITION,4 +SR0_RAW_CTRL,4 +SR0_RAW_SENS,4 +SR0_RC_CHAN,4 +SR1_ADSB,5 +SR1_EXT_STAT,1 +SR1_EXTRA1,1 +SR1_EXTRA2,1 +SR1_EXTRA3,1 +SR1_PARAMS,10 +SR1_POSITION,1 +SR1_RAW_CTRL,1 +SR1_RAW_SENS,1 +SR1_RC_CHAN,1 +SR2_ADSB,5 +SR2_EXT_STAT,1 +SR2_EXTRA1,1 +SR2_EXTRA2,1 +SR2_EXTRA3,1 +SR2_PARAMS,10 +SR2_POSITION,1 +SR2_RAW_CTRL,1 +SR2_RAW_SENS,1 +SR2_RC_CHAN,1 +SR3_ADSB,5 +SR3_EXT_STAT,1 +SR3_EXTRA1,1 +SR3_EXTRA2,1 +SR3_EXTRA3,1 +SR3_PARAMS,10 +SR3_POSITION,1 +SR3_RAW_CTRL,1 +SR3_RAW_SENS,1 +SR3_RC_CHAN,1 +SR4_ADSB,5 +SR4_EXT_STAT,1 +SR4_EXTRA1,1 +SR4_EXTRA2,1 +SR4_EXTRA3,1 +SR4_PARAMS,10 +SR4_POSITION,1 +SR4_RAW_CTRL,1 +SR4_RAW_SENS,1 +SR4_RC_CHAN,1 +SR5_ADSB,5 +SR5_EXT_STAT,1 +SR5_EXTRA1,1 +SR5_EXTRA2,1 +SR5_EXTRA3,1 +SR5_PARAMS,10 +SR5_POSITION,1 +SR5_RAW_CTRL,1 +SR5_RAW_SENS,1 +SR5_RC_CHAN,1 +SR6_ADSB,5 +SR6_EXT_STAT,1 +SR6_EXTRA1,1 +SR6_EXTRA2,1 +SR6_EXTRA3,1 +SR6_PARAMS,10 +SR6_POSITION,1 +SR6_RAW_CTRL,1 +SR6_RAW_SENS,1 +SR6_RC_CHAN,1 +STAB_PITCH_DOWN,0 +STALL_PREVENTION,0 +STEER2SRV_D,0.005 +STEER2SRV_DRTFCT,10 +STEER2SRV_DRTMIN,4500 +STEER2SRV_DRTSPD,0 +STEER2SRV_FF,0 +STEER2SRV_I,0.2 +STEER2SRV_IMAX,1500 +STEER2SRV_MINSPD,1 +STEER2SRV_P,1.8 +STEER2SRV_TCONST,0.75 +STICK_MIXING,0 +SYSID_ENFORCE,0 +SYSID_MYGCS,255 +SYSID_THISMAV,1 +TECS_APPR_SMAX,0 +TECS_CLMB_MAX,5 +TECS_FLARE_HGT,1 +TECS_HDEM_TCONST,3 +TECS_HGT_OMEGA,3 +TECS_INTEG_GAIN,0.1 +TECS_LAND_ARSPD,49.17 +TECS_LAND_DAMP,0.5 +TECS_LAND_IGAIN,0 +TECS_LAND_PDAMP,0 +TECS_LAND_PMAX,20 +TECS_LAND_SINK,1 +TECS_LAND_SPDWGT,1 +TECS_LAND_SRC,0.2 +TECS_LAND_TCONST,5 +TECS_LAND_TDAMP,0 +TECS_LAND_THR,-1 +TECS_OPTIONS,1 +TECS_PITCH_MAX,-4 +TECS_PITCH_MIN,-16 +TECS_PTCH_DAMP,0 +TECS_PTCH_FF_K,0 +TECS_PTCH_FF_V0,12 +TECS_RLL2THR,10 +TECS_SINK_MAX,15 +TECS_SINK_MIN,10 +TECS_SPD_OMEGA,2 +TECS_SPDWEIGHT,1 +TECS_SYNAIRSPEED,0 +TECS_THR_DAMP,0.5 +TECS_TIME_CONST,5 +TECS_TKOFF_IGAIN,0 +TECS_VERT_ACC,7 +TELEM_DELAY,0 +TEMP_LOG,0 +TEMP1_TYPE,0 +TEMP2_TYPE,0 +TEMP3_TYPE,0 +TERRAIN_CACHE_SZ,12 +TERRAIN_ENABLE,1 +TERRAIN_FOLLOW,0 +TERRAIN_LOOKAHD,2000 +TERRAIN_MARGIN,0.05 +TERRAIN_OFS_MAX,30 +TERRAIN_OPTIONS,0 +TERRAIN_SPACING,100 +THR_FAILSAFE,2 +THR_FS_VALUE,890 +THR_MAX,1 +THR_MIN,0 +THR_PASS_STAB,0 +THR_SLEWRATE,100 +THR_SUPP_MAN,0 +THROTTLE_NUDGE,0 +TKOFF_ACCEL_CNT,1 +TKOFF_ALT,50 +TKOFF_DIST,200 +TKOFF_FLAP_PCNT,0 +TKOFF_GND_PITCH,5 +TKOFF_LVL_ALT,20 +TKOFF_LVL_PITCH,15 +TKOFF_OPTIONS,0 +TKOFF_PLIM_SEC,2 +TKOFF_ROTATE_SPD,0 +TKOFF_TDRAG_ELEV,0 +TKOFF_TDRAG_SPD1,0 +TKOFF_THR_DELAY,2 +TKOFF_THR_MAX,0 +TKOFF_THR_MAX_T,4 +TKOFF_THR_MIN,60 +TKOFF_THR_MINACC,0 +TKOFF_THR_MINSPD,0 +TKOFF_THR_SLEW,0 +TKOFF_TIMEOUT,0 +TRIM_THROTTLE,45 +TUNE_CHAN,0 +TUNE_CHAN_MAX,2000 +TUNE_CHAN_MIN,1000 +TUNE_ERR_THRESH,0.15 +TUNE_MODE_REVERT,1 +TUNE_PARAM,0 +TUNE_RANGE,2 +TUNE_SELECTOR,0 +USE_REV_THRUST,2 +VISO_TYPE,0 +VTX_ENABLE,0 +WP_LOITER_RAD,80 +WP_MAX_RADIUS,0 +WP_RADIUS,400 +YAW_RATE_ENABLE,0 +YAW2SRV_DAMP,0 +YAW2SRV_IMAX,1500 +YAW2SRV_INT,0 +YAW2SRV_RLL,1 +YAW2SRV_SLIP,0 diff --git a/Tools/autotest/models/sa_gd2000_xplane.json b/Tools/autotest/models/sa_gd2000_xplane.json new file mode 100644 index 00000000000000..5ca2d847da6073 --- /dev/null +++ b/Tools/autotest/models/sa_gd2000_xplane.json @@ -0,0 +1,15 @@ +# XPlane DREF map file for a simple plane +# assumes: Aileron, Elevator, Throttle, Rudder, flaps + +{ + "settings" : { "debug" : 0 }, + + "sim/operation/override/override_control_surfaces" : { "type" : "fixed", "value" : 1 }, + + # ailerons + "sim/flightmodel2/wing/elevator1_deg[12]" : { "type" : "angle", "range" : 20, "channel" : 4 }, # right rear + "sim/flightmodel2/wing/elevator1_deg[13]" : { "type" : "angle", "range" : -20, "channel" : 2 }, # left rear + "sim/flightmodel2/wing/elevator1_deg[14]" : { "type" : "angle", "range" : 20, "channel" : 1 }, # front right + "sim/flightmodel2/wing/elevator1_deg[15]" : { "type" : "angle", "range" : -20, "channel" : 3 } # front left + +} diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index 2a49de12bea9fa..c170ea2dfb8fb0 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/libraries/SITL/SIM_Plane.h @@ -59,7 +59,7 @@ class Plane : public Aircraft { float alpha_stall = 0.4712; float c_drag_q = 0; float c_drag_deltae = 0.0; - float c_drag_p = 0.1; + float c_drag_p = 0.0025; float c_y_0 = 0; float c_y_b = -0.98; float c_y_p = 0; diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index 3633bb4b1a7394..3c38365a2da41c 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -40,7 +40,7 @@ extern const AP_HAL::HAL& hal; #if APM_BUILD_TYPE(APM_BUILD_Heli) #define XPLANE_JSON "xplane_heli.json" #else -#define XPLANE_JSON "xplane_plane.json" +#define XPLANE_JSON "sa_gd2000_xplane.json" #endif // DATA@ frame types. Thanks to TauLabs xplanesimulator.h @@ -103,6 +103,8 @@ XPlane::XPlane(const char *frame_str) : printf("Waiting for XPlane data on UDP port %u and sending to port %u\n", (unsigned)bind_port, (unsigned)xplane_port); + AP_Param::load_defaults_file("@ROMFS/models/sa_gd2000.parm", false); + // XPlane sensor data is not good enough for EKF. Use fake EKF by default AP_Param::set_default_by_name("AHRS_EKF_TYPE", 10); AP_Param::set_default_by_name("GPS1_TYPE", 100); diff --git a/scripts/droptest.lua b/scripts/droptest.lua new file mode 100644 index 00000000000000..9941f03d9329eb --- /dev/null +++ b/scripts/droptest.lua @@ -0,0 +1,829 @@ +-- control SilentArrow drop + +-- de-glitch lidar range for emergency landing +-- add default params in ROMFS + +-- tests to run +-- check on loss of GPS, that we switch to pitot airspeed + +done_init = false +local button_number = 1 +local button_state = 0 + +local MODE_MANUAL = 0 +local MODE_AUTO = 10 + +local LANDING_AMSL = 430.0 +local RUNWAY_LENGTH = 1.0 +local CHANGE_MARGIN = 100.0 + +local GLIDE_SLOPE = 6.0 + +local MAX_WIND = 10.0 + +local SMALL_WP_DIST = 500 + +-- expected height loss for a 180 degree turn, meters +local TURN_HEIGHT_LOSS = 180 + +local release_start_t = 0.0 +local last_tick_t = 0.0 +local last_mission_update_t = 0.0 +local last_mission_setup_t = 0.0 +local last_wp_change_t = 0.0 +local release_t = 0 + +local ENABLE_LOGGING = false +local logging_init = false + + +function logit(txt) + if not ENABLE_LOGGING then + return + end + + if (logging_init == false) then + logfile = io.open("log.txt", "w") + logging_init = true + end + + logfile:write(txt .. "\n") + logfile:flush() +end + +DO_JUMP = 177 +NAV_WAYPOINT = 16 +NAV_LAND = 21 +DO_LAND_START = 189 + +local APPROACH_DIST_MAX = 6400 +local BASE_DIST = 1390 +local STEP_RATIO = 0.5 +-- see if we are running on SITL +local is_SITL = param:get('SIM_SPEEDUP') + +local TARGET_AIRSPEED = param:get('AIRSPEED_CRUISE') + +function get_glide_slope() + GLIDE_SLOPE = param:get('SCR_USER2') + if GLIDE_SLOPE <= 0 then + GLIDE_SLOPE = 6.0 + end +end + +-- fill in LANDING_AMSL, height of first landing point in mission +function get_landing_AMSL() + local N = mission:num_commands() + for i = 1, N-1 do + local m = mission:get_item(i) + if m:command() == NAV_LAND then + local loc = get_location(i) + ahrs:set_home(loc) + LANDING_AMSL = m:z() + return + end + end +end + +-- return ground course in degrees +function ground_course() + if gps:status(0) >= 3 and gps:ground_speed(0) > 20 then + return gps:ground_course(0) + end + if gps:num_sensors() >= 2 and gps:status(1) >= 3 and gps:ground_speed(1) > 20 then + return gps:ground_course(1) + end + -- use mission direction + local cnum = mission:get_current_nav_index() + local N = mission:num_commands() + if cnum > N-1 then + return 0.0 + end + if mission:get_item(cnum):command() == NAV_WAYPOINT then + local loc1 = get_position() + local loc2 = get_location(cnum+1) + return math.deg(loc1:get_bearing(loc2)) + end + return 0.0 +end + +function feet(m) + return m * 3.2808399 +end + +function wrap_180(angle) + if angle > 180 then + angle = angle - 360.0 + end + return angle +end + +function right_direction(cnum) + local loc = get_position() + if loc == nil then + return false + end + local loc2 = get_location(cnum) + local gcrs = wrap_180(ground_course()) + local gcrs2 = wrap_180(math.deg(loc:get_bearing(loc2))) + local err = wrap_180(gcrs2 - gcrs) + local dist = loc:get_distance(loc2) + if dist < 100 then + -- too close + return false + end + if math.abs(err) > 60 then + return false + end + return true +end + +function resolve_jump(i) + local m = mission:get_item(i) + while m:command() == DO_JUMP do + i = math.floor(m:param1()) + m = mission:get_item(i) + end + return i +end + +-- return true if cnum is a candidate for wp selection +function is_candidate(cnum) + local N = mission:num_commands() + if cnum > N-3 then + return false + end + m = mission:get_item(cnum) + m2 = mission:get_item(cnum+1) + m3 = mission:get_item(cnum+2) + if m:command() == NAV_WAYPOINT and m2:command() == NAV_WAYPOINT and (m3:command() == DO_JUMP or m3:command() == NAV_WAYPOINT or m3:command() == NAV_LAND) then + return true + end + return false +end + + +-- return true if cnum is a candidate for wp change +function is_change_candidate(cnum) + logit(string.format('is_change_candidate(%d)', cnum)) + + if is_candidate(cnum) then + logit(string.format(' YES -> is_candidate')) + return true + end + local loc = ahrs:get_position() + local m1 = mission:get_item(cnum) + if m1:command() ~= NAV_WAYPOINT then + -- only change when navigating to a WP + logit(string.format(' NO -> not WP')) + return false + end + if loc:alt() * 0.01 - LANDING_AMSL > 1000 then + -- if we have lots of height then we can change + logit(string.format(' YES -> plenty of height')) + return true + end + local loc2 = get_location(cnum) + if loc:get_distance(loc2) < SMALL_WP_DIST then + -- if we are within 1.5km then no change + logit(string.format(' NO -> within %.0f', SMALL_WP_DIST)) + return false + end + if loc:get_distance(loc2) > 3500 then + -- if a long way from target allow change + logit(string.format(' YES -> long way')) + return true + end + if cnum > mission:num_commands()-2 then + logit(string.format(' NO -> num cmds')) + return false + end + local m2 = mission:get_item(cnum+1) + if m1:command() == NAV_WAYPOINT and m2:command() ~= NAV_LAND then + -- allow change of cross WPs when more than 2km away + logit(string.format(' YES -> is land')) + return true + end + logit(string.format(' NO -> default')) + return false +end + +function get_location(i) + local m = mission:get_item(i) + local loc = Location() + loc:lat(m:x()) + loc:lng(m:y()) + loc:relative_alt(false) + loc:terrain_alt(false) + loc:origin_alt(false) + loc:alt(math.floor(m:z()*100)) + return loc +end + +function get_position() + local loc = ahrs:get_position() + if not loc then + loc = gps:location(0) + end + if not loc and gps:num_sensors() >= 2 then + loc = gps:location(1) + end + if not loc then + return nil + end + return loc +end + +-- vector2 cross product +function vec2_cross(vec1, vec2) + return vec1:x()*vec2:y() - vec1:y()*vec2:x() +end + +-- vector2 dot product +function vec2_dot(vec1, vec2) + return vec1:x()*vec2:x() + vec1:y()*vec2:y() +end + +function constrain(v, minv, maxv) + if v < minv then + v = minv + end + if v > maxv then + v = maxv + end + return v +end + +-- calculate wind adjustment for a WP segment +function wind_adjustment(loc1, loc2) + local distNE = loc1:get_distance_NE(loc2) + local dist = distNE:length() + if dist < 1 then + return 0.0 + end + distNE:normalize() + local wind3d = ahrs:wind_estimate() + + -- get 2d wind + local wind2d = Vector2f() + wind2d:x(wind3d:x()) + wind2d:y(wind3d:y()) + + -- dot product gives component along flight path + local dot = vec2_dot(distNE, wind2d) + dot = constrain(dot, -MAX_WIND, MAX_WIND) + + local change = -dot / TARGET_AIRSPEED + return dist * change +end + +function turn_adjustment(bearing_change_deg) + local height_loss = TURN_HEIGHT_LOSS * math.abs(bearing_change_deg / 180.0) + return height_loss * GLIDE_SLOPE +end + +-- get distance to landing point, ignoring current location +function distance_to_land_nopos(cnum) + local N = mission:num_commands() + if cnum >= N then + return -1 + end + local distance = 0 + local i = cnum + local last_bearing = 0 + while i < N do + m = mission:get_item(i) + if m:command() == NAV_LAND then + break + end + local i2 = resolve_jump(i+1) + local loc1 = get_location(i) + local loc2 = get_location(i2) + + local d1 = loc1:get_distance(loc2) + + distance = distance + d1 + + -- gcs:send_text(0, string.format("WP%u->WP%u d=%.0f dist=%.0f", i, i2, d1, distance)) + + -- account for height lost in turns + local bearing = wrap_180(math.deg(loc1:get_bearing(loc2))) + if i == cnum then + last_bearing = bearing + end + local bearing_change = math.abs(wrap_180(bearing - last_bearing)) + last_bearing = bearing + distance = distance + turn_adjustment(bearing_change) + + -- account for wind + distance = distance + wind_adjustment(loc1, loc2) + + i = i2 + end + -- subtract half runway length, for ideal landing mid-runway + return distance - RUNWAY_LENGTH * 0.5 +end + +-- get distance to landing point, with current location +function distance_to_land(cnum) + local loc = get_position() + local N = mission:num_commands() + if cnum >= N then + return -1 + end + local loc1 = get_location(cnum) + local distance = loc:get_distance(loc1) + distance = distance + wind_adjustment(loc, loc1) + + local gcrs = wrap_180(ground_course()) + local wpcrs = wrap_180(math.deg(loc:get_bearing(loc1))) + local bearing_error = math.abs(wrap_180(gcrs - wpcrs)) + + distance = distance + turn_adjustment(bearing_error) + + return distance + distance_to_land_nopos(cnum) +end + +-- see if we are on the optimal waypoint number for landing +-- given the assumed glide slope +function mission_update() + logit("mission_update()") + local cnum = mission:get_current_nav_index() + if cnum <= 0 then + logit("no mission") + -- not flying mission yet + return + end + local m = mission:get_item(cnum) + if not m then + logit("invalid mission") + -- invalid mission? + gcs:send_text(0, string.format("Invalid current cnum %u", cnum)) + return + end + if m:command() ~= NAV_WAYPOINT then + -- only update when tracking to a waypoint (not LAND) + logit("not NAV_WAYPOINT") + return + end + local loc = get_position() + if loc == nil then + logit("no position") + return + end + local current_distance = distance_to_land(cnum) + local current_alt = loc:alt() * 0.01 - LANDING_AMSL + local avail_dist = GLIDE_SLOPE * current_alt + local current_slope = current_distance / current_alt + local current_err = current_distance - avail_dist + local original_err = current_err + gcs:send_text(0, string.format("cnum=%u dist=%.0f alt=%.0f slope=%.2f err=%.0f", + cnum, feet(current_distance), feet(current_alt), current_slope, + feet(-current_err))) + logit(string.format("cnum=%u dist=%.0f alt=%.0f slope=%.2f err=%.0f", + cnum, feet(current_distance), feet(current_alt), current_slope, + feet(-current_err))) + + local current_wp_loc = get_location(cnum) + local current_wp_dist = loc:get_distance(current_wp_loc) + if current_wp_dist < SMALL_WP_DIST then + -- when within 1.5km of current wp consider moving to next WP + if mission:get_item(cnum+1):command() == NAV_WAYPOINT then + -- check if we should skip the current WP and move to the next WP + local loc1 = get_position() + local loc2 = get_location(cnum) + local loc3 = get_location(cnum+1) + local dist = loc1:get_distance(loc2) + local dist_change = loc2:get_distance(loc3) + local target_bearing = math.deg(loc1:get_bearing(loc2)) + local target_bearing2 = math.deg(loc1:get_bearing(loc3)) + local ang_change = math.abs(target_bearing - target_bearing2) + if ang_change < 10 and dist < 1000 and dist_change < 1000 then + gcs:send_text(0, string.format("Skip NEW WP %u ang=%.0f dc=%.0f", cnum+1, ang_change, dist_change)) + mission:set_current_cmd(cnum+1) + return true + end + end + logit(string.format("wp dist %.2f", current_wp_dist)) + return false + end + + if not is_change_candidate(cnum) then + -- only change if on a candidate now + logit("not candidate") + return false + end + + local t = 0.001 * millis():tofloat() + if t - last_wp_change_t < 5 then + -- don't change too rapidly + return + end + + -- look for alternatives + local N = mission:num_commands() + local best = cnum + for i = 1, N-3 do + if is_candidate(i) and right_direction(i) then + -- this is an alternative path + local dist = distance_to_land(i) + local err = dist - avail_dist + local diff = math.abs(current_err) - math.abs(err) + logit(string.format(" i=%u dist=%.0f err=%.0f current_err=%.0f diff=%.0f", i, dist, err, current_err, diff)) + if diff > CHANGE_MARGIN then + best = i + current_err = err + end + end + end + improvement = math.abs(current_err) - math.abs(original_err) + if cnum ~= best then + gcs:send_text(0, string.format("NEW WP %u err=%.0f imp=%.0f", best, current_err, improvement)) + logit(string.format("NEW WP %u err=%.0f imp=%.0f", best, current_err, improvement)) + mission:set_current_cmd(best) + last_wp_change_t = t + end +end + +function release_trigger() + gcs:send_text(0, string.format("release trigger")) + vehicle:set_mode(MODE_AUTO) + arming:arm_force() + if mission:num_commands() > 2 then + mission:set_current_cmd(2) -- ? + end + notify:handle_rgb(0,255,0,0) +end + +function set_standby() + local mode = vehicle:get_mode() + if mode ~= MODE_MANUAL then + gcs:send_text(0, string.format("forcing standby MANUAL")) + vehicle:set_mode(MODE_MANUAL) + arming:disarm() + mission:set_current_cmd(1) + -- maybe LOITER mode if no GPS lock? + end + local gps_status = gps:status(0) + if gps_status == 0 and gps:num_sensors() >= 2 then + gps_status = gps:status(1) + end + if gps_status == 0 then + -- red flash slow for no GPS + notify:handle_rgb(255,0,0,2) + elseif mission:num_commands() <= 2 then + -- red flash fast means no mission + notify:handle_rgb(255,0,0,10) + elseif gps_status >= 3 then + -- green blinking 3D lock, manual + notify:handle_rgb(0,255,0,2) + else + -- flashing blue for no 3D lock + notify:handle_rgb(0,0,255,2) + end + local t = 0.001 * millis():tofloat() + if t - last_tick_t > 10 then + last_tick_t = t + gcs:send_text(0, string.format("Drop: MANUAL idle ")) + end +end + +function fix_WP_heights() + gcs:send_text(0, string.format("Fixing WP heights")) + local N = mission:num_commands() + for i = 1, N-1 do + local m = mission:get_item(i) + if m:command() == NAV_WAYPOINT then + local dist = distance_to_land_nopos(i) + local current_alt = m:z() + local new_alt = LANDING_AMSL + dist / GLIDE_SLOPE + if math.abs(current_alt - new_alt) > -1 or m:frame() ~= 0 then + gcs:send_text(0, string.format("Fixing WP[%u] d=%.0f alt %.1f->%.1f", i, dist, current_alt, new_alt)) + m:z(new_alt) + m:frame(0) + mission:set_item(i, m) + end + end + end +end +-- see if mission is setup for auto-creation +function create_mission_check() + if mission:num_commands() ~= 4 then + return false + end + if mission:get_item(1):command() ~= NAV_LAND or mission:get_item(2):command() ~= NAV_LAND then + return false + end + if mission:get_item(3):command() ~= DO_LAND_START then + return false + end + return true +end + +function loc_copy(loc) + local loc2 = Location() + loc2:lat(loc:lat()) + loc2:lng(loc:lng()) + loc2:alt(loc:alt()) + return loc2 +end + +-- set wp location +function wp_setloc(wp, loc) + wp:x(loc:lat()) + wp:y(loc:lng()) + wp:z(loc:alt()*0.01) +end + +-- get wp location +function wp_getloc(wp) + local loc = Location() + loc:lat(wp:x()) + loc:lng(wp:y()) + loc:alt(math.floor(wp:z()*100)) + return loc +end + +-- shift a wp in polar coordinates +function wp_offset(wp, ang1, dist1) + local loc = wp_getloc(wp) + loc:offset_bearing(ang1,dist1) + wp:x(loc:lat()) + wp:y(loc:lng()) +end + +-- copy and shift a location +function loc_shift(loc,angle,dist) + local loc2 = Location() + loc2:lat(loc:lat()) + loc2:lng(loc:lng()) + loc2:alt(loc:alt()) + loc2:offset_bearing(angle,dist) + return loc2 +end + +-- add a waypoint to the end +function wp_add(loc,ctype,param1,param2) + local wp = mavlink_mission_item_int_t() + wp_setloc(wp,loc) + wp:command(ctype) + local seq = mission:num_commands() + wp:seq(seq) + wp:param1(param1) + wp:param2(param2) + mission:set_item(seq,wp) +end + +-- adjust approach distance based on glide slope and location of LAND1 +-- and DO_LAND_START +function adjust_approach_dist(land1_loc, dls_loc, angle) + local alt_change = (dls_loc:alt() - land1_loc:alt()) * 0.01 + local target_dist = 1.3 * GLIDE_SLOPE * alt_change + gcs:send_text(0, string.format("alt_change=%.2f target_dist=%.2f", alt_change, target_dist)) + + -- converge on correct approach distance + local dist + for i = 1, 10 do + local loc2 = loc_shift(land1_loc, angle, APPROACH_DIST_MAX) + dist = land1_loc:get_distance(loc2) + loc2:get_distance(dls_loc) + APPROACH_DIST_MAX = APPROACH_DIST_MAX * target_dist / dist + end + gcs:send_text(0, string.format("app_dist=%.2f dist=%.2f target_dist=%.2f", APPROACH_DIST_MAX, dist, target_dist)) +end + +function create_pattern(wp, basepos, angle, base_angle, runway_length) + + local jump_target = mission:num_commands() + 3 + + wp:command(NAV_WAYPOINT) + local loc = loc_copy(basepos) + loc:offset_bearing(angle,APPROACH_DIST_MAX) + loc:offset_bearing(base_angle,BASE_DIST) + wp_add(loc,NAV_WAYPOINT,0,0) + + loc = loc_copy(basepos) + loc:offset_bearing(angle,APPROACH_DIST_MAX) + wp_add(loc,NAV_WAYPOINT,0,0) + + loc = loc_copy(basepos) + loc:offset_bearing(angle,runway_length*1.2) + wp_add(loc,NAV_WAYPOINT,0,0) + + loc = loc_copy(basepos) + wp_add(loc,NAV_LAND,0,0) + + local step = runway_length*STEP_RATIO + NUM_ALTERNATES = math.min(70,math.floor((APPROACH_DIST_MAX - runway_length*1.5) / step)) + + gcs:send_text(0, string.format("NUM_ALTERNATES=%u", NUM_ALTERNATES)) + + for i = 1, NUM_ALTERNATES do + loc = Location() + wp_add(loc,DO_JUMP,jump_target,-1) + + loc = loc_copy(basepos) + loc:offset_bearing(angle,APPROACH_DIST_MAX - step*i) + loc:offset_bearing(base_angle,BASE_DIST) + wp_add(loc,NAV_WAYPOINT,0,0) + + loc = loc_copy(basepos) + loc:offset_bearing(angle,APPROACH_DIST_MAX - step*i) + wp_add(loc,NAV_WAYPOINT,0,0) + end + + loc = Location() + wp_add(loc,DO_JUMP,jump_target,-1) +end + +-- auto-create mission +function create_mission() + gcs:send_text(0, string.format("creating mission")) + local land1_loc = get_location(1) + local land2_loc = get_location(2) + local dls = mission:get_item(3) + local dls_loc = get_location(3) + local runway_length = land1_loc:get_distance(land2_loc) + + local alt_agl = (dls_loc:alt() - land1_loc:alt()) * 0.01 + if alt_agl <= 0 then + gcs:send_text(0, string.format("invalid altitudes")) + return + end + + -- refresh glide slope + get_glide_slope() + + local flight_dist1 = dls_loc:get_distance(land1_loc) + local flight_dist2 = dls_loc:get_distance(land2_loc) + local slope1 = flight_dist1 / alt_agl + local slope2 = flight_dist2 / alt_agl + + if slope1 > GLIDE_SLOPE then + gcs:send_text(0, string.format("bad glide slope1 %.2f", slope1)) + return + end + if slope2 > GLIDE_SLOPE then + gcs:send_text(0, string.format("bad glide slope2 %.2f", slope2)) + return + end + + + local bearing12 = math.deg(land1_loc:get_bearing(land2_loc)) + local bearing21 = math.deg(land2_loc:get_bearing(land1_loc)) + local base_angle1 = bearing12 + 90 + local base_angle2 = bearing12 - 90 + + -- work out which base angle brings us closer to the DLS + local base_dist1 = loc_shift(land1_loc,base_angle1,10):get_distance(dls_loc) + local base_dist2 = loc_shift(land1_loc,base_angle2,10):get_distance(dls_loc) + if base_dist1 < base_dist2 then + base_angle = base_angle1 + else + base_angle = base_angle2 + end + + adjust_approach_dist(land1_loc, dls_loc, bearing12) + APPROACH_DIST_MAX = math.max(APPROACH_DIST_MAX, 3*runway_length) + + local wp = mission:get_item(1) + wp:command(NAV_WAYPOINT) + + mission:clear() + + -- setup home + wp_add(dls_loc,NAV_WAYPOINT,0,0) + + -- setup DLS1 + wp_add(dls_loc,DO_LAND_START,0,0) + + -- first pattern + create_pattern(wp, land1_loc, bearing12, base_angle, runway_length) + + adjust_approach_dist(land2_loc, dls_loc, bearing21) + APPROACH_DIST_MAX = math.max(APPROACH_DIST_MAX, 3*runway_length) + + -- setup DLS2 + wp_add(dls_loc,DO_LAND_START,0,0) + + -- second pattern + create_pattern(wp, land2_loc, bearing21, base_angle, runway_length) + + fix_WP_heights() +end + +-- init system +function init() + button_state = button:get_button_state(button_number) + get_landing_AMSL() + get_glide_slope() + + gcs:send_text(0, string.format("LANDING_AMSL %.1f GLIDE_SLOPE %.1f", LANDING_AMSL, GLIDE_SLOPE)) + + fix_WP_heights() + done_init = true +end + +--[[ + airspeed schedule in 30s steps, stops on final airspeed +--]] +local AIRSPEED_SCHEDULE_MPH = { + 140, + 135, + 130, + 125, + 120, + 130 +} + +function airspeed_update(dt) + local stage = math.floor(dt / 30.0) + stage = math.min(stage, #AIRSPEED_SCHEDULE_MPH-1) + local spd_mph = AIRSPEED_SCHEDULE_MPH[stage+1] + local spd_mps = spd_mph * 0.44704 + if TARGET_AIRSPEED ~= spd_mps then + gcs:send_text(0, string.format("Target airspeed %.1f mph", spd_mph)) + param:set('AIRSPEED_CRUISE', spd_mps) + TARGET_AIRSPEED = spd_mps + end +end + +function update() + if not done_init then + init() + end + if not is_SITL and rc:has_valid_input() and rc:get_pwm(8) > 1800 then + -- disable automation + notify:handle_rgb(255,255,255,10) + return + end + if not arming:is_armed() and create_mission_check() then + create_mission() + end + local t = 0.001 * millis():tofloat() + + local state + if is_SITL then + -- use armed state for button in SITL + state = arming:is_armed() + else + state = button:get_button_state(button_number) + end + + local state = button:get_button_state(button_number) + if state ~= button_state then + gcs:send_text(0, string.format("release: " .. tostring(state))) + button_state = state + if button_state then + release_start_t = t + end + end + + if button_state and release_start_t > 0 and (t - release_start_t) > param:get('SCR_USER1') then + release_trigger() + release_start_t = 0.0 + release_t = t + end + + if t - last_mission_setup_t >= 1.0 then + last_mission_setup_t = t + get_glide_slope() + get_landing_AMSL() + end + + if not button_state then + set_standby() + elseif mission:num_commands() < 2 then + -- no mission, red fast + notify:handle_rgb(255,0,0,10) + elseif t - last_mission_update_t >= 1.0 then + last_mission_update_t = t + notify:handle_rgb(0,255,0,0) + mission_update() + if release_t > 0 then + airspeed_update(t-release_t) + end + end +end + +function set_fault_led() + -- setup LED for lua fault condition + notify:handle_rgb(255,255,0,2) +end + +-- wrapper around update(). This calls update() at 10Hz +-- and if update faults then an error is displayed, but the script is not stopped +function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(0, "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + local success, err = pcall(set_fault_led) + return protected_wrapper, 1000 + end + -- otherwise run at 10Hz + return protected_wrapper, 100 +end + +-- start running update loop +return protected_wrapper() \ No newline at end of file