mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
874c6f385b
The implementation before this change had two timeouts, a hard-coded timeout of 0.5 seconds as well as a by param configurable timeout with certain failsafe actions set. This change aims to fix two problems: 1. The hard-coded offboard timeout can be triggered easily with sped up lockstep simulation. Since i t is hard-coded it can't be adapted to the speed factor. 2. The offboard signal can time out but no action will be taken just yet. This means we end up in an in-between stage where no warning or failsafe action has happened yet, even though certain flags are set to a timeout state. This patch aims to fix this by unifying the two timeouts to the existing configurable param. The convoluted double timeout logic is replaced by a simple hysteresis. For anyone that has previously not changed the default timeout param (0), the param will now be changed to 0.5 seconds which reflects the previously hardcoded time. For anyone with a specific timeout configured, the behaviour should remain the same. Also, going forward, timeouts lower than 0.5 seconds should be possible.
289 lines
7.2 KiB
Bash
289 lines
7.2 KiB
Bash
#!/bin/sh
|
|
|
|
# PX4 commands need the 'px4-' prefix in bash.
|
|
# (px4-alias.sh is expected to be in the PATH)
|
|
# shellcheck disable=SC1091
|
|
. px4-alias.sh
|
|
|
|
SCRIPT_DIR="$(CDPATH='' cd -- "$(dirname -- "$0")" && pwd)"
|
|
|
|
#
|
|
# Main SITL startup script
|
|
#
|
|
|
|
# check for ekf2 replay
|
|
# shellcheck disable=SC2154
|
|
if [ "$replay_mode" = "ekf2" ]
|
|
then
|
|
sh etc/init.d-posix/rc.replay
|
|
exit 0
|
|
fi
|
|
|
|
# initialize script variables
|
|
set AUX_MODE none
|
|
set IO_PRESENT no
|
|
set LOG_FILE bootlog.txt
|
|
set MAV_TYPE none
|
|
set MIXER none
|
|
set MIXER_AUX none
|
|
set MIXER_FILE none
|
|
set OUTPUT_MODE sim
|
|
set PWM_OUT none
|
|
set SDCARD_MIXERS_PATH etc/mixers
|
|
set USE_IO no
|
|
set VEHICLE_TYPE none
|
|
set LOGGER_BUF 1000
|
|
|
|
set RUN_MINIMAL_SHELL no
|
|
|
|
# Use the variable set by sitl_run.sh to choose the model settings.
|
|
if [ "$PX4_SIM_MODEL" = "shell" ]; then
|
|
set RUN_MINIMAL_SHELL yes
|
|
else
|
|
# Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL})
|
|
# TODO: unify with rc.autostart generation
|
|
# shellcheck disable=SC2012
|
|
REQUESTED_AUTOSTART=$(ls "$SCRIPT_DIR" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p')
|
|
if [ -z "$REQUESTED_AUTOSTART" ]; then
|
|
echo "Error: Unknown model '$PX4_SIM_MODEL'"
|
|
exit 1
|
|
fi
|
|
fi
|
|
|
|
# clear bootlog
|
|
[ -f $LOG_FILE ] && rm $LOG_FILE
|
|
|
|
|
|
uorb start
|
|
|
|
# Load parameters
|
|
set PARAM_FILE eeprom/parameters_"$REQUESTED_AUTOSTART"
|
|
param select $PARAM_FILE
|
|
|
|
if [ -f $PARAM_FILE ]
|
|
then
|
|
if param load
|
|
then
|
|
echo "[param] Loaded: $PARAM_FILE"
|
|
else
|
|
echo "[param] FAILED loading $PARAM_FILE"
|
|
fi
|
|
else
|
|
echo "[param] parameter file not found, creating $PARAM_FILE"
|
|
fi
|
|
|
|
# exit early when the minimal shell is requested
|
|
[ $RUN_MINIMAL_SHELL = yes ] && exit 0
|
|
|
|
|
|
# Use environment variable PX4_ESTIMATOR to choose estimator.
|
|
if [ "$PX4_ESTIMATOR" = "q" ]; then
|
|
param set SYS_MC_EST_GROUP 3
|
|
elif [ "$PX4_ESTIMATOR" = "ekf2" ]; then
|
|
param set SYS_MC_EST_GROUP 2
|
|
elif [ "$PX4_ESTIMATOR" = "lpe" ]; then
|
|
param set SYS_MC_EST_GROUP 1
|
|
elif [ "$PX4_ESTIMATOR" = "inav" ]; then
|
|
param set SYS_MC_EST_GROUP 0
|
|
fi
|
|
|
|
if param compare SYS_AUTOSTART $REQUESTED_AUTOSTART
|
|
then
|
|
set AUTOCNF no
|
|
else
|
|
set AUTOCNF yes
|
|
param set SYS_AUTOCONFIG 1
|
|
fi
|
|
|
|
if param compare SYS_AUTOCONFIG 1
|
|
then
|
|
set AUTOCNF yes
|
|
|
|
# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID
|
|
param reset_nostart RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT_UUID
|
|
fi
|
|
|
|
# multi-instance setup
|
|
# shellcheck disable=SC2154
|
|
param set MAV_SYS_ID $((px4_instance+1))
|
|
simulator_tcp_port=$((4560+px4_instance))
|
|
udp_offboard_port_local=$((14580+px4_instance))
|
|
udp_offboard_port_remote=$((14540+px4_instance))
|
|
[ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps
|
|
udp_gcs_port_local=$((18570+px4_instance))
|
|
|
|
|
|
if [ $AUTOCNF = yes ]
|
|
then
|
|
param set SYS_AUTOSTART $REQUESTED_AUTOSTART
|
|
|
|
param set BAT_N_CELLS 3
|
|
|
|
param set CAL_ACC0_ID 1311244
|
|
param set CAL_ACC_PRIME 1311244
|
|
|
|
param set CAL_GYRO0_ID 2294028
|
|
param set CAL_GYRO_PRIME 2294028
|
|
|
|
param set CAL_MAG0_ID 197388
|
|
param set CAL_MAG_PRIME 197388
|
|
|
|
param set CAL_BARO_PRIME 6620172
|
|
|
|
param set CBRK_AIRSPD_CHK 0
|
|
|
|
param set COM_DISARM_LAND 0.5
|
|
param set COM_OBL_ACT 2
|
|
param set COM_OBL_RC_ACT 0
|
|
param set COM_RC_IN_MODE 1
|
|
|
|
param set EKF2_AID_MASK 1
|
|
param set EKF2_ANGERR_INIT 0.01
|
|
param set EKF2_HGT_MODE 0
|
|
param set EKF2_GBIAS_INIT 0.01
|
|
|
|
# Prevent high accel bias
|
|
param set COM_ARM_EKF_AB 0.005
|
|
|
|
# Speedup SITL startup
|
|
param set EKF2_REQ_GPS_H 0.5
|
|
|
|
# LPE: GPS only mode
|
|
param set LPE_FUSION 145
|
|
|
|
param set MC_PITCH_P 6
|
|
param set MC_PITCHRATE_P 0.2
|
|
param set MC_ROLL_P 6
|
|
param set MC_ROLLRATE_P 0.2
|
|
|
|
param set MPC_ALT_MODE 0
|
|
param set MPC_HOLD_MAX_Z 2
|
|
param set MPC_Z_VEL_I 0.15
|
|
param set MPC_Z_VEL_P 0.6
|
|
param set MPC_XY_P 0.8
|
|
param set MPC_XY_VEL_P 0.2
|
|
param set MPC_XY_VEL_I 0.02
|
|
param set MPC_XY_VEL_D 0.016
|
|
|
|
param set MPC_JERK_MIN 10
|
|
param set MPC_JERK_MAX 20
|
|
param set MPC_ACC_HOR_MAX 3
|
|
param set MPC_SPOOLUP_TIME 0.5
|
|
param set MPC_TKO_RAMP_T 1
|
|
|
|
param set NAV_ACC_RAD 2
|
|
param set NAV_DLL_ACT 2
|
|
|
|
param set RTL_DESCEND_ALT 5
|
|
param set RTL_LAND_DELAY 5
|
|
param set RTL_RETURN_ALT 30
|
|
|
|
# By default log from boot until first disarm.
|
|
param set SDLOG_MODE 1
|
|
# enable default, estimator replay and vision/avoidance logging profiles
|
|
param set SDLOG_PROFILE 131
|
|
param set SDLOG_DIRS_MAX 7
|
|
param set SENS_BOARD_ROT 0
|
|
param set SENS_BOARD_X_OFF 0.000001
|
|
param set SENS_DPRES_OFF 0.001
|
|
param set SYS_RESTART_TYPE 2
|
|
|
|
param set TRIG_INTERFACE 3
|
|
|
|
fi
|
|
|
|
# Adapt timeout parameters if simulation runs faster or slower than realtime.
|
|
if [ ! -z $PX4_SIM_SPEED_FACTOR ]; then
|
|
COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
|
|
echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
|
|
param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
|
|
|
|
COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
|
|
echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
|
|
param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
|
|
|
|
COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
|
|
echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
|
|
param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
|
|
fi
|
|
|
|
# Autostart ID
|
|
autostart_file=''
|
|
for f in etc/init.d-posix/"$(param show -q SYS_AUTOSTART)"_*
|
|
do
|
|
filename=$(basename "$f")
|
|
case "$filename" in
|
|
*\.*)
|
|
# ignore files that contain a dot (e.g. <vehicle>.post)
|
|
;;
|
|
*)
|
|
autostart_file="$f"
|
|
;;
|
|
esac
|
|
done
|
|
if [ ! -e "$autostart_file" ]; then
|
|
echo "Error: no autostart file found ($autostart_file)"
|
|
exit 1
|
|
fi
|
|
|
|
sh "$autostart_file"
|
|
|
|
#
|
|
# If autoconfig parameter was set, reset it and save parameters.
|
|
#
|
|
if [ $AUTOCNF = yes ]
|
|
then
|
|
param set SYS_AUTOCONFIG 0
|
|
fi
|
|
|
|
dataman start
|
|
replay tryapplyparams
|
|
simulator start -c $simulator_tcp_port
|
|
tone_alarm start
|
|
rc_update start
|
|
sensors start
|
|
commander start
|
|
navigator start
|
|
|
|
|
|
if ! param compare -s MNT_MODE_IN -1
|
|
then
|
|
vmount start
|
|
fi
|
|
|
|
if param greater -s TRIG_MODE 0
|
|
then
|
|
camera_trigger start
|
|
camera_feedback start
|
|
fi
|
|
|
|
# Configure vehicle type specific parameters.
|
|
# Note: rc.vehicle_setup is the entry point for rc.interface,
|
|
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
|
|
#
|
|
sh etc/init.d/rc.vehicle_setup
|
|
|
|
# GCS link
|
|
mavlink start -x -u $udp_gcs_port_local -r 4000000
|
|
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local
|
|
mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local
|
|
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local
|
|
mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local
|
|
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local
|
|
mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local
|
|
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local
|
|
mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local
|
|
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
|
|
|
|
# API/Offboard link
|
|
mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
|
|
|
|
# execute autostart post script if any
|
|
[ -e "$autostart_file".post ] && sh "$autostart_file".post
|
|
|
|
# Run script to start logging
|
|
sh etc/init.d/rc.logging
|
|
|
|
mavlink boot_complete
|
|
replay trystart
|