mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-30 22:51:35 +08:00
Matthias Grob
d92e66863a
Before #14212 the velocity control gains used in the multicopter position controller were defined as a scale between velocity error in one axis (or it's integral and derivative respectively) and the unit thrust vector. The problem with this is that the normalization of the unit thrust vector changes per vehicle or even vehicle configuration as 0 and 100% thrust get a different physical response. That's why the gains are now defined as scale between velocity error (integral/derivative) and the output acceleration in m/s².
293 lines
7.5 KiB
Bash
293 lines
7.5 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_onboard_payload_port_local=$((14280+px4_instance))
|
|
udp_onboard_payload_port_remote=$((14030+px4_instance))
|
|
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 CBRK_AIRSPD_CHK 0
|
|
param set CBRK_SUPPLY_CHK 894281
|
|
|
|
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
|
|
|
|
# Simulator IMU data provided at 250 Hz
|
|
param set IMU_INTEG_RATE 250
|
|
|
|
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_P_ACC 12.0
|
|
param set MPC_Z_VEL_I_ACC 3.0
|
|
param set MPC_XY_P 0.8
|
|
param set MPC_XY_VEL_P_ACC 4.0
|
|
param set MPC_XY_VEL_I_ACC 0.4
|
|
param set MPC_XY_VEL_D_ACC 0.32
|
|
|
|
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
|
|
|
|
param set COM_CPU_MAX -1 # disable check, no CPU load reported on posix yet
|
|
|
|
# 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
|
|
|
|
# Onboard link to camera
|
|
mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_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
|