From 519aa55235a753a41e2d5b8008c2c098c0a2d4d7 Mon Sep 17 00:00:00 2001 From: Shaochang Tan <478710209@qq.com> Date: Sun, 20 Sep 2020 21:21:09 +0800 Subject: [PATCH] fix more than 10 vehicle can not connect the MAVROS --- .gitignore | 3 +- coordination/launch_generator/generator.py | 12 +- .../launch_generator/multi_vehicle.launch | 551 +++++++++++++++++- sitl_config/init.d-posix/rcS | 79 ++- .../launch/single_vehicle_spawn_xtd.launch | 4 +- 5 files changed, 565 insertions(+), 84 deletions(-) diff --git a/.gitignore b/.gitignore index 5163cf6..602012e 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,5 @@ project *.pbstream sensing/slam/vio/VINS-Fusion/camera_models/camera_calib_example .gitee -.vscode \ No newline at end of file +.vscode +.idea \ No newline at end of file diff --git a/coordination/launch_generator/generator.py b/coordination/launch_generator/generator.py index b01a02e..aff41c2 100644 --- a/coordination/launch_generator/generator.py +++ b/coordination/launch_generator/generator.py @@ -82,7 +82,7 @@ with open('launch_temp_1.11','r') as f: with open('multi_vehicle.launch','w') as f: f.write(launch_head) row_in_all = 0 - id_in_all = 1 + id_in_all = 0 for type_id in range(8): type_num = num_of_type[type_id] row_in_type = row_of_type[type_id] @@ -100,11 +100,9 @@ with open('multi_vehicle.launch','w') as f: if type_num > 0: for id_in_type in range(0,type_num): - - mavlink_1=34570-1+id_in_all*2 - mavlink_2=mavlink_1+1 - onboard=14540+id_in_all - SITL=24560+(id_in_all)*2 + offboard_local=34580+id_in_all + offboard_remote=24540+id_in_all + SITL=18570+id_in_all TCP=4560+id_in_all for line in launch_lines: if " - + - + @@ -34,8 +34,8 @@ - - + + @@ -50,9 +50,9 @@ - + - + @@ -63,8 +63,8 @@ - - + + @@ -79,21 +79,21 @@ - + - + - - + + - - + + @@ -108,12 +108,12 @@ - + - + - + @@ -121,8 +121,8 @@ - - + + @@ -137,21 +137,21 @@ - + - + - - + + - - + + @@ -166,9 +166,67 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -179,8 +237,443 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sitl_config/init.d-posix/rcS b/sitl_config/init.d-posix/rcS index 07a5f9a..98b311b 100644 --- a/sitl_config/init.d-posix/rcS +++ b/sitl_config/init.d-posix/rcS @@ -32,7 +32,8 @@ set PWM_OUT none set SDCARD_MIXERS_PATH etc/mixers set USE_IO no set VEHICLE_TYPE none -set LOGGER_BUF 1000 +set LOGGER_ARGS "" +set LOGGER_BUF 1000 set RUN_MINIMAL_SHELL no @@ -107,24 +108,24 @@ fi # 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_offboard_port_local=$((34580+px4_instance)) +udp_offboard_port_remote=$((24540+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 BAT_N_CELLS 4 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_GYRO0_ID 1311244 + param set CAL_GYRO_PRIME 1311244 param set CAL_MAG0_ID 197388 param set CAL_MAG_PRIME 197388 @@ -132,21 +133,9 @@ then 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 + # Don't require RC calibration and configuration param set COM_RC_IN_MODE 1 - # GPS used - param set EKF2_AID_MASK 1 - # Vision used and GPS denied - #param set EKF2_AID_MASK 24 - - # Barometer used for hight measurement - param set EKF2_HGT_MODE 0 - # Barometer denied and vision used for hight measurement - #param set EKF2_HGT_MODE 3 - param set EKF2_ANGERR_INIT 0.01 param set EKF2_GBIAS_INIT 0.01 @@ -159,31 +148,18 @@ then # 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_I 0.15 - param set MPC_Z_VEL_P 0.6 + 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 0.2 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.016 + 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. @@ -201,8 +177,11 @@ fi param set COM_CPU_MAX -1 # disable check, no CPU load reported on posix yet +# Simulator IMU data provided at 250 Hz +param set IMU_INTEG_RATE 250 + # Adapt timeout parameters if simulation runs faster or slower than realtime. -if [ ! -z $PX4_SIM_SPEED_FACTOR ]; then +if [ -n "$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 @@ -246,8 +225,12 @@ then fi dataman start -replay tryapplyparams -simulator start -c $simulator_tcp_port +# only start the simulator if not in replay mode, as both control the lockstep time +if ! replay tryapplyparams +then + simulator start -c $simulator_tcp_port +fi +battery_simulator start tone_alarm start rc_update start sensors start @@ -288,13 +271,19 @@ mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local 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 +#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 +if param compare SYS_MC_EST_GROUP 2 +then + set LOGGER_ARGS "-p ekf2_timestamps" +else + set LOGGER_ARGS "-p vehicle_attitude" +fi sh etc/init.d/rc.logging mavlink boot_complete -replay trystart \ No newline at end of file +replay trystart diff --git a/sitl_config/launch/single_vehicle_spawn_xtd.launch b/sitl_config/launch/single_vehicle_spawn_xtd.launch index f719f32..799cc69 100644 --- a/sitl_config/launch/single_vehicle_spawn_xtd.launch +++ b/sitl_config/launch/single_vehicle_spawn_xtd.launch @@ -10,9 +10,9 @@ - + - +