From 7c8cbfd82d8d24a8c19508d98bd03197cc6f0b0c Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Wed, 27 Sep 2023 00:29:31 +0300 Subject: [PATCH] refactor mixer, add px4 v1.14 dronecan vtol --- Dockerfile | 1 + communicators/uavcan_communicator | 2 +- configs/{px4_common.yaml => px4/common.yaml} | 2 +- configs/{px4_cyphal.yaml => px4/cyphal.yaml} | 0 .../{px4_dronecan.yaml => px4/dronecan.yaml} | 0 .../v1.13/vtol_13070/airframe.yaml} | 0 configs/px4/v1.14/octorotor/README.md | 5 +++ .../v1.14/octorotor/airframe.yaml} | 9 +++-- configs/px4/v1.14/quadcopter/README.md | 5 +++ .../v1.14/quadcopter/airframe.yaml} | 9 +++-- configs/px4/v1.14/standard_vtol/README.md | 2 +- configs/px4/v1.14/standard_vtol/airframe.yaml | 4 ++ configs/px4/v1.14/zilant_quadcopter/README.md | 5 +++ .../v1.14/zilant_quadcopter/airframe.yaml} | 9 +++-- scripts/configure.sh | 40 ++++++++++++------- scripts/docker.sh | 26 ++++++++---- scripts/run_sim.sh | 39 ++++++++++++------ uav_dynamics/inno_vtol_dynamics | 2 +- 18 files changed, 110 insertions(+), 50 deletions(-) rename configs/{px4_common.yaml => px4/common.yaml} (69%) rename configs/{px4_cyphal.yaml => px4/cyphal.yaml} (100%) rename configs/{px4_dronecan.yaml => px4/dronecan.yaml} (100%) rename configs/{px4_v1.13_inno_vtol_airframe.yaml => px4/v1.13/vtol_13070/airframe.yaml} (100%) create mode 100644 configs/px4/v1.14/octorotor/README.md rename configs/{px4_v1.14_octorotor_airframe.yaml => px4/v1.14/octorotor/airframe.yaml} (95%) create mode 100644 configs/px4/v1.14/quadcopter/README.md rename configs/{px4_v1.14_quadcopter_airframe.yaml => px4/v1.14/quadcopter/airframe.yaml} (95%) create mode 100644 configs/px4/v1.14/zilant_quadcopter/README.md rename configs/{px4_v1.14_inno_quadcopter_airframe.yaml => px4/v1.14/zilant_quadcopter/airframe.yaml} (93%) diff --git a/Dockerfile b/Dockerfile index fd63523..d96faec 100644 --- a/Dockerfile +++ b/Dockerfile @@ -53,6 +53,7 @@ COPY catkin_build.sh catkin_build.sh COPY uav_dynamics/inno_vtol_dynamics/libs uav_dynamics/inno_vtol_dynamics/libs COPY uav_dynamics/inno_vtol_dynamics/meshes uav_dynamics/inno_vtol_dynamics/meshes COPY uav_dynamics/inno_vtol_dynamics/src uav_dynamics/inno_vtol_dynamics/src +COPY uav_dynamics/inno_vtol_dynamics/tests uav_dynamics/inno_vtol_dynamics/tests COPY uav_dynamics/inno_vtol_dynamics/urdf uav_dynamics/inno_vtol_dynamics/urdf COPY uav_dynamics/inno_vtol_dynamics/CMakeLists.txt uav_dynamics/inno_vtol_dynamics/CMakeLists.txt COPY uav_dynamics/inno_vtol_dynamics/package.xml uav_dynamics/inno_vtol_dynamics/package.xml diff --git a/communicators/uavcan_communicator b/communicators/uavcan_communicator index 67c33ae..14147fb 160000 --- a/communicators/uavcan_communicator +++ b/communicators/uavcan_communicator @@ -1 +1 @@ -Subproject commit 67c33ae8633ddfe4e5def81124a4269ed6f8f294 +Subproject commit 14147fbe4e2db631e20697e3a5376a9e69e7fede diff --git a/configs/px4_common.yaml b/configs/px4/common.yaml similarity index 69% rename from configs/px4_common.yaml rename to configs/px4/common.yaml index 13a9666..9dfb7d2 100644 --- a/configs/px4_common.yaml +++ b/configs/px4/common.yaml @@ -4,4 +4,4 @@ COM_ARM_CHK_ESCS: 0 CBRK_IO_SAFETY: 22027 SYS_BOARD_SENS: 0 COM_RC_IN_MODE: 1 -FD_ACT_EN: 1 # Enable Actuator Failure check +FD_ACT_EN: 0 # Enable Actuator Failure check diff --git a/configs/px4_cyphal.yaml b/configs/px4/cyphal.yaml similarity index 100% rename from configs/px4_cyphal.yaml rename to configs/px4/cyphal.yaml diff --git a/configs/px4_dronecan.yaml b/configs/px4/dronecan.yaml similarity index 100% rename from configs/px4_dronecan.yaml rename to configs/px4/dronecan.yaml diff --git a/configs/px4_v1.13_inno_vtol_airframe.yaml b/configs/px4/v1.13/vtol_13070/airframe.yaml similarity index 100% rename from configs/px4_v1.13_inno_vtol_airframe.yaml rename to configs/px4/v1.13/vtol_13070/airframe.yaml diff --git a/configs/px4/v1.14/octorotor/README.md b/configs/px4/v1.14/octorotor/README.md new file mode 100644 index 0000000..75d6377 --- /dev/null +++ b/configs/px4/v1.14/octorotor/README.md @@ -0,0 +1,5 @@ +# Generic 10" Octo coaxial (12001) + +## Airframe + +drawing diff --git a/configs/px4_v1.14_octorotor_airframe.yaml b/configs/px4/v1.14/octorotor/airframe.yaml similarity index 95% rename from configs/px4_v1.14_octorotor_airframe.yaml rename to configs/px4/v1.14/octorotor/airframe.yaml index ac7d013..4ed9f57 100644 --- a/configs/px4_v1.14_octorotor_airframe.yaml +++ b/configs/px4/v1.14/octorotor/airframe.yaml @@ -1,5 +1,10 @@ SYS_AUTOSTART: 12001 +MC_YAWRATE_P: 0.2 +MC_YAWRATE_I: 0.1 +MC_YAWRATE_MAX: 200.0 + +# For Cyphal UCAN1_ESC_FUNC1: 101 UCAN1_ESC_FUNC2: 102 UCAN1_ESC_FUNC3: 103 @@ -8,7 +13,3 @@ UCAN1_ESC_FUNC5: 105 UCAN1_ESC_FUNC6: 106 UCAN1_ESC_FUNC7: 107 UCAN1_ESC_FUNC8: 108 - -MC_YAWRATE_P: 0.2 -MC_YAWRATE_I: 0.1 -MC_YAWRATE_MAX: 200.0 diff --git a/configs/px4/v1.14/quadcopter/README.md b/configs/px4/v1.14/quadcopter/README.md new file mode 100644 index 0000000..3c7bf16 --- /dev/null +++ b/configs/px4/v1.14/quadcopter/README.md @@ -0,0 +1,5 @@ +# Generic Quadcopter X (4001) + +## Airframe + +drawing diff --git a/configs/px4_v1.14_quadcopter_airframe.yaml b/configs/px4/v1.14/quadcopter/airframe.yaml similarity index 95% rename from configs/px4_v1.14_quadcopter_airframe.yaml rename to configs/px4/v1.14/quadcopter/airframe.yaml index c4e7bc9..3041533 100644 --- a/configs/px4_v1.14_quadcopter_airframe.yaml +++ b/configs/px4/v1.14/quadcopter/airframe.yaml @@ -1,14 +1,15 @@ SYS_AUTOSTART: 4001 +MC_YAWRATE_P: 0.2 +MC_YAWRATE_I: 0.1 +MC_YAWRATE_MAX: 200.0 + +# For Cyphal UCAN1_ESC_FUNC1: 101 UCAN1_ESC_FUNC2: 102 UCAN1_ESC_FUNC3: 103 UCAN1_ESC_FUNC4: 104 -MC_YAWRATE_P: 0.2 -MC_YAWRATE_I: 0.1 -MC_YAWRATE_MAX: 200.0 - # For DroneCAN UAVCAN_EC_FUNC1: 101 UAVCAN_EC_FUNC2: 102 diff --git a/configs/px4/v1.14/standard_vtol/README.md b/configs/px4/v1.14/standard_vtol/README.md index 72d04f6..cd9e812 100644 --- a/configs/px4/v1.14/standard_vtol/README.md +++ b/configs/px4/v1.14/standard_vtol/README.md @@ -1,4 +1,4 @@ -# Configs +# Generic Standard VTOL (13000) ## Airframe diff --git a/configs/px4/v1.14/standard_vtol/airframe.yaml b/configs/px4/v1.14/standard_vtol/airframe.yaml index e3adb6e..33e8081 100644 --- a/configs/px4/v1.14/standard_vtol/airframe.yaml +++ b/configs/px4/v1.14/standard_vtol/airframe.yaml @@ -1,6 +1,7 @@ SYS_AUTOSTART: 13000 FW_AIRSPD_MAX: 35.0 # Max Airspeed (CAS) +FW_AIRSPD_TRIM: 25.0 # Trim (Cruise) Airspeed (CAS) FW_AIRSPD_MIN: 20.0 # Minimum Airspeed (CAS) FW_AIRSPD_STALL: 15.0 # Stall Airspeed (CAS) NAV_LOITER_RAD: 150.0 # Loiter radius (FW only) @@ -11,7 +12,10 @@ UAVCAN_EC_FUNC1: 101 # motor 1 UAVCAN_EC_FUNC2: 102 # motor 2 UAVCAN_EC_FUNC3: 103 # motor 3 UAVCAN_EC_FUNC4: 104 # motor 4 + UAVCAN_EC_FUNC5: 105 # motor 5 +UAVCAN_EC_MAX5: 4096 # motor 5 + UAVCAN_SV_FUNC1: 201 # servo 1 (left aileron) UAVCAN_SV_FUNC2: 202 # servo 2 (right aileron) UAVCAN_SV_FUNC3: 203 # servo 3 (elevators) diff --git a/configs/px4/v1.14/zilant_quadcopter/README.md b/configs/px4/v1.14/zilant_quadcopter/README.md new file mode 100644 index 0000000..3c7bf16 --- /dev/null +++ b/configs/px4/v1.14/zilant_quadcopter/README.md @@ -0,0 +1,5 @@ +# Generic Quadcopter X (4001) + +## Airframe + +drawing diff --git a/configs/px4_v1.14_inno_quadcopter_airframe.yaml b/configs/px4/v1.14/zilant_quadcopter/airframe.yaml similarity index 93% rename from configs/px4_v1.14_inno_quadcopter_airframe.yaml rename to configs/px4/v1.14/zilant_quadcopter/airframe.yaml index d0883d3..6f79694 100644 --- a/configs/px4_v1.14_inno_quadcopter_airframe.yaml +++ b/configs/px4/v1.14/zilant_quadcopter/airframe.yaml @@ -1,10 +1,11 @@ SYS_AUTOSTART: 4001 +MC_YAWRATE_P: 0.6 +MC_YAWRATE_I: 0.02 +MC_YAWRATE_MAX: 40.0 + +# For Cyphal UCAN1_ESC_FUNC1: 101 UCAN1_ESC_FUNC2: 102 UCAN1_ESC_FUNC3: 103 UCAN1_ESC_FUNC4: 104 - -MC_YAWRATE_P: 0.6 -MC_YAWRATE_I: 0.02 -MC_YAWRATE_MAX: 40.0 diff --git a/scripts/configure.sh b/scripts/configure.sh index 6b3dafe..c7a5b1b 100755 --- a/scripts/configure.sh +++ b/scripts/configure.sh @@ -17,33 +17,41 @@ Automatical configuration: configure_px4_v1_14_cyphal_quadcopter() { wget -O $DOWNLOADED_BINARY_PATH $PX4_V1_14_0_BETA_CYPHAL_URL autopilot-configurator -v --firmware $DOWNLOADED_BINARY_PATH -f --config \ - ${REPOSITORY_DIR}/configs/px4_v1.14_quadcopter_airframe.yaml \ - ${REPOSITORY_DIR}/configs/px4_cyphal.yaml \ - ${REPOSITORY_DIR}/configs/px4_common.yaml + ${REPOSITORY_DIR}/configs/px4/v1.14/quadcopter/airframe.yaml \ + ${REPOSITORY_DIR}/configs/px4/cyphal.yaml \ + ${REPOSITORY_DIR}/configs/px4/common.yaml } configure_px4_v1_14_cyphal_octorotor() { wget -O $DOWNLOADED_BINARY_PATH $PX4_V1_14_0_BETA_CYPHAL_URL autopilot-configurator -v --firmware $DOWNLOADED_BINARY_PATH -f --config \ - ${REPOSITORY_DIR}/configs/px4_v1.14_octorotor_airframe.yaml \ - ${REPOSITORY_DIR}/configs/px4_cyphal.yaml \ - ${REPOSITORY_DIR}/configs/px4_common.yaml + ${REPOSITORY_DIR}/configs/px4/v1.14/octorotor/airframe.yaml \ + ${REPOSITORY_DIR}/configs/px4/cyphal.yaml \ + ${REPOSITORY_DIR}/configs/px4/common.yaml } configure_px4_v1_14_dronecan_quadrotor() { wget -O $DOWNLOADED_BINARY_PATH $PX4_V1_14_0_BETA_DRONECAN_URL autopilot-configurator -v --firmware $DOWNLOADED_BINARY_PATH -f --config \ - ${REPOSITORY_DIR}/configs/px4_v1.14_quadcopter_airframe.yaml \ - ${REPOSITORY_DIR}/configs/px4_dronecan.yaml \ - ${REPOSITORY_DIR}/configs/px4_common.yaml + ${REPOSITORY_DIR}/configs/px4/v1.14/quadcopter/airframe.yaml \ + ${REPOSITORY_DIR}/configs/px4/dronecan.yaml \ + ${REPOSITORY_DIR}/configs/px4/common.yaml } -configure_px4_v1_12_dronecan_vtol() { +px4_v1_12_1_dronecan_vtol() { wget -O $DOWNLOADED_BINARY_PATH $PX4_V1_12_1_DRONECAN_URL autopilot-configurator -v --firmware $DOWNLOADED_BINARY_PATH -f --config \ - ${REPOSITORY_DIR}/configs/px4_v1.13_inno_vtol_airframe.yaml \ - ${REPOSITORY_DIR}/configs/px4_dronecan.yaml \ - ${REPOSITORY_DIR}/configs/px4_common.yaml + ${REPOSITORY_DIR}/configs/px4/v1.13/vtol_13070/airframe.yaml \ + ${REPOSITORY_DIR}/configs/px4/dronecan.yaml \ + ${REPOSITORY_DIR}/configs/px4/common.yaml +} + +px4_v1_14_0_beta_dronecan_vtol() { + wget -O $DOWNLOADED_BINARY_PATH $PX4_V1_14_0_BETA_DRONECAN_URL + autopilot-configurator -v --firmware $DOWNLOADED_BINARY_PATH -f --config \ + ${REPOSITORY_DIR}/configs/px4/v1.14/standard_vtol/airframe.yaml \ + ${REPOSITORY_DIR}/configs/px4/dronecan.yaml \ + ${REPOSITORY_DIR}/configs/px4/common.yaml } if [ -z $1 ]; then @@ -83,8 +91,10 @@ elif [[ $1 == "cyphal_octorotor" ]]; then configure_px4_v1_14_cyphal_octorotor elif [[ $1 == "dronecan_quadrotor" ]]; then configure_px4_v1_14_dronecan_quadrotor -elif [[ $1 == "dronecan_vtol" ]]; then - configure_px4_v1_12_dronecan_vtol +elif [[ $1 == "px4_v1_12_1_dronecan_vtol" ]]; then + px4_v1_12_1_dronecan_vtol +elif [[ $1 == "px4_v1_14_0_beta_dronecan_vtol" ]]; then + px4_v1_14_0_beta_dronecan_vtol else printf "$RED$SCRIPT_NAME ERROR (line ${LINENO}): Unknown argument: '$1' $NC\n" exit 1 diff --git a/scripts/docker.sh b/scripts/docker.sh index 043fe35..86102a4 100755 --- a/scripts/docker.sh +++ b/scripts/docker.sh @@ -17,11 +17,12 @@ Supported modes (with aliases): ------------------------------------------------------------------------------- cyphal_quadrotor,cq | Cyphal PX4 v1.14-beta Quadrotor x (4001) dronecan_quadrotor,dq | DroneCAN PX4 v1.14-beta Quadrotor (4001) - dronecan_vtol,dv | DroneCAN PX4 v1.12 inno_vtol + dronecan_vtol_v1_14_0,dv | DroneCAN PX4 v1.14-beta Standard VTOL (13000) + dronecan_vtol_v1_12_1,dvo | DroneCAN PX4 v1.12 vtol 13070 ------------------------------------------------------------------------------- cyphal_standard_vtol,csv | Cyphal PX4 v1.14-beta Standard VTOL (13000) cyphal_octorotor,co | Cyphal PX4 v1.14-beta Octorotor Coaxial (12001) - sitl_inno_vtol | MAVLink PX4 v1.12 inno_vtol + sitl_inno_vtol | MAVLink PX4 v1.12 vtol 13070 sitl_flight_goggles | MAVLink PX4 v1.12 Quadrotor (4001) cyphal_and_dronecan | 2 CAN AP v4.4.0 Copter ------------------------------------------------------------------------------- @@ -121,13 +122,22 @@ push_docker_image() { docker push $IMAGE_NAME } -dronecan_vtol() { +dronecan_vtol_v1_12_1() { kill_all_related_containers setup_dronecan_hitl_config if [[ $OPTIONS == "--force" ]]; then - ./configure.sh dronecan_vtol + ./configure.sh px4_v1_12_1_dronecan_vtol fi - docker container run --rm $DOCKER_FLAGS $IMAGE_NAME ./scripts/run_sim.sh dronecan_inno_vtol + docker container run --rm $DOCKER_FLAGS $IMAGE_NAME ./scripts/run_sim.sh px4_v1_12_1_dronecan_vtol +} + +dronecan_vtol_v1_14_0() { + kill_all_related_containers + setup_dronecan_hitl_config + if [[ $OPTIONS == "--force" ]]; then + ./configure.sh px4_v1_14_0_beta_dronecan_vtol + fi + docker container run --rm $DOCKER_FLAGS $IMAGE_NAME ./scripts/run_sim.sh px4_v1_14_0_beta_dronecan_vtol } dronecan_quadrotor() { @@ -214,8 +224,10 @@ elif [ "$1" = "pull" ]; then pull_docker_image elif [ "$1" = "push" ]; then push_docker_image -elif [ "$1" = "dronecan_vtol" ] || [ "$1" = "dv" ]; then - dronecan_vtol +elif [ "$1" = "dronecan_vtol_v1_12_1" ] || [ "$1" = "dvo" ]; then + dronecan_vtol_v1_12_1 +elif [ "$1" = "dronecan_vtol_v1_14_0" ] || [ "$1" = "dv" ]; then + dronecan_vtol_v1_14_0 elif [ "$1" = "dronecan_quadrotor" ] || [ "$1" = "dq" ]; then dronecan_quadrotor elif [ "$1" = "sitl_inno_vtol" ]; then diff --git a/scripts/run_sim.sh b/scripts/run_sim.sh index dcb8ef1..07babfa 100755 --- a/scripts/run_sim.sh +++ b/scripts/run_sim.sh @@ -5,12 +5,13 @@ print_help() { It automatically run all auxilliary scripts required to each specific mode and source necessary setup.bash files. It supports all possible simulator modes. -https://github.com/InnopolisAero/innopolis_vtol_dynamics +https://github.com/RaccoonlabDev/innopolis_vtol_dynamics Usage: run_sim.sh Commands: - dronecan_inno_vtol Run dynamics simulator in DroneCan HITL mode for inno_vtol airframe + px4_v1_14_0_beta_dronecan_vtol Run dynamics simulator in DroneCan HITL mode for px4 vtol 13000 airframe + px4_v1_12_1_dronecan_vtol Run dynamics simulator in DroneCan HITL mode for px4 vtol 13070 airframe dronecan_flight_goggles Run dynamics simulator in DroneCan HITL mode for flight_goggles airframe cyphal_quadrotor Cyphal HITL PX4 Quadrotor (4001) cyphal_octorotor Cyphal HITL PX4 Octorotor (12001) @@ -82,7 +83,7 @@ setup_combined_hitl() { fi } -dronecan_inno_vtol() { +px4_v1_14_0_beta_dronecan_vtol() { setup_ros setup_dronecan_hitl $SCRIPT_DIR/airframe_printer.sh 13000 @@ -90,8 +91,20 @@ dronecan_inno_vtol() { run_dronecan_communicator:=true \ vehicle:=innopolis_vtol \ vehicle_params:=vtol_params \ - mixer:=inno_vtol_mixer \ - dynamics:=inno_vtol + mixer:=px4_v1_14_0_vtol_13000_mixer \ + dynamics:=vtol_dynamics +} + +px4_v1_12_1_dronecan_vtol() { + setup_ros + setup_dronecan_hitl + $SCRIPT_DIR/airframe_printer.sh 13000 + roslaunch innopolis_vtol_dynamics hitl.launch \ + run_dronecan_communicator:=true \ + vehicle:=innopolis_vtol \ + vehicle_params:=vtol_params \ + mixer:=vtol_13070_mixer \ + dynamics:=vtol_dynamics } dronecan_flight_goggles() { @@ -139,7 +152,7 @@ cyphal_standard_vtol() { vehicle:=innopolis_vtol \ vehicle_params:=vtol_params \ mixer:=direct_mixer \ - dynamics:=inno_vtol + dynamics:=vtol_dynamics } cyphal_and_dronecan_inno_vtol() { @@ -160,8 +173,8 @@ sitl_inno_vtol() { roslaunch innopolis_vtol_dynamics sitl.launch \ vehicle:=innopolis_vtol \ vehicle_params:=vtol_params \ - mixer:=inno_vtol_mixer \ - dynamics:=inno_vtol \ + mixer:=vtol_13070_mixer \ + dynamics:=vtol_dynamics \ run_sitl_flight_stack:="false" } @@ -181,8 +194,8 @@ sitl_inno_vtol_with_flight_stack() { roslaunch innopolis_vtol_dynamics sitl.launch \ vehicle:=innopolis_vtol \ vehicle_params:=quadrotor_params \ - mixer:=inno_vtol_mixer \ - dynamics:=inno_vtol \ + mixer:=vtol_13070_mixer \ + dynamics:=vtol_dynamics \ run_sitl_flight_stack:="true" } @@ -204,8 +217,10 @@ if [ "${BASH_SOURCE[0]}" -ef "$0" ]; then fi SCRIPT_DIR="$(dirname "$0")" -if [ "$1" = "dronecan_inno_vtol" ]; then - dronecan_inno_vtol +if [ "$1" = "px4_v1_14_0_beta_dronecan_vtol" ]; then + px4_v1_14_0_beta_dronecan_vtol +elif [ "$1" = "px4_v1_12_1_dronecan_vtol" ]; then + px4_v1_12_1_dronecan_vtol elif [ "$1" = "dronecan_flight_goggles" ]; then dronecan_flight_goggles elif [ "$1" = "cyphal_quadrotor" ]; then diff --git a/uav_dynamics/inno_vtol_dynamics b/uav_dynamics/inno_vtol_dynamics index 2419b79..8e2e9b5 160000 --- a/uav_dynamics/inno_vtol_dynamics +++ b/uav_dynamics/inno_vtol_dynamics @@ -1 +1 @@ -Subproject commit 2419b7964e65f00e0ac345b1e15ba24e0fc98125 +Subproject commit 8e2e9b5b5f70205c8b39f4fac27712745f6580de