├── src ├── fw_plane_sim │ ├── plugins │ │ ├── .keep │ │ └── nodelet_plugins.xml │ ├── src │ │ ├── px4_lib │ │ │ ├── slew_rate │ │ │ │ ├── dummy.cpp │ │ │ │ └── CMakeLists.txt │ │ │ ├── matrix │ │ │ │ ├── matrix │ │ │ │ │ ├── math.hpp │ │ │ │ │ ├── filter.hpp │ │ │ │ │ ├── integration.hpp │ │ │ │ │ ├── Scalar.hpp │ │ │ │ │ └── Vector2.hpp │ │ │ │ ├── test │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ ├── MatrixUnwrapTest.cpp │ │ │ │ │ ├── MatrixScalarMultiplicationTest.cpp │ │ │ │ │ ├── MatrixTransposeTest.cpp │ │ │ │ │ ├── MatrixHatveeTest.cpp │ │ │ │ │ ├── MatrixUpperRightTriangleTest.cpp │ │ │ │ │ └── MatrixIntegralTest.cpp │ │ │ │ └── CMakeLists.txt │ │ │ ├── uORB │ │ │ │ └── topics │ │ │ │ │ ├── vehicle_local_position_setpoint.h │ │ │ │ │ ├── vehicle_attitude_setpoint.h │ │ │ │ │ ├── cpuload.h │ │ │ │ │ ├── vehicle_rates_setpoint.h │ │ │ │ │ ├── vehicle_angular_velocity.h │ │ │ │ │ ├── vehicle_constraints.h │ │ │ │ │ ├── vehicle_attitude.h │ │ │ │ │ ├── actuator_armed.h │ │ │ │ │ ├── home_position.h │ │ │ │ │ ├── offboard_control_mode.h │ │ │ │ │ └── vehicle_air_data.h │ │ │ ├── parameters │ │ │ │ └── px4_parameters.cpp │ │ │ ├── hysteresis │ │ │ │ └── CMakeLists.txt │ │ │ ├── geo │ │ │ │ └── CMakeLists.txt │ │ │ └── mathlib │ │ │ │ ├── mathlib.h │ │ │ │ ├── math │ │ │ │ └── test │ │ │ │ │ └── test.hpp │ │ │ │ └── CMakeLists.txt │ │ ├── fw_sim │ │ │ └── BHDynamic │ │ │ │ ├── libBHDynamic.so │ │ │ │ ├── backup │ │ │ │ ├── x86-20241001-formation │ │ │ │ │ ├── libBHDynamic.so │ │ │ │ │ └── BHDynamic.h │ │ │ │ └── arm64-20241112-guidance │ │ │ │ │ ├── libBHDynamic.so │ │ │ │ │ └── BHDynamic.h │ │ │ │ └── BHDynamic.h │ │ └── fw_plane_visualizer_node.cpp │ ├── model │ │ └── meshes │ │ │ ├── body.stl │ │ │ ├── rudder.stl │ │ │ ├── elevators.stl │ │ │ ├── left_flap.stl │ │ │ ├── iris_prop_cw.stl │ │ │ ├── left_aileron.stl │ │ │ ├── right_flap.stl │ │ │ ├── iris_prop_ccw.stl │ │ │ └── right_aileron.stl │ └── include │ │ └── fw_plane_sim │ │ └── fw_sim │ │ └── fw_sim_node.hpp ├── CMakeLists.txt ├── sss_sim_env │ ├── msg │ │ └── TimeRequest.msg │ ├── srv │ │ ├── ClientRegister.srv │ │ ├── ClientUnregister.srv │ │ └── SimClockControl.srv │ ├── scripts │ │ ├── ui │ │ │ └── py_cmd.sh │ │ └── clock.png │ ├── include │ │ └── sss_sim_env │ │ │ └── sss_utils.hpp │ ├── plugins │ │ └── nodelet_plugins.xml │ ├── launch │ │ └── sim_clock.launch │ └── src │ │ └── sim_clock │ │ └── sim_clock.cpp ├── px4_rotor_sim │ ├── src │ │ └── mavros_px4_quadrotor_sim │ │ │ ├── px4_modules │ │ │ ├── px4_lib │ │ │ │ ├── slew_rate │ │ │ │ │ ├── dummy.cpp │ │ │ │ │ └── CMakeLists.txt │ │ │ │ ├── matrix │ │ │ │ │ ├── matrix │ │ │ │ │ │ ├── math.hpp │ │ │ │ │ │ ├── filter.hpp │ │ │ │ │ │ ├── integration.hpp │ │ │ │ │ │ ├── Scalar.hpp │ │ │ │ │ │ └── Vector2.hpp │ │ │ │ │ ├── test │ │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ │ ├── MatrixUnwrapTest.cpp │ │ │ │ │ │ ├── MatrixScalarMultiplicationTest.cpp │ │ │ │ │ │ ├── MatrixHatveeTest.cpp │ │ │ │ │ │ ├── MatrixTransposeTest.cpp │ │ │ │ │ │ └── MatrixUpperRightTriangleTest.cpp │ │ │ │ │ └── CMakeLists.txt │ │ │ │ ├── uORB │ │ │ │ │ └── topics │ │ │ │ │ │ ├── vehicle_local_position_setpoint.h │ │ │ │ │ │ ├── vehicle_attitude_setpoint.h │ │ │ │ │ │ ├── cpuload.h │ │ │ │ │ │ ├── vehicle_rates_setpoint.h │ │ │ │ │ │ ├── vehicle_angular_velocity.h │ │ │ │ │ │ ├── vehicle_constraints.h │ │ │ │ │ │ ├── vehicle_attitude.h │ │ │ │ │ │ ├── actuator_armed.h │ │ │ │ │ │ ├── home_position.h │ │ │ │ │ │ ├── offboard_control_mode.h │ │ │ │ │ │ └── vehicle_air_data.h │ │ │ │ ├── parameters │ │ │ │ │ └── px4_parameters.cpp │ │ │ │ ├── hysteresis │ │ │ │ │ └── CMakeLists.txt │ │ │ │ ├── geo │ │ │ │ │ └── CMakeLists.txt │ │ │ │ └── mathlib │ │ │ │ │ ├── mathlib.h │ │ │ │ │ └── math │ │ │ │ │ └── test │ │ │ │ │ └── test.hpp │ │ │ ├── mc_att_control │ │ │ │ ├── Kconfig │ │ │ │ ├── CMakeLists.txt │ │ │ │ └── AttitudeControl │ │ │ │ │ └── CMakeLists.txt │ │ │ ├── mc_pos_control │ │ │ │ ├── Kconfig │ │ │ │ ├── Takeoff │ │ │ │ │ └── CMakeLists.txt │ │ │ │ ├── CMakeLists.txt │ │ │ │ └── PositionControl │ │ │ │ │ └── CMakeLists.txt │ │ │ └── mavlink │ │ │ │ ├── mavlink_msg_list.cpp │ │ │ │ ├── mavlink_streamer.cpp │ │ │ │ └── mavlink_msg_list.hpp │ │ │ ├── mavros_sim │ │ │ ├── launch │ │ │ │ └── mavros_sim_test.launch │ │ │ ├── lib │ │ │ │ └── uas_timesync.cpp │ │ │ └── MavrosSim.hpp │ │ │ └── drone_visualizer_node.cpp │ ├── model │ │ └── meshes │ │ │ ├── iris.stl │ │ │ ├── iris_prop_ccw.stl │ │ │ └── iris_prop_cw.stl │ ├── include │ │ ├── mavlink │ │ │ ├── README.md │ │ │ └── v2.0 │ │ │ │ ├── message_definitions │ │ │ │ ├── standard.xml │ │ │ │ ├── test.xml │ │ │ │ └── paparazzi.xml │ │ │ │ ├── all │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── test │ │ │ │ ├── version.h │ │ │ │ ├── mavlink.h │ │ │ │ └── test.h │ │ │ │ ├── ASLUAV │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── AVSSUAS │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── common │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── icarous │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── minimal │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── standard │ │ │ │ ├── version.h │ │ │ │ ├── mavlink.h │ │ │ │ ├── testsuite.h │ │ │ │ └── standard.h │ │ │ │ ├── storm32 │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── csAirLink │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── cubepilot │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── paparazzi │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── uAvionix │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── ualberta │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── development │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── matrixpilot │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ ├── ardupilotmega │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ │ │ └── python_array_test │ │ │ │ ├── version.h │ │ │ │ └── mavlink.h │ │ └── mavros_px4_quadrotor_sim │ │ │ └── mavros_px4_quadrotor_sim_node.hpp │ ├── plugins │ │ └── nodelet_plugins.xml │ └── launch │ │ ├── test_multi_group.launch │ │ └── drone_visualizer_multi.launch ├── ugv_sim │ ├── model │ │ ├── meshes │ │ │ ├── urm04.STL │ │ │ ├── wheel_shaft.STL │ │ │ ├── nexus_base_link.STL │ │ │ ├── mecanum_wheel_left.STL │ │ │ ├── mecanum_wheel_right.STL │ │ │ └── nexus_base_link_collision.STL │ │ └── urm04_sensor.xacro │ ├── plugins │ │ └── nodelet_plugins.xml │ ├── include │ │ └── ugv_sim │ │ │ └── wheeltec_ugv_sim_node.hpp │ └── launch │ │ └── visualize_ugv_multi.launch └── tello_sim │ ├── model │ └── meshes │ │ ├── tello_body.stl │ │ ├── tello_prop_ccw.stl │ │ └── tello_prop_cw.stl │ ├── plugins │ └── nodelet_plugins.xml │ ├── include │ └── tello_sim │ │ └── tello_quadrotor_sim_node.hpp │ └── launch │ └── visualize_tello_multi.launch ├── pictures ├── sss-logo.png ├── sim_clock.png ├── gif-4uav-4ugv.gif ├── multi-fw-sim.png ├── multi-ugv-sim.png ├── px4rotor-100.gif ├── sss-framework.png ├── multi-tello-sim.png └── multi-px4-rotor-sim.png ├── .gitignore ├── install.sh ├── .vscode └── c_cpp_properties.json └── LICENSE /src/fw_plane_sim/plugins/.keep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/slew_rate/dummy.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/sss_sim_env/msg/TimeRequest.msg: -------------------------------------------------------------------------------- 1 | uint32 time_client_id 2 | time request_time 3 | -------------------------------------------------------------------------------- /src/sss_sim_env/srv/ClientRegister.srv: -------------------------------------------------------------------------------- 1 | --- 2 | int64 client_id 3 | bool success 4 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/slew_rate/dummy.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/sss_sim_env/srv/ClientUnregister.srv: -------------------------------------------------------------------------------- 1 | int64 client_id 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /pictures/sss-logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/sss-logo.png -------------------------------------------------------------------------------- /pictures/sim_clock.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/sim_clock.png -------------------------------------------------------------------------------- /src/sss_sim_env/scripts/ui/py_cmd.sh: -------------------------------------------------------------------------------- 1 | python -m PyQt5.uic.pyuic main_window.ui -o main_window.py 2 | -------------------------------------------------------------------------------- /pictures/gif-4uav-4ugv.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/gif-4uav-4ugv.gif -------------------------------------------------------------------------------- /pictures/multi-fw-sim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/multi-fw-sim.png -------------------------------------------------------------------------------- /pictures/multi-ugv-sim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/multi-ugv-sim.png -------------------------------------------------------------------------------- /pictures/px4rotor-100.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/px4rotor-100.gif -------------------------------------------------------------------------------- /pictures/sss-framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/sss-framework.png -------------------------------------------------------------------------------- /pictures/multi-tello-sim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/multi-tello-sim.png -------------------------------------------------------------------------------- /pictures/multi-px4-rotor-sim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/pictures/multi-px4-rotor-sim.png -------------------------------------------------------------------------------- /src/sss_sim_env/scripts/clock.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/sss_sim_env/scripts/clock.png -------------------------------------------------------------------------------- /src/ugv_sim/model/meshes/urm04.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/ugv_sim/model/meshes/urm04.STL -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/body.stl -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/rudder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/rudder.stl -------------------------------------------------------------------------------- /src/px4_rotor_sim/model/meshes/iris.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/px4_rotor_sim/model/meshes/iris.stl -------------------------------------------------------------------------------- /src/tello_sim/model/meshes/tello_body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/tello_sim/model/meshes/tello_body.stl -------------------------------------------------------------------------------- /src/ugv_sim/model/meshes/wheel_shaft.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/ugv_sim/model/meshes/wheel_shaft.STL -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/elevators.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/elevators.stl -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/left_flap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/left_flap.stl -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/iris_prop_cw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/iris_prop_cw.stl -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/left_aileron.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/left_aileron.stl -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/right_flap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/right_flap.stl -------------------------------------------------------------------------------- /src/tello_sim/model/meshes/tello_prop_ccw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/tello_sim/model/meshes/tello_prop_ccw.stl -------------------------------------------------------------------------------- /src/tello_sim/model/meshes/tello_prop_cw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/tello_sim/model/meshes/tello_prop_cw.stl -------------------------------------------------------------------------------- /src/ugv_sim/model/meshes/nexus_base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/ugv_sim/model/meshes/nexus_base_link.STL -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/iris_prop_ccw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/iris_prop_ccw.stl -------------------------------------------------------------------------------- /src/fw_plane_sim/model/meshes/right_aileron.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/model/meshes/right_aileron.stl -------------------------------------------------------------------------------- /src/px4_rotor_sim/model/meshes/iris_prop_ccw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/px4_rotor_sim/model/meshes/iris_prop_ccw.stl -------------------------------------------------------------------------------- /src/px4_rotor_sim/model/meshes/iris_prop_cw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/px4_rotor_sim/model/meshes/iris_prop_cw.stl -------------------------------------------------------------------------------- /src/ugv_sim/model/meshes/mecanum_wheel_left.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/ugv_sim/model/meshes/mecanum_wheel_left.STL -------------------------------------------------------------------------------- /src/ugv_sim/model/meshes/mecanum_wheel_right.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/ugv_sim/model/meshes/mecanum_wheel_right.STL -------------------------------------------------------------------------------- /src/fw_plane_sim/src/fw_sim/BHDynamic/libBHDynamic.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/src/fw_sim/BHDynamic/libBHDynamic.so -------------------------------------------------------------------------------- /src/ugv_sim/model/meshes/nexus_base_link_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/ugv_sim/model/meshes/nexus_base_link_collision.STL -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/README.md: -------------------------------------------------------------------------------- 1 | Official mavlink c and c++ v2.0 library cloned from https://github.com/mavlink/c_library_v2.git 2 | 3 | 2023-12-24 by Peixuan Shu 4 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/fw_sim/BHDynamic/backup/x86-20241001-formation/libBHDynamic.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/src/fw_sim/BHDynamic/backup/x86-20241001-formation/libBHDynamic.so -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # git ignore the following folders: 2 | /build 3 | /devel 4 | /install 5 | # /logs 6 | # src/CMakeLists.txt 7 | /.catkin_workspace 8 | /.catkin_tools 9 | # .vscode 10 | *.egg-info 11 | __pycache__ 12 | 13 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/fw_sim/BHDynamic/backup/arm64-20241112-guidance/libBHDynamic.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shupx/swarm_sync_sim/HEAD/src/fw_plane_sim/src/fw_sim/BHDynamic/backup/arm64-20241112-guidance/libBHDynamic.so -------------------------------------------------------------------------------- /src/sss_sim_env/srv/SimClockControl.srv: -------------------------------------------------------------------------------- 1 | # Service for control sim clock 2 | 3 | bool proceed # true to proceed, false to pause 4 | 5 | float32 max_sim_speed # simulation speed ratio 6 | float32 ORIGINAL_SPEED=0 # keep original sim speed 7 | --- 8 | bool success 9 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/message_definitions/standard.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | minimal.xml 5 | 0 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # Exit immediately if a command exits with a non-zero status. 4 | set -e 5 | 6 | # Dependencies 7 | sudo apt install ros-noetic-mavros ros-noetic-mavros-extras ros-noetic-robot-state-publisher -y 8 | pip3 install PyQt5 9 | 10 | # Build 11 | git clone https://gitee.com/bhswift/swarm_sync_sim.git 12 | cd swarm_sync_sim/ 13 | catkin_make 14 | echo "source $PWD/devel/setup.bash" >> ~/.bashrc 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/all/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from all.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/test/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/ASLUAV/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ASLUAV.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/AVSSUAS/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from AVSSUAS.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/common/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/icarous/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from icarous.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 46 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/minimal/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from minimal.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 22 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/standard/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from standard.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 22 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/storm32/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from storm32.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/csAirLink/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from csAirLink.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 100 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/cubepilot/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from cubepilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/paparazzi/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from paparazzi.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/uAvionix/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from uAvionix.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/ualberta/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ualberta.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mc_att_control/Kconfig: -------------------------------------------------------------------------------- 1 | menuconfig MODULES_MC_ATT_CONTROL 2 | bool "mc_att_control" 3 | default n 4 | ---help--- 5 | Enable support for mc_att_control 6 | 7 | menuconfig USER_MC_ATT_CONTROL 8 | bool "mc_att_control running as userspace module" 9 | default n 10 | depends on BOARD_PROTECTED && MODULES_MC_ATT_CONTROL 11 | ---help--- 12 | Put mc_att_control in userspace memory 13 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mc_pos_control/Kconfig: -------------------------------------------------------------------------------- 1 | menuconfig MODULES_MC_POS_CONTROL 2 | bool "mc_pos_control" 3 | default n 4 | ---help--- 5 | Enable support for mc_pos_control 6 | 7 | menuconfig USER_MC_POS_CONTROL 8 | bool "mc_pos_control running as userspace module" 9 | default y 10 | depends on BOARD_PROTECTED && MODULES_MC_POS_CONTROL 11 | ---help--- 12 | Put mc_pos_control in userspace memory 13 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/development/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from development.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/matrixpilot/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/ardupilotmega/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/python_array_test/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from python_array_test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Dec 14 2023" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/fw_plane_sim/plugins/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | Simulation node of fixed-wing UAV with flight controller. 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/mavros_sim/launch/mavros_sim_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/ugv_sim/plugins/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | Simulation node of Wheeltec UGV control and dynamics. 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/plugins/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | Simulation node of Mavros, PX4 and rotor dynamics. 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/tello_sim/plugins/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | Simulation node of Tello quadrotor control and dynamics. 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/sss_sim_env/include/sss_sim_env/sss_utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file sss_utils.hpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Some tools and functions utilized by other sim/real agent nodes. 5 | * 6 | * Note: This program relies on 7 | * 8 | * @version 1.0 9 | * @date 2023-12-9 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2023, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | #ifndef __SSS_UTILS__ 18 | #define __SSS_UTILS__ 19 | 20 | #include "sss_sim_env/createTimer.hpp" 21 | #include "sss_sim_env/Timer.hpp" 22 | #include "sss_sim_env/Sleep.hpp" 23 | 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/matrix/math.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "stdlib_imports.hpp" 5 | #ifdef __PX4_QURT 6 | #include "dspal_math.h" 7 | #endif 8 | #include "helper_functions.hpp" 9 | #include "Matrix.hpp" 10 | #include "SquareMatrix.hpp" 11 | #include "Slice.hpp" 12 | #include "Vector.hpp" 13 | #include "Vector2.hpp" 14 | #include "Vector3.hpp" 15 | #include "Euler.hpp" 16 | #include "Dcm.hpp" 17 | #include "Scalar.hpp" 18 | #include "Quaternion.hpp" 19 | #include "AxisAngle.hpp" 20 | #include "LeastSquaresSolver.hpp" 21 | #include "Dual.hpp" 22 | #include "PseudoInverse.hpp" 23 | #include "SparseVector.hpp" 24 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/matrix/math.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "stdlib_imports.hpp" 5 | #ifdef __PX4_QURT 6 | #include "dspal_math.h" 7 | #endif 8 | #include "helper_functions.hpp" 9 | #include "Matrix.hpp" 10 | #include "SquareMatrix.hpp" 11 | #include "Slice.hpp" 12 | #include "Vector.hpp" 13 | #include "Vector2.hpp" 14 | #include "Vector3.hpp" 15 | #include "Euler.hpp" 16 | #include "Dcm.hpp" 17 | #include "Scalar.hpp" 18 | #include "Quaternion.hpp" 19 | #include "AxisAngle.hpp" 20 | #include "LeastSquaresSolver.hpp" 21 | #include "Dual.hpp" 22 | #include "PseudoInverse.hpp" 23 | #include "SparseVector.hpp" 24 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/matrix/filter.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "math.hpp" 4 | 5 | namespace matrix 6 | { 7 | 8 | template 9 | int kalman_correct( 10 | const Matrix &P, 11 | const Matrix &C, 12 | const Matrix &R, 13 | const Matrix &r, 14 | Matrix &dx, 15 | Matrix &dP, 16 | Type &beta 17 | ) 18 | { 19 | SquareMatrix S_I = SquareMatrix(C * P * C.T() + R).I(); 20 | Matrix K = P * C.T() * S_I; 21 | dx = K * r; 22 | beta = Scalar(r.T() * S_I * r); 23 | dP = K * C * P * (-1); 24 | return 0; 25 | } 26 | 27 | } // namespace matrix 28 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/matrix/filter.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "math.hpp" 4 | 5 | namespace matrix 6 | { 7 | 8 | template 9 | int kalman_correct( 10 | const Matrix &P, 11 | const Matrix &C, 12 | const Matrix &R, 13 | const Matrix &r, 14 | Matrix &dx, 15 | Matrix &dP, 16 | Type &beta 17 | ) 18 | { 19 | SquareMatrix S_I = SquareMatrix(C * P * C.T() + R).I(); 20 | Matrix K = P * C.T() * S_I; 21 | dx = K * r; 22 | beta = Scalar(r.T() * S_I * r); 23 | dP = K * C * P * (-1); 24 | return 0; 25 | } 26 | 27 | } // namespace matrix 28 | -------------------------------------------------------------------------------- /src/sss_sim_env/plugins/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 13 | 14 | 15 | Simulation clock that determines the simulation time process. 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/all/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from all.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -931073999192403249 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "all.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/test/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 8697368313095621257 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "test.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/ASLUAV/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ASLUAV.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 6087441405184234891 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "ASLUAV.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/AVSSUAS/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from AVSSUAS.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 45282055904297250 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "AVSSUAS.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/common/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 6210132856844804598 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "common.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/icarous/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from icarous.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 3009406400933093156 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "icarous.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/minimal/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from minimal.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 6990176576158435032 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "minimal.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/standard/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from standard.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -557885868295501135 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "standard.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/storm32/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from storm32.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -5103317512902396477 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "storm32.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/uAvionix/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from uAvionix.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -3697130354912404086 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "uAvionix.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/ualberta/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ualberta.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -4355782560967296552 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "ualberta.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/csAirLink/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from csAirLink.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -5686112884452948951 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "csAirLink.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/cubepilot/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from cubepilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 6596611090383384209 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "cubepilot.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/paparazzi/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from paparazzi.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 2241368053555884668 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "paparazzi.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/development/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from development.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 7179464087613887894 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "development.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/matrixpilot/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -2094448179346179136 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "matrixpilot.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/ardupilotmega/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -189277157834347460 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "ardupilotmega.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/vehicle_local_position_setpoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Peixuan Shu (shupeixuan@qq.com) 3 | * @brief simulated uorb topic struct definition 4 | * 5 | * 6 | * @version 1.0 7 | * @date 2023-11-28 8 | * 9 | * @license BSD 3-Clause License 10 | * @copyright (c) 2023, Peixuan Shu 11 | * All rights reserved. 12 | * 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | struct vehicle_local_position_setpoint_s { 22 | 23 | uint64_t timestamp; 24 | float x; 25 | float y; 26 | float z; 27 | float yaw; 28 | float yawspeed; 29 | float vx; 30 | float vy; 31 | float vz; 32 | float acceleration[3]; 33 | float jerk[3]; 34 | float thrust[3]; 35 | uint8_t _padding0[4]; // required for logger 36 | 37 | }; 38 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/python_array_test/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from python_array_test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -2984496276416024277 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "python_array_test.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/vehicle_local_position_setpoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Peixuan Shu (shupeixuan@qq.com) 3 | * @brief simulated uorb topic struct definition 4 | * 5 | * 6 | * @version 1.0 7 | * @date 2023-11-28 8 | * 9 | * @license BSD 3-Clause License 10 | * @copyright (c) 2023, Peixuan Shu 11 | * All rights reserved. 12 | * 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | struct vehicle_local_position_setpoint_s { 22 | 23 | uint64_t timestamp; 24 | float x; 25 | float y; 26 | float z; 27 | float yaw; 28 | float yawspeed; 29 | float vx; 30 | float vy; 31 | float vz; 32 | float acceleration[3]; 33 | float jerk[3]; 34 | float thrust[3]; 35 | uint8_t _padding0[4]; // required for logger 36 | 37 | }; 38 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "includePath": [ 5 | "/opt/ros/noetic/include/**", 6 | "/usr/include/**", 7 | "./src/sss_sim_env/include/", 8 | "./src/px4_rotor_sim/include/", 9 | "./src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/", 10 | "./src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib", 11 | "./src/tello_sim/include/", 12 | "./src/ugv_sim/include/", 13 | "./devel/include/" 14 | ], 15 | "name":"ROS", 16 | "intelliSenseMode": "linux-gcc-x64", 17 | "compilerPath": "/usr/bin/gcc", 18 | "cStandard": "c17", 19 | "cppStandard": "gnu++14" 20 | } 21 | ], 22 | "version": 4 23 | } -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/parameters/px4_parameters.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copied from px4_sitl_default build. 3 | * Modified by Peixuan Shu 4 | * Store all global(extern) px4 parameters 5 | * Peixuan Shu 6 | */ 7 | 8 | #include "px4_parameters.hpp" 9 | 10 | namespace px4 { 11 | 12 | std::vector parameters_default(parameters, parameters+sizeof(parameters)/sizeof(param_info_s)); // used as the default element of parameters_vectors // added by Peixuan Shu 13 | 14 | 15 | // Store global(extern) px4 parameters of all UAVs (Created by Peixuan Shu) 16 | // initialize with at least one element for UAV 1 17 | std::vector> parameters_vectors{parameters_default}; // define (allocate storage) 18 | 19 | 20 | /* allocate global storage for messages of agent i */ 21 | void allocate_px4_params_storage(int expected_agent_num) 22 | { 23 | while(parameters_vectors.size() < expected_agent_num) 24 | { 25 | parameters_vectors.emplace_back(parameters_default); 26 | } 27 | } 28 | 29 | } // namespace px4 -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/vehicle_attitude_setpoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Peixuan Shu (shupeixuan@qq.com) 3 | * @brief simulated uorb topic struct definition 4 | * 5 | * 6 | * @version 1.0 7 | * @date 2023-11-28 8 | * 9 | * @license BSD 3-Clause License 10 | * @copyright (c) 2023, Peixuan Shu 11 | * All rights reserved. 12 | * 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | struct vehicle_attitude_setpoint_s { 22 | uint64_t timestamp; 23 | float roll_body; 24 | float pitch_body; 25 | float yaw_body; 26 | float yaw_sp_move_rate; 27 | float q_d[4]; 28 | float thrust_body[3]; 29 | bool roll_reset_integral; 30 | bool pitch_reset_integral; 31 | bool yaw_reset_integral; 32 | bool fw_control_yaw; 33 | uint8_t apply_flaps; 34 | uint8_t _padding0[7]; // required for logger 35 | 36 | static constexpr uint8_t FLAPS_OFF = 0; 37 | static constexpr uint8_t FLAPS_LAND = 1; 38 | static constexpr uint8_t FLAPS_TAKEOFF = 2; 39 | 40 | }; 41 | 42 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/parameters/px4_parameters.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copied from px4_sitl_default build. 3 | * Modified by Peixuan Shu 4 | * Store all global(extern) px4 parameters 5 | * Peixuan Shu 6 | */ 7 | 8 | #include "px4_parameters.hpp" 9 | 10 | namespace px4 { 11 | 12 | std::vector parameters_default(parameters, parameters+sizeof(parameters)/sizeof(param_info_s)); // used as the default element of parameters_vectors // added by Peixuan Shu 13 | 14 | 15 | // Store global(extern) px4 parameters of all UAVs (Created by Peixuan Shu) 16 | // initialize with at least one element for UAV 1 17 | std::vector> parameters_vectors{parameters_default}; // define (allocate storage) 18 | 19 | 20 | /* allocate global storage for messages of agent i */ 21 | void allocate_px4_params_storage(int expected_agent_num) 22 | { 23 | while(parameters_vectors.size() < expected_agent_num) 24 | { 25 | parameters_vectors.emplace_back(parameters_default); 26 | } 27 | } 28 | 29 | } // namespace px4 -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/vehicle_attitude_setpoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Peixuan Shu (shupeixuan@qq.com) 3 | * @brief simulated uorb topic struct definition 4 | * 5 | * 6 | * @version 1.0 7 | * @date 2023-11-28 8 | * 9 | * @license BSD 3-Clause License 10 | * @copyright (c) 2023, Peixuan Shu 11 | * All rights reserved. 12 | * 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | struct vehicle_attitude_setpoint_s { 22 | uint64_t timestamp; 23 | float roll_body; 24 | float pitch_body; 25 | float yaw_body; 26 | float yaw_sp_move_rate; 27 | float q_d[4]; 28 | float thrust_body[3]; 29 | bool roll_reset_integral; 30 | bool pitch_reset_integral; 31 | bool yaw_reset_integral; 32 | bool fw_control_yaw; 33 | uint8_t apply_flaps; 34 | uint8_t _padding0[7]; // required for logger 35 | 36 | static constexpr uint8_t FLAPS_OFF = 0; 37 | static constexpr uint8_t FLAPS_LAND = 1; 38 | static constexpr uint8_t FLAPS_TAKEOFF = 2; 39 | 40 | }; 41 | 42 | -------------------------------------------------------------------------------- /src/sss_sim_env/launch/sim_clock.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/matrix/integration.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "math.hpp" 4 | 5 | namespace matrix 6 | { 7 | 8 | template 9 | int integrate_rk4( 10 | Vector (*f)(Type, const Matrix &x, const Matrix &u), 11 | const Matrix &y0, 12 | const Matrix &u, 13 | Type t0, 14 | Type tf, 15 | Type h0, 16 | Matrix &y1 17 | ) 18 | { 19 | // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods 20 | Type t1 = t0; 21 | y1 = y0; 22 | Type h = h0; 23 | Vector k1, k2, k3, k4; 24 | 25 | if (tf < t0) { return -1; } // make sure t1 > t0 26 | 27 | while (t1 < tf) { 28 | if (t1 + h0 < tf) { 29 | h = h0; 30 | 31 | } else { 32 | h = tf - t1; 33 | } 34 | 35 | k1 = f(t1, y1, u); 36 | k2 = f(t1 + h / 2, y1 + k1 * h / 2, u); 37 | k3 = f(t1 + h / 2, y1 + k2 * h / 2, u); 38 | k4 = f(t1 + h, y1 + k3 * h, u); 39 | y1 += (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6); 40 | t1 += h; 41 | } 42 | 43 | return 0; 44 | } 45 | 46 | } // namespace matrix 47 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/standard/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from standard.xml 3 | * @see https://mavlink.io/en/ 4 | */ 5 | #pragma once 6 | #ifndef STANDARD_TESTSUITE_H 7 | #define STANDARD_TESTSUITE_H 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | #ifndef MAVLINK_TEST_ALL 14 | #define MAVLINK_TEST_ALL 15 | static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); 16 | static void mavlink_test_standard(uint8_t, uint8_t, mavlink_message_t *last_msg); 17 | 18 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 19 | { 20 | mavlink_test_minimal(system_id, component_id, last_msg); 21 | mavlink_test_standard(system_id, component_id, last_msg); 22 | } 23 | #endif 24 | 25 | #include "../minimal/testsuite.h" 26 | 27 | 28 | 29 | static void mavlink_test_standard(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 30 | { 31 | 32 | } 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif // __cplusplus 37 | #endif // STANDARD_TESTSUITE_H 38 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/matrix/integration.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "math.hpp" 4 | 5 | namespace matrix 6 | { 7 | 8 | template 9 | int integrate_rk4( 10 | Vector (*f)(Type, const Matrix &x, const Matrix &u), 11 | const Matrix &y0, 12 | const Matrix &u, 13 | Type t0, 14 | Type tf, 15 | Type h0, 16 | Matrix &y1 17 | ) 18 | { 19 | // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods 20 | Type t1 = t0; 21 | y1 = y0; 22 | Type h = h0; 23 | Vector k1, k2, k3, k4; 24 | 25 | if (tf < t0) { return -1; } // make sure t1 > t0 26 | 27 | while (t1 < tf) { 28 | if (t1 + h0 < tf) { 29 | h = h0; 30 | 31 | } else { 32 | h = tf - t1; 33 | } 34 | 35 | k1 = f(t1, y1, u); 36 | k2 = f(t1 + h / 2, y1 + k1 * h / 2, u); 37 | k3 = f(t1 + h / 2, y1 + k2 * h / 2, u); 38 | k4 = f(t1 + h, y1 + k3 * h, u); 39 | y1 += (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6); 40 | t1 += h; 41 | } 42 | 43 | return 0; 44 | } 45 | 46 | } // namespace matrix 47 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/matrix/Scalar.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Scalar.hpp 3 | * 4 | * Defines conversion of matrix to scalar. 5 | * 6 | * @author James Goppert 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "math.hpp" 12 | 13 | namespace matrix 14 | { 15 | 16 | template 17 | class Scalar 18 | { 19 | public: 20 | Scalar() = delete; 21 | 22 | Scalar(const Matrix &other) : 23 | _value{other(0, 0)} 24 | { 25 | } 26 | 27 | Scalar(Type other) : _value(other) 28 | { 29 | } 30 | 31 | operator const Type &() 32 | { 33 | return _value; 34 | } 35 | 36 | operator Matrix() const 37 | { 38 | Matrix m; 39 | m(0, 0) = _value; 40 | return m; 41 | } 42 | 43 | operator Vector() const 44 | { 45 | Vector m; 46 | m(0) = _value; 47 | return m; 48 | } 49 | 50 | bool operator==(const float other) const 51 | { 52 | return isEqualF(_value, other); 53 | } 54 | 55 | private: 56 | const Type _value; 57 | 58 | }; 59 | 60 | using Scalarf = Scalar; 61 | using Scalard = Scalar; 62 | 63 | } // namespace matrix 64 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/fw_plane_visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file fw_plane_visualizer_node.cpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Publish rotor propeller joint position and base_link tf states for the robot model visualization in rviz 5 | * 6 | * Note: This program relies on mavros, px4 geo.h 7 | * 8 | * @version 1.0 9 | * @date 2024-8-29 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2024, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | #include 18 | #include "fw_plane_sim/fw_plane_visualizer.hpp" 19 | 20 | using namespace FwPlaneSimulator; 21 | 22 | int main(int argc, char **argv) 23 | { 24 | ros::init(argc, argv, "fw_plane_visualizer"); 25 | ros::NodeHandle nh; 26 | ros::NodeHandle nh_private("~"); 27 | 28 | //Use unique_ptr to auto-destory the object when exiting. 29 | std::unique_ptr visualizer(new Visualizer(nh, nh_private)); 30 | 31 | ros::Rate loop_rate(50); // Hz 32 | while (ros::ok()) 33 | { 34 | visualizer->Run(); 35 | 36 | ros::spinOnce(); 37 | loop_rate.sleep(); 38 | } 39 | return 0; 40 | } -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/matrix/Scalar.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Scalar.hpp 3 | * 4 | * Defines conversion of matrix to scalar. 5 | * 6 | * @author James Goppert 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "math.hpp" 12 | 13 | namespace matrix 14 | { 15 | 16 | template 17 | class Scalar 18 | { 19 | public: 20 | Scalar() = delete; 21 | 22 | Scalar(const Matrix &other) : 23 | _value{other(0, 0)} 24 | { 25 | } 26 | 27 | Scalar(Type other) : _value(other) 28 | { 29 | } 30 | 31 | operator const Type &() 32 | { 33 | return _value; 34 | } 35 | 36 | operator Matrix() const 37 | { 38 | Matrix m; 39 | m(0, 0) = _value; 40 | return m; 41 | } 42 | 43 | operator Vector() const 44 | { 45 | Vector m; 46 | m(0) = _value; 47 | return m; 48 | } 49 | 50 | bool operator==(const float other) const 51 | { 52 | return isEqualF(_value, other); 53 | } 54 | 55 | private: 56 | const Type _value; 57 | 58 | }; 59 | 60 | using Scalarf = Scalar; 61 | using Scalard = Scalar; 62 | 63 | } // namespace matrix 64 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/drone_visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file drone_visualizer_node.cpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Publish rotor propeller joint position and base_link tf states for the robot model visualization in rviz 5 | * 6 | * Note: This program relies on mavros, px4 geo.h 7 | * 8 | * @version 1.0 9 | * @date 2023-12-30 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2023, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | #include 18 | #include "mavros_px4_quadrotor_sim/drone_visualizer.hpp" 19 | 20 | using namespace MavrosQuadSimulator; 21 | 22 | int main(int argc, char **argv) 23 | { 24 | ros::init(argc, argv, "drone_visualizer"); 25 | ros::NodeHandle nh; 26 | ros::NodeHandle nh_private("~"); 27 | 28 | //Use unique_ptr to auto-destory the object when exiting. 29 | std::unique_ptr visualizer(new Visualizer(nh, nh_private)); 30 | 31 | ros::Rate loop_rate(50); // Hz 32 | while (ros::ok()) 33 | { 34 | visualizer->Run(); 35 | 36 | ros::spinOnce(); 37 | loop_rate.sleep(); 38 | } 39 | return 0; 40 | } -------------------------------------------------------------------------------- /src/fw_plane_sim/include/fw_plane_sim/fw_sim/fw_sim_node.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file fw_sim_node.hpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Fixed wing flight controller driver sim + quadrotor_dynamics. Main loop 5 | * 6 | * Note: This program relies on fw_driver_sim, ss_utils 7 | * 8 | * @version 1.0 9 | * @date 2024-10-02 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2024, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | 18 | 19 | #pragma once 20 | 21 | #include 22 | #include 23 | 24 | #include "BHDynamic.h" 25 | #include "fw_plane_sim/fw_sim/fw_driver_sim.hpp" 26 | 27 | 28 | namespace FwSimulator 29 | { 30 | 31 | class Agent 32 | { 33 | public: 34 | Agent(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 35 | 36 | private: 37 | 38 | ros::NodeHandle nh_; 39 | ros::NodeHandle nh_private_; 40 | 41 | std::shared_ptr fw_driver_sim_; 42 | 43 | double mainloop_period_; 44 | double mainloop_last_time_ = 0; 45 | sss_utils::Timer mainloop_timer_; 46 | 47 | void mainloop(const ros::TimerEvent &event); 48 | }; 49 | 50 | } -------------------------------------------------------------------------------- /src/ugv_sim/include/ugv_sim/wheeltec_ugv_sim_node.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file wheeltec_ugv_sim_node.hpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Tello driver sim + quadrotor_dynamics. Main loop 5 | * 6 | * Note: This program relies on wheeltec_driver_sim, dynamics and sss_utils 7 | * 8 | * @version 1.0 9 | * @date 2024-1-23 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2024, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | 23 | #include "ugv_sim/ugv_dynamics.hpp" 24 | #include "ugv_sim/wheeltec_driver_sim.hpp" 25 | 26 | 27 | namespace WheeltecUgvSimulator 28 | { 29 | 30 | class Agent 31 | { 32 | public: 33 | Agent(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 34 | 35 | private: 36 | 37 | ros::NodeHandle nh_; 38 | ros::NodeHandle nh_private_; 39 | 40 | std::shared_ptr dynamics_; 41 | std::shared_ptr wheeltec_driver_sim_; 42 | 43 | double mainloop_period_; 44 | double mainloop_last_time_ = 0; 45 | sss_utils::Timer mainloop_timer_; 46 | 47 | void mainloop(const ros::TimerEvent &event); 48 | }; 49 | 50 | } -------------------------------------------------------------------------------- /src/tello_sim/include/tello_sim/tello_quadrotor_sim_node.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tello_quadrotor_sim.hpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Tello controller + quadrotor_dynamics. Main loop 5 | * 6 | * Note: This program relies on tello driver sim, dynamics and sss_utils 7 | * 8 | * @version 1.0 9 | * @date 2024-1-16 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2024, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | 23 | #include "tello_sim/quadrotor_dynamics.hpp" 24 | #include "tello_sim/tello_driver_sim.hpp" 25 | 26 | 27 | namespace TelloQuadSimulator 28 | { 29 | 30 | class Agent 31 | { 32 | public: 33 | Agent(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 34 | 35 | private: 36 | 37 | ros::NodeHandle nh_; 38 | ros::NodeHandle nh_private_; 39 | 40 | // std::shared_ptr tello_driver_sim_(nh_, nh_private_); 41 | std::shared_ptr dynamics_; 42 | std::shared_ptr tello_driver_sim_; 43 | 44 | double mainloop_period_; 45 | double mainloop_last_time_ = 0; 46 | sss_utils::Timer mainloop_timer_; 47 | 48 | void mainloop(const ros::TimerEvent &event); 49 | }; 50 | 51 | } -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_compile_options( 2 | -Wno-double-promotion 3 | -Wno-float-equal 4 | ) 5 | 6 | px4_add_unit_gtest(SRC MatrixAssignmentTest.cpp) 7 | px4_add_unit_gtest(SRC MatrixAttitudeTest.cpp) 8 | px4_add_unit_gtest(SRC MatrixCopyToTest.cpp) 9 | px4_add_unit_gtest(SRC MatrixDualTest.cpp) 10 | px4_add_unit_gtest(SRC MatrixFilterTest.cpp) 11 | px4_add_unit_gtest(SRC MatrixHatveeTest.cpp) 12 | px4_add_unit_gtest(SRC MatrixHelperTest.cpp) 13 | px4_add_unit_gtest(SRC MatrixIntegralTest.cpp) 14 | px4_add_unit_gtest(SRC MatrixInverseTest.cpp) 15 | px4_add_unit_gtest(SRC MatrixLeastSquaresTest.cpp) 16 | px4_add_unit_gtest(SRC MatrixMultiplicationTest.cpp) 17 | px4_add_unit_gtest(SRC MatrixPseudoInverseTest.cpp) 18 | px4_add_unit_gtest(SRC MatrixScalarMultiplicationTest.cpp) 19 | px4_add_unit_gtest(SRC MatrixSetIdentityTest.cpp) 20 | px4_add_unit_gtest(SRC MatrixSliceTest.cpp) 21 | px4_add_unit_gtest(SRC MatrixSparseVectorTest.cpp) 22 | px4_add_unit_gtest(SRC MatrixSquareTest.cpp) 23 | px4_add_unit_gtest(SRC MatrixTransposeTest.cpp) 24 | px4_add_unit_gtest(SRC MatrixVectorTest.cpp) 25 | px4_add_unit_gtest(SRC MatrixUnwrapTest.cpp) 26 | px4_add_unit_gtest(SRC MatrixUpperRightTriangleTest.cpp) 27 | px4_add_unit_gtest(SRC MatrixVector2Test.cpp) 28 | px4_add_unit_gtest(SRC MatrixVector3Test.cpp) 29 | px4_add_unit_gtest(SRC MatrixVectorAssignmentTest.cpp) 30 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_compile_options( 2 | -Wno-double-promotion 3 | -Wno-float-equal 4 | ) 5 | 6 | px4_add_unit_gtest(SRC MatrixAssignmentTest.cpp) 7 | px4_add_unit_gtest(SRC MatrixAttitudeTest.cpp) 8 | px4_add_unit_gtest(SRC MatrixCopyToTest.cpp) 9 | px4_add_unit_gtest(SRC MatrixDualTest.cpp) 10 | px4_add_unit_gtest(SRC MatrixFilterTest.cpp) 11 | px4_add_unit_gtest(SRC MatrixHatveeTest.cpp) 12 | px4_add_unit_gtest(SRC MatrixHelperTest.cpp) 13 | px4_add_unit_gtest(SRC MatrixIntegralTest.cpp) 14 | px4_add_unit_gtest(SRC MatrixInverseTest.cpp) 15 | px4_add_unit_gtest(SRC MatrixLeastSquaresTest.cpp) 16 | px4_add_unit_gtest(SRC MatrixMultiplicationTest.cpp) 17 | px4_add_unit_gtest(SRC MatrixPseudoInverseTest.cpp) 18 | px4_add_unit_gtest(SRC MatrixScalarMultiplicationTest.cpp) 19 | px4_add_unit_gtest(SRC MatrixSetIdentityTest.cpp) 20 | px4_add_unit_gtest(SRC MatrixSliceTest.cpp) 21 | px4_add_unit_gtest(SRC MatrixSparseVectorTest.cpp) 22 | px4_add_unit_gtest(SRC MatrixSquareTest.cpp) 23 | px4_add_unit_gtest(SRC MatrixTransposeTest.cpp) 24 | px4_add_unit_gtest(SRC MatrixVectorTest.cpp) 25 | px4_add_unit_gtest(SRC MatrixUnwrapTest.cpp) 26 | px4_add_unit_gtest(SRC MatrixUpperRightTriangleTest.cpp) 27 | px4_add_unit_gtest(SRC MatrixVector2Test.cpp) 28 | px4_add_unit_gtest(SRC MatrixVector3Test.cpp) 29 | px4_add_unit_gtest(SRC MatrixVectorAssignmentTest.cpp) 30 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/fw_sim/BHDynamic/backup/x86-20241001-formation/BHDynamic.h: -------------------------------------------------------------------------------- 1 | #ifndef __BHDYNAMICS_H__ 2 | #define __BHDYNAMICS_H__ 3 | 4 | // BHDynamics.h : 头文件 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | /* 这种方式确保当头文件被 C++ 编译器编译时，函数按照 C 的方式导出。 10 | 当头文件被 C 编译器编译时，extern "C" 不会起作用，因为 C 编译器不识别这个关键字。*/ 11 | 12 | 13 | typedef struct 14 | { 15 | double posiNInit; // 北向初始位置，m 16 | double posiEInit; // 东向初始位置，m 17 | double posiDInit; // 地向初始位置，m 18 | double velInit; // 初始速度，m/s 19 | double pitchInit; // 初始俯仰角，rad 20 | double yawInit; // 初始偏航角，rad 21 | double rollInit; // 初始滚转角，rad 22 | } State_Init; // 初始化状态结构体 23 | 24 | typedef struct 25 | { 26 | double posiNState; // 北向位置输出，m 27 | double posiEState; // 东向位置输出，m 28 | double posiDState; // 地向位置输出，m 29 | double pitchState; // 俯仰角输出，rad 30 | double yawState; // 偏航角输出，rad 31 | double rollState; // 滚转角输出，rad 32 | double velState; // 和速度输出，m/s 33 | double velalphaState; // 速度倾角，输出暂为0 34 | double velbeteState; // 速度偏角，输出暂为0 35 | double alphaState; // 攻角，输出暂为0 36 | double beteState; // 侧滑角，输出暂为0 37 | } State_Output; // 输出状态结构体 38 | 39 | // State_Init baseState; 40 | // State_Output GS_state; 41 | 42 | void InitState(State_Init baseState); 43 | State_Output OutLoopCtrl_1(double High_input, double Vel_input, double Roll_input); 44 | State_Output OutLoopCtrl_2(double High_input, double Vel_input, double Yaw_input); 45 | 46 | #ifdef __cplusplus 47 | } 48 | #endif 49 | 50 | #endif // __BHDYNAMICS_H__ -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mavlink/mavlink_msg_list.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mavlink_msg_list.cpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Store the mavlink messages to simulate the send and receive of mavlink messages. 5 | * 6 | * Note: This program relies on 7 | * 8 | * @version 1.0 9 | * @date 2023-12-17 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2023, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | 18 | #include "mavlink_msg_list.hpp" 19 | 20 | 21 | namespace px4 22 | { 23 | 24 | // Init mavlink lists here (they are global/extern!) 25 | 26 | // initialize with at least one element for UAV 1 27 | std::vector> mavlink_stream_lists{std::array{}}; // define (allocate storage) 28 | 29 | // initialize with at least one element for UAV 1 30 | std::vector> mavlink_receive_lists {std::array{}}; // define (allocate storage) 31 | 32 | 33 | /* allocate global storage for messages of agent i */ 34 | void allocate_mavlink_message_storage(int expected_agent_num) 35 | { 36 | while(mavlink_stream_lists.size() < expected_agent_num) 37 | { 38 | mavlink_stream_lists.emplace_back(std::array{}); 39 | } 40 | while(mavlink_receive_lists.size() < expected_agent_num) 41 | { 42 | mavlink_receive_lists.emplace_back(std::array{}); 43 | } 44 | } 45 | 46 | } -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/matrix/Vector2.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Vector2.hpp 3 | * 4 | * 2D vector class. 5 | * 6 | * @author James Goppert 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "math.hpp" 12 | 13 | namespace matrix 14 | { 15 | 16 | template 17 | class Vector; 18 | 19 | template 20 | class Vector2 : public Vector 21 | { 22 | public: 23 | 24 | using Matrix21 = Matrix; 25 | using Vector3 = Vector; 26 | 27 | Vector2() = default; 28 | 29 | Vector2(const Matrix21 &other) : 30 | Vector(other) 31 | { 32 | } 33 | 34 | explicit Vector2(const Type data_[2]) : 35 | Vector(data_) 36 | { 37 | } 38 | 39 | Vector2(Type x, Type y) 40 | { 41 | Vector2 &v(*this); 42 | v(0) = x; 43 | v(1) = y; 44 | } 45 | 46 | template 47 | Vector2(const Slice &slice_in) : Vector(slice_in) 48 | { 49 | } 50 | 51 | template 52 | Vector2(const Slice &slice_in) : Vector(slice_in) 53 | { 54 | } 55 | 56 | explicit Vector2(const Vector3 &other) 57 | { 58 | Vector2 &v(*this); 59 | v(0) = other(0); 60 | v(1) = other(1); 61 | } 62 | 63 | Type cross(const Matrix21 &b) const 64 | { 65 | const Vector2 &a(*this); 66 | return a(0) * b(1, 0) - a(1) * b(0, 0); 67 | } 68 | 69 | Type operator%(const Matrix21 &b) const 70 | { 71 | return (*this).cross(b); 72 | } 73 | 74 | }; 75 | 76 | 77 | using Vector2f = Vector2; 78 | using Vector2d = Vector2; 79 | 80 | } // namespace matrix 81 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Peixuan Shu 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/matrix/Vector2.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Vector2.hpp 3 | * 4 | * 2D vector class. 5 | * 6 | * @author James Goppert 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "math.hpp" 12 | 13 | namespace matrix 14 | { 15 | 16 | template 17 | class Vector; 18 | 19 | template 20 | class Vector2 : public Vector 21 | { 22 | public: 23 | 24 | using Matrix21 = Matrix; 25 | using Vector3 = Vector; 26 | 27 | Vector2() = default; 28 | 29 | Vector2(const Matrix21 &other) : 30 | Vector(other) 31 | { 32 | } 33 | 34 | explicit Vector2(const Type data_[2]) : 35 | Vector(data_) 36 | { 37 | } 38 | 39 | Vector2(Type x, Type y) 40 | { 41 | Vector2 &v(*this); 42 | v(0) = x; 43 | v(1) = y; 44 | } 45 | 46 | template 47 | Vector2(const Slice &slice_in) : Vector(slice_in) 48 | { 49 | } 50 | 51 | template 52 | Vector2(const Slice &slice_in) : Vector(slice_in) 53 | { 54 | } 55 | 56 | explicit Vector2(const Vector3 &other) 57 | { 58 | Vector2 &v(*this); 59 | v(0) = other(0); 60 | v(1) = other(1); 61 | } 62 | 63 | Type cross(const Matrix21 &b) const 64 | { 65 | const Vector2 &a(*this); 66 | return a(0) * b(1, 0) - a(1) * b(0, 0); 67 | } 68 | 69 | Type operator%(const Matrix21 &b) const 70 | { 71 | return (*this).cross(b); 72 | } 73 | 74 | }; 75 | 76 | 77 | using Vector2f = Vector2; 78 | using Vector2d = Vector2; 79 | 80 | } // namespace matrix 81 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/test/test.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol generated from test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_TEST_H 7 | #define MAVLINK_TEST_H 8 | 9 | #ifndef MAVLINK_H 10 | #error Wrong include order: MAVLINK_TEST.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. 11 | #endif 12 | 13 | #define MAVLINK_TEST_XML_HASH 8697368313095621257 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | 19 | // MESSAGE LENGTHS AND CRCS 20 | 21 | #ifndef MAVLINK_MESSAGE_LENGTHS 22 | #define MAVLINK_MESSAGE_LENGTHS {} 23 | #endif 24 | 25 | #ifndef MAVLINK_MESSAGE_CRCS 26 | #define MAVLINK_MESSAGE_CRCS {{17000, 103, 179, 179, 0, 0, 0}} 27 | #endif 28 | 29 | #include "../protocol.h" 30 | 31 | #define MAVLINK_ENABLED_TEST 32 | 33 | // ENUM DEFINITIONS 34 | 35 | 36 | 37 | // MAVLINK VERSION 38 | 39 | #ifndef MAVLINK_VERSION 40 | #define MAVLINK_VERSION 3 41 | #endif 42 | 43 | #if (MAVLINK_VERSION == 0) 44 | #undef MAVLINK_VERSION 45 | #define MAVLINK_VERSION 3 46 | #endif 47 | 48 | // MESSAGE DEFINITIONS 49 | #include "./mavlink_msg_test_types.h" 50 | 51 | // base include 52 | 53 | 54 | 55 | #if MAVLINK_TEST_XML_HASH == MAVLINK_PRIMARY_XML_HASH 56 | # define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES} 57 | # define MAVLINK_MESSAGE_NAMES {{ "TEST_TYPES", 17000 }} 58 | # if MAVLINK_COMMAND_24BIT 59 | # include "../mavlink_get_info.h" 60 | # endif 61 | #endif 62 | 63 | #ifdef __cplusplus 64 | } 65 | #endif // __cplusplus 66 | #endif // MAVLINK_TEST_H 67 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/message_definitions/test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 3 4 | 5 | 6 | Test all field types 7 | char 8 | string 9 | uint8_t 10 | uint16_t 11 | uint32_t 12 | uint64_t 13 | int8_t 14 | int16_t 15 | int32_t 16 | int64_t 17 | float 18 | double 19 | uint8_t_array 20 | uint16_t_array 21 | uint32_t_array 22 | uint64_t_array 23 | int8_t_array 24 | int16_t_array 25 | int32_t_array 26 | int64_t_array 27 | float_array 28 | double_array 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/ugv_sim/model/urm04_sensor.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | true 9 | ${update_rate} 10 | 0 0 0 0 0 0 11 | false 12 | 13 | 14 | 15 | ${ray_count} 16 | 1 17 | -${horizontal_field_of_view/2} 18 | ${horizontal_field_of_view/2} 19 | 20 | 21 | ${ray_count} 22 | 1 23 | -${vertical_field_of_view/2} 24 | ${vertical_field_of_view/2} 25 | 26 | 27 | 28 | ${min_range} 29 | ${max_range} 30 | 0.01 31 | 32 | 33 | 34 | 35 | 0.005 36 | ${ros_topic} 37 | ${frame_name} 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/fw_sim/BHDynamic/backup/arm64-20241112-guidance/BHDynamic.h: -------------------------------------------------------------------------------- 1 | #ifndef __BHDYNAMICS_H__ 2 | #define __BHDYNAMICS_H__ 3 | 4 | // BHDynamics.h : 头文件 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | /* 这种方式确保当头文件被 C++ 编译器编译时，函数按照 C 的方式导出。 10 | 当头文件被 C 编译器编译时，extern "C" 不会起作用，因为 C 编译器不识别这个关键字。*/ 11 | 12 | typedef struct 13 | { 14 | double posiNInit; // 北向初始位置，m 15 | double posiEInit; // 东向初始位置，m 16 | double posiDInit; // 地向初始位置，m 17 | double velInit; // 初始速度，m/s 18 | double pitchInit; // 初始俯仰角，rad 19 | double yawInit; // 初始偏航角，rad 20 | double rollInit; // 初始滚转角，rad 21 | } State_Init; // 初始化状态结构体 22 | 23 | typedef struct 24 | { 25 | double posiNState; // 北向位置输出，m 26 | double posiEState; // 东向位置输出，m 27 | double posiDState; // 地向位置输出，m 28 | double pitchState; // 俯仰角输出，rad 29 | double yawState; // 偏航角输出，rad 30 | double rollState; // 滚转角输出，rad 31 | double velState; // 和速度输出，m/s 32 | double velalphaState; // 速度倾角, rad 33 | double velbeteState; // 速度偏角, rad 34 | double alphaState; // 攻角，输出暂为0 35 | double beteState; // 侧滑角，输出暂为0 36 | } State_Output; // 输出状态结构体 37 | 38 | // State_Init baseState; 39 | // State_Output GS_state; 40 | 41 | void InitState(State_Init baseState); 42 | State_Output OutLoopCtrl_1(double High_input, double Vel_input, double Roll_input); // 1.北航控制一，输入高度(m)，速度(m/s)，滚转(deg) 43 | State_Output OutLoopCtrl_2(double High_input, double Vel_input, double Yaw_input); // 2.北航控制二，输入高度(m)，速度(m/s)，偏航(deg) 44 | State_Output OutLoopCtrl_3(double Pitch_input, double Vel_input, double Roll_input);// 3.北航控制二，输入俯仰(m)，速度(m/s)，滚转(deg) 45 | 46 | #ifdef __cplusplus 47 | } 48 | #endif 49 | 50 | #endif // __BHDYNAMICS_H__ -------------------------------------------------------------------------------- /src/px4_rotor_sim/launch/test_multi_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/standard/standard.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol generated from standard.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_STANDARD_H 7 | #define MAVLINK_STANDARD_H 8 | 9 | #ifndef MAVLINK_H 10 | #error Wrong include order: MAVLINK_STANDARD.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. 11 | #endif 12 | 13 | #define MAVLINK_STANDARD_XML_HASH -557885868295501135 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | 19 | // MESSAGE LENGTHS AND CRCS 20 | 21 | #ifndef MAVLINK_MESSAGE_LENGTHS 22 | #define MAVLINK_MESSAGE_LENGTHS {} 23 | #endif 24 | 25 | #ifndef MAVLINK_MESSAGE_CRCS 26 | #define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}} 27 | #endif 28 | 29 | #include "../protocol.h" 30 | 31 | #define MAVLINK_ENABLED_STANDARD 32 | 33 | // ENUM DEFINITIONS 34 | 35 | 36 | 37 | // MAVLINK VERSION 38 | 39 | #ifndef MAVLINK_VERSION 40 | #define MAVLINK_VERSION 2 41 | #endif 42 | 43 | #if (MAVLINK_VERSION == 0) 44 | #undef MAVLINK_VERSION 45 | #define MAVLINK_VERSION 2 46 | #endif 47 | 48 | // MESSAGE DEFINITIONS 49 | 50 | 51 | // base include 52 | #include "../minimal/minimal.h" 53 | 54 | 55 | #if MAVLINK_STANDARD_XML_HASH == MAVLINK_PRIMARY_XML_HASH 56 | # define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION} 57 | # define MAVLINK_MESSAGE_NAMES {{ "HEARTBEAT", 0 }, { "PROTOCOL_VERSION", 300 }} 58 | # if MAVLINK_COMMAND_24BIT 59 | # include "../mavlink_get_info.h" 60 | # endif 61 | #endif 62 | 63 | #ifdef __cplusplus 64 | } 65 | #endif // __cplusplus 66 | #endif // MAVLINK_STANDARD_H 67 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/fw_sim/BHDynamic/BHDynamic.h: -------------------------------------------------------------------------------- 1 | #ifndef __BHDYNAMICS_H__ 2 | #define __BHDYNAMICS_H__ 3 | 4 | // BHDynamics.h : 头文件 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | /* 这种方式确保当头文件被 C++ 编译器编译时，函数按照 C 的方式导出。 10 | 当头文件被 C 编译器编译时，extern "C" 不会起作用，因为 C 编译器不识别这个关键字。*/ 11 | 12 | typedef struct 13 | { 14 | double posiNInit; // 北向初始位置，m 15 | double posiEInit; // 东向初始位置，m 16 | double posiDInit; // 地向初始位置，m 17 | double velInit; // 初始速度，m/s 18 | double pitchInit; // 初始俯仰角，rad 19 | double yawInit; // 初始偏航角，rad 20 | double rollInit; // 初始滚转角，rad 21 | } State_Init; // 初始化状态结构体 22 | 23 | typedef struct 24 | { 25 | double posiNState; // 北向位置输出，m 26 | double posiEState; // 东向位置输出，m 27 | double posiDState; // 地向位置输出，m 28 | double pitchState; // 俯仰角输出，rad 29 | double yawState; // 偏航角输出，rad 30 | double rollState; // 滚转角输出，rad 31 | double velState; // 和速度输出，m/s 32 | double velalphaState; // 速度倾角, deg 33 | double velbeteState; // 速度偏角, deg 34 | double alphaState; // 攻角，输出暂为0 35 | double beteState; // 侧滑角，输出暂为0 36 | } State_Output; // 输出状态结构体 37 | 38 | // State_Init baseState; 39 | // State_Output GS_state; 40 | 41 | void InitState(State_Init baseState); 42 | State_Output OutLoopCtrl_1(double High_input, double Vel_input, double Roll_input); // 1.北航控制一，输入高度(m)，速度(m/s)，滚转(deg) 43 | State_Output OutLoopCtrl_2(double High_input, double Vel_input, double Yaw_input); // 2.北航控制二，输入高度(m)，速度(m/s)，偏航(deg) 44 | State_Output OutLoopCtrl_3(double Pitch_input, double Vel_input, double Roll_input);// 3.北航控制二，输入俯仰(m)，速度(m/s)，滚转(deg) 45 | 46 | /* 输入限制： */ 47 | // 速度：18~32 m/s 48 | // 滚转：-40~40 deg 49 | // 高度： >0 m 50 | // 俯仰角：-45~15 deg 51 | 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | #endif // __BHDYNAMICS_H__ -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/test/MatrixUnwrapTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace matrix; 5 | 6 | TEST(MatrixUnwrapTest, UnwrapFloats) 7 | { 8 | const float M_TWO_PI_F = float(M_PI * 2); 9 | 10 | float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; 11 | float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25}; 12 | 13 | for (int i = 0; i < 6; i++) { 14 | unwrapped_angles[i] *= M_TWO_PI_F; 15 | wrapped_angles[i] *= M_TWO_PI_F; 16 | } 17 | 18 | // positive unwrapping 19 | float last_angle = wrapped_angles[0]; 20 | 21 | for (int i = 1; i < 6; i++) { 22 | last_angle = unwrap_pi(last_angle, wrapped_angles[i]); 23 | EXPECT_FLOAT_EQ(last_angle, unwrapped_angles[i]); 24 | } 25 | 26 | // negative unwrapping 27 | last_angle = -wrapped_angles[0]; 28 | 29 | for (int i = 1; i < 6; i++) { 30 | last_angle = unwrap_pi(last_angle, -wrapped_angles[i]); 31 | EXPECT_FLOAT_EQ(last_angle, -unwrapped_angles[i]); 32 | } 33 | } 34 | 35 | TEST(MatrixUnwrapTest, UnwrapDoubles) 36 | { 37 | const double M_TWO_PI = M_PI * 2; 38 | 39 | double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; 40 | double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25}; 41 | 42 | for (int i = 0; i < 6; i++) { 43 | unwrapped_angles[i] *= M_TWO_PI; 44 | wrapped_angles[i] *= M_TWO_PI; 45 | } 46 | 47 | // positive unwrapping 48 | double last_angle = wrapped_angles[0]; 49 | 50 | for (int i = 1; i < 6; i++) { 51 | last_angle = unwrap_pi(last_angle, wrapped_angles[i]); 52 | EXPECT_DOUBLE_EQ(last_angle, unwrapped_angles[i]); 53 | } 54 | 55 | // negative unwrapping 56 | last_angle = -wrapped_angles[0]; 57 | 58 | for (int i = 1; i < 6; i++) { 59 | last_angle = unwrap_pi(last_angle, -wrapped_angles[i]); 60 | EXPECT_DOUBLE_EQ(last_angle, -unwrapped_angles[i]); 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/test/MatrixUnwrapTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace matrix; 5 | 6 | TEST(MatrixUnwrapTest, UnwrapFloats) 7 | { 8 | const float M_TWO_PI_F = float(M_PI * 2); 9 | 10 | float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; 11 | float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25}; 12 | 13 | for (int i = 0; i < 6; i++) { 14 | unwrapped_angles[i] *= M_TWO_PI_F; 15 | wrapped_angles[i] *= M_TWO_PI_F; 16 | } 17 | 18 | // positive unwrapping 19 | float last_angle = wrapped_angles[0]; 20 | 21 | for (int i = 1; i < 6; i++) { 22 | last_angle = unwrap_pi(last_angle, wrapped_angles[i]); 23 | EXPECT_FLOAT_EQ(last_angle, unwrapped_angles[i]); 24 | } 25 | 26 | // negative unwrapping 27 | last_angle = -wrapped_angles[0]; 28 | 29 | for (int i = 1; i < 6; i++) { 30 | last_angle = unwrap_pi(last_angle, -wrapped_angles[i]); 31 | EXPECT_FLOAT_EQ(last_angle, -unwrapped_angles[i]); 32 | } 33 | } 34 | 35 | TEST(MatrixUnwrapTest, UnwrapDoubles) 36 | { 37 | const double M_TWO_PI = M_PI * 2; 38 | 39 | double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; 40 | double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25}; 41 | 42 | for (int i = 0; i < 6; i++) { 43 | unwrapped_angles[i] *= M_TWO_PI; 44 | wrapped_angles[i] *= M_TWO_PI; 45 | } 46 | 47 | // positive unwrapping 48 | double last_angle = wrapped_angles[0]; 49 | 50 | for (int i = 1; i < 6; i++) { 51 | last_angle = unwrap_pi(last_angle, wrapped_angles[i]); 52 | EXPECT_DOUBLE_EQ(last_angle, unwrapped_angles[i]); 53 | } 54 | 55 | // negative unwrapping 56 | last_angle = -wrapped_angles[0]; 57 | 58 | for (int i = 1; i < 6; i++) { 59 | last_angle = unwrap_pi(last_angle, -wrapped_angles[i]); 60 | EXPECT_DOUBLE_EQ(last_angle, -unwrapped_angles[i]); 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2021 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | if(BUILD_TESTING) 35 | add_subdirectory(test) 36 | endif() 37 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2021 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | if(BUILD_TESTING) 35 | add_subdirectory(test) 36 | endif() 37 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/hysteresis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2019 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(hysteresis hysteresis.cpp) 35 | 36 | px4_add_unit_gtest(SRC HysteresisTest.cpp LINKLIBS hysteresis) 37 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavros_px4_quadrotor_sim/mavros_px4_quadrotor_sim_node.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mavros_px4_quadrotor_sim_node.hpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Mavros(sim) + PX4 controller + quadrotor_dynamics. main loop 5 | * 6 | * Note: This program relies on mavros_sim, px4_sitl, dynamics and sss_utils 7 | * 8 | * @version 1.0 9 | * @date 2023-12-10 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2023, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | 18 | #ifndef __MAVROS_PX4_QUADROTOR_SIM_NODE_H__ 19 | #define __MAVROS_PX4_QUADROTOR_SIM_NODE_H__ 20 | 21 | #include 22 | #include 23 | 24 | #include "mavros_sim/MavrosSim.hpp" 25 | #include "mavros_px4_quadrotor_sim/px4_sitl.hpp" 26 | #include "mavros_px4_quadrotor_sim/quadrotor_dynamics.hpp" 27 | #include "mavros_px4_quadrotor_sim/drone_visualizer.hpp" 28 | 29 | 30 | namespace MavrosQuadSimulator 31 | { 32 | 33 | extern int agent_index; // index of MavrosQuadSimulator agents (this variable is global across nodelets) 34 | int agent_index = 0; // index of MavrosQuadSimulator agents (this variable is global across nodelets) 35 | 36 | class Agent 37 | { 38 | private: 39 | int agent_id_=-1; // agent id 40 | 41 | public: 42 | Agent(int agent_id, const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 43 | 44 | private: 45 | bool is_sim_time_; 46 | 47 | ros::NodeHandle nh_; 48 | ros::NodeHandle nh_private_; 49 | 50 | std::shared_ptr dynamics_; 51 | std::shared_ptr px4sitl_; 52 | std::shared_ptr mavros_sim_; 53 | std::shared_ptr visualizer_; 54 | 55 | double mainloop_period_; 56 | double mainloop_last_time_ = 0; 57 | sss_utils::Timer mainloop_timer_; 58 | 59 | void mainloop(const ros::TimerEvent &event); 60 | }; 61 | 62 | } 63 | 64 | #endif -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mavlink/mavlink_streamer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mavlink_streamer.cpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Simulate the streaming of mavlink messages in PX4. 5 | * 6 | * Note: This program relies on mavlink 7 | * 8 | * @version 1.0 9 | * @date 2023-12-17 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2023, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | #include "mavlink_streamer.hpp" 18 | 19 | MavlinkStreamer::MavlinkStreamer(int agent_id) : agent_id_(agent_id) 20 | { 21 | mavlink_stream_AttitudeQuaternion_ = STREAM_MAKE_PTR(MavlinkStreamAttitudeQuaternion)(50, this); // set streaming rate 22 | mavlink_stream_LocalPositionNED_ = STREAM_MAKE_PTR(MavlinkStreamLocalPositionNED)(30, this); // set streaming rate 23 | mavlink_stream_AttitudeTarget_ = STREAM_MAKE_PTR(MavlinkStreamAttitudeTarget)(50, this); // set streaming rate 24 | mavlink_stream_PositionTargetLocalNed_ = STREAM_MAKE_PTR(MavlinkStreamPositionTargetLocalNed)(30, this); // set streaming rate 25 | mavlink_stream_Heartbeat_ = STREAM_MAKE_PTR(MavlinkStreamHeartbeat)(10, this); // set streaming rate 26 | mavlink_stream_SysStatus_ = STREAM_MAKE_PTR(MavlinkStreamSysStatus)(2, this); // set streaming rate 27 | mavlink_stream_GlobalPositionInt_ = STREAM_MAKE_PTR(MavlinkStreamGlobalPositionInt)(30, this); // set streaming rate 28 | mavlink_stream_GpsGlobalOrigin_ = STREAM_MAKE_PTR(MavlinkStreamGpsGlobalOrigin)(100, this); // set streaming rate (Actually it will only send the mavlink stream when the gps origin is updated. Refer to streams/GPS_GLOBAL_ORIGIN.hpp) 29 | 30 | //@TODO extended_sys_state for extended_state 31 | 32 | } 33 | 34 | 35 | MavlinkStreamer::~MavlinkStreamer() 36 | { 37 | stream_signal_.disconnect_all_slots(); 38 | } 39 | 40 | void MavlinkStreamer::Stream(const uint64_t &time_us) 41 | { 42 | // stream all mavlink messages 43 | stream_signal_(time_us); 44 | } -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/hysteresis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2019 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(hysteresis hysteresis.cpp) 35 | 36 | px4_add_unit_gtest(SRC HysteresisTest.cpp LINKLIBS hysteresis) 37 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/geo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | add_library(geo 35 | geo.cpp 36 | geo.h 37 | ) 38 | add_dependencies(geo prebuild_targets) 39 | target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) 40 | 41 | px4_add_unit_gtest(SRC test_geo.cpp LINKLIBS geo) 42 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/geo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | add_library(geo 35 | geo.cpp 36 | geo.h 37 | ) 38 | add_dependencies(geo prebuild_targets) 39 | target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) 40 | 41 | px4_add_unit_gtest(SRC test_geo.cpp LINKLIBS geo) 42 | -------------------------------------------------------------------------------- /src/sss_sim_env/src/sim_clock/sim_clock.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file sim_clock.cpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief simulation clock. Determine the simulation time process. 5 | * 6 | * Note: This program relies on 7 | * 8 | * @version 1.0 9 | * @date 2023-11-19 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2023, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | #include "sss_sim_env/TimeServer.hpp" 21 | 22 | 23 | class SimClock :public nodelet::Nodelet 24 | { 25 | public: 26 | SimClock(){} 27 | 28 | public: 29 | void onInit() 30 | { 31 | ros::NodeHandle nh = getNodeHandle(); 32 | ros::NodeHandle nh_private = getPrivateNodeHandle(); 33 | // ros::NodeHandle nh = getMTNodeHandle(); 34 | // ros::NodeHandle nh_private = getMTPrivateNodeHandle(); 35 | 36 | time_server = std::make_unique(nh, nh_private); 37 | 38 | ROS_INFO("SimClock Inited"); 39 | // NODELET_DEBUG("My debug statement"); 40 | // NODELET_DEBUG_STREAM("my debug statement " << (double) 1.0) 41 | // NODELET_DEBUG_COND( 1 == 1, "my debug_statement") 42 | // NODELET_DEBUG_STREAM_COND( 1 == 1, "my debug statement " << (double) 1.0) 43 | } 44 | 45 | private: 46 | std::unique_ptr time_server; 47 | }; 48 | 49 | #include 50 | PLUGINLIB_EXPORT_CLASS(SimClock, nodelet::Nodelet); 51 | 52 | 53 | 54 | // int main(int argc, char **argv) 55 | // { 56 | // ros::init(argc, argv, "sim_clock"); 57 | // ros::NodeHandle nh_private("~"); 58 | // ros::NodeHandle nh; 59 | 60 | // // TimeServer time_server(nh, nh_private); 61 | 62 | // // //create an object named time_server in heap rather than stack 63 | // // TimeServer* time_server = new TimeServer(nh, nh_private); 64 | 65 | // //Use unique_ptr to auto-destory the object when exiting. 66 | // std::unique_ptr time_server(new TimeServer(nh, nh_private)); 67 | 68 | // ros::spin(); 69 | // return 0; 70 | // } -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/mavros_sim/lib/uas_timesync.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file uas_timesync.cpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Change from mavros to mavros_sim 5 | * 6 | * Similar to https://github.com/mavlink/mavros/blob/master/mavros/src/lib/uas_timesync.cpp 7 | * 8 | * Note: This program relies on 9 | * 10 | * @version 1.0 11 | * @date 2023-12-19 12 | * 13 | * @license BSD 3-Clause License 14 | * @copyright (c) 2023, Peixuan Shu 15 | * All rights reserved. 16 | * 17 | */ 18 | 19 | /** 20 | * @brief MAVROS UAS manager 21 | * @file uas.cpp 22 | * @author Vladimir Ermakov 23 | */ 24 | /* 25 | * Copyright 2014,2017 Vladimir Ermakov, M.H.Kabir. 26 | * 27 | * This file is part of the mavros package and subject to the license terms 28 | * in the top-level LICENSE file of the mavros repository. 29 | * https://github.com/mavlink/mavros/tree/master/LICENSE.md 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include "mavros_uas.h" 36 | // #include 37 | #include 38 | 39 | using namespace mavros_sim; 40 | 41 | 42 | /* -*- time syncronise functions -*- */ 43 | 44 | static inline ros::Time ros_time_from_ns(const uint64_t stamp_ns) { 45 | return ros::Time( 46 | stamp_ns / 1000000000UL, // t_sec 47 | stamp_ns % 1000000000UL); // t_nsec 48 | } 49 | 50 | ros::Time UAS::synchronise_stamp(uint32_t time_boot_ms) { 51 | // copy offset from atomic var 52 | uint64_t offset_ns = time_offset; 53 | 54 | if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { 55 | uint64_t stamp_ns = static_cast(time_boot_ms) * 1000000UL + offset_ns; 56 | return ros_time_from_ns(stamp_ns); 57 | } 58 | else 59 | return ros::Time::now(); 60 | } 61 | 62 | ros::Time UAS::synchronise_stamp(uint64_t time_usec) { 63 | uint64_t offset_ns = time_offset; 64 | 65 | if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { 66 | uint64_t stamp_ns = time_usec * 1000UL + offset_ns; 67 | return ros_time_from_ns(stamp_ns); 68 | } 69 | else 70 | return ros::Time::now(); 71 | } 72 | 73 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mc_pos_control/Takeoff/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2019 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(Takeoff 35 | Takeoff.cpp 36 | Takeoff.hpp 37 | ) 38 | target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 39 | target_link_libraries(Takeoff PUBLIC hysteresis) 40 | 41 | px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) 42 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/mathlib/mathlib.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2012 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file mathlib.h 36 | * 37 | * Common header for mathlib exports. 38 | */ 39 | 40 | #pragma once 41 | 42 | #ifdef __cplusplus 43 | 44 | #include "math/Limits.hpp" 45 | #include "math/Functions.hpp" 46 | #include "math/SearchMin.hpp" 47 | #include "math/TrajMath.hpp" 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/cpuload.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/cpuload.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct cpuload_s { 44 | uint64_t timestamp; 45 | float load; 46 | float ram_usage; 47 | }; -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mc_att_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | add_subdirectory(AttitudeControl) 35 | 36 | px4_add_module( 37 | MODULE modules__mc_att_control 38 | MAIN mc_att_control 39 | COMPILE_FLAGS 40 | ${MAX_CUSTOM_OPT_LEVEL} 41 | SRCS 42 | mc_att_control_main.cpp 43 | mc_att_control.hpp 44 | DEPENDS 45 | AttitudeControl 46 | mathlib 47 | px4_work_queue 48 | ) 49 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/mathlib/mathlib.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2012 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file mathlib.h 36 | * 37 | * Common header for mathlib exports. 38 | */ 39 | 40 | #pragma once 41 | 42 | #ifdef __cplusplus 43 | 44 | #include "math/Limits.hpp" 45 | #include "math/Functions.hpp" 46 | #include "math/SearchMin.hpp" 47 | #include "math/TrajMath.hpp" 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/cpuload.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/cpuload.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct cpuload_s { 44 | uint64_t timestamp; 45 | float load; 46 | float ram_usage; 47 | }; -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/slew_rate/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2020 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(SlewRate 35 | dummy.cpp 36 | SlewRate.hpp 37 | SlewRateYaw.hpp 38 | ) 39 | target_include_directories(SlewRate 40 | PUBLIC 41 | ${CMAKE_CURRENT_SOURCE_DIR} 42 | ) 43 | 44 | target_link_libraries(SlewRate PUBLIC mathlib) 45 | 46 | px4_add_unit_gtest(SRC SlewRateTest.cpp LINKLIBS SlewRate) 47 | px4_add_unit_gtest(SRC SlewRateYawTest.cpp LINKLIBS SlewRate) 48 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mc_att_control/AttitudeControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2019 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(AttitudeControl 35 | AttitudeControl.cpp 36 | AttitudeControl.hpp 37 | ) 38 | target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) 39 | target_include_directories(AttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 40 | 41 | px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl) 42 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mc_pos_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | add_subdirectory(PositionControl) 35 | add_subdirectory(Takeoff) 36 | 37 | px4_add_module( 38 | MODULE modules__mc_pos_control 39 | MAIN mc_pos_control 40 | COMPILE_FLAGS 41 | SRCS 42 | MulticopterPositionControl.cpp 43 | MulticopterPositionControl.hpp 44 | DEPENDS 45 | PositionControl 46 | Takeoff 47 | controllib 48 | geo 49 | SlewRate 50 | ) 51 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/include/mavlink/v2.0/message_definitions/paparazzi.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 3 5 | 6 | 7 | 8 | 9 | 10 | Message encoding a mission script item. This message is emitted upon a request for the next script item. 11 | System ID 12 | Component ID 13 | Sequence 14 | The name of the mission script, NULL terminated. 15 | 16 | 17 | Request script item with the sequence number seq. The response of the system to this message should be a SCRIPT_ITEM message. 18 | System ID 19 | Component ID 20 | Sequence 21 | 22 | 23 | Request the overall list of mission items from the system/component. 24 | System ID 25 | Component ID 26 | 27 | 28 | This message is emitted as response to SCRIPT_REQUEST_LIST by the MAV to get the number of mission scripts. 29 | System ID 30 | Component ID 31 | Number of script items in the sequence 32 | 33 | 34 | This message informs about the currently active SCRIPT. 35 | Active Sequence 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/slew_rate/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2020 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(SlewRate 35 | dummy.cpp 36 | SlewRate.hpp 37 | SlewRateYaw.hpp 38 | ) 39 | target_include_directories(SlewRate 40 | PUBLIC 41 | ${CMAKE_CURRENT_SOURCE_DIR} 42 | ) 43 | 44 | target_link_libraries(SlewRate PUBLIC mathlib) 45 | 46 | px4_add_unit_gtest(SRC SlewRateTest.cpp LINKLIBS SlewRate) 47 | px4_add_unit_gtest(SRC SlewRateYawTest.cpp LINKLIBS SlewRate) 48 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mc_pos_control/PositionControl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2019 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(PositionControl 35 | ControlMath.cpp 36 | ControlMath.hpp 37 | PositionControl.cpp 38 | PositionControl.hpp 39 | ) 40 | target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 41 | 42 | px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl) 43 | px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl) 44 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/vehicle_rates_setpoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_rates_setpoint.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_rates_setpoint_s { 44 | 45 | uint64_t timestamp; 46 | float roll; 47 | float pitch; 48 | float yaw; 49 | float thrust_body[3]; 50 | }; 51 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/mavros_sim/MavrosSim.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file MavrosSim.hpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Simulated mavros that receives ROS topics and transfer to the 5 | * quadrotor dynamics and broadcast quadrotor states to ROS topics. 6 | * 7 | * Note: This program relies on 8 | * 9 | * @version 1.0 10 | * @date 2023-11-29 11 | * 12 | * @license BSD 3-Clause License 13 | * @copyright (c) 2023, Peixuan Shu 14 | * All rights reserved. 15 | * 16 | */ 17 | 18 | 19 | #include 20 | 21 | #include "plugins/setpoint_raw.cpp" 22 | #include "plugins/local_position.cpp" 23 | #include "plugins/imu.cpp" 24 | #include "plugins/sys_status.cpp" 25 | #include "plugins/command.cpp" 26 | #include "plugins/global_position.cpp" 27 | 28 | #include "lib/mavros_uas.h" 29 | 30 | #include "px4_modules/mavlink/mavlink_msg_list.hpp" // store the simulated extern(global) mavlink messages (Created by Peixuan Shu) 31 | 32 | namespace mavros_sim 33 | { 34 | 35 | class MavrosSim 36 | { 37 | private: 38 | int agent_id_ = -1; // UAV id. 39 | 40 | public: 41 | /* Load mavros_sim plugins(mavlink msg -> mavros ROS msg; mavros ROS msg -> mavlink msg)*/ 42 | MavrosSim(int agent_id, const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 43 | 44 | /* Publish all updated mavlink messages into ROS topics (Added by Peixuan Shu) */ 45 | void PublishRosMessage(); 46 | 47 | private: 48 | ros::NodeHandle nh_; 49 | ros::NodeHandle nh_private_; 50 | 51 | std::shared_ptr uas_; // store some common data and functions 52 | 53 | std::unique_ptr setpoint_raw_plugin_; 54 | std::unique_ptr local_position_plugin_; 55 | std::unique_ptr imu_plugin_; 56 | std::unique_ptr sys_status_plugin_; 57 | std::unique_ptr command_plugin_; 58 | std::unique_ptr global_position_plugin_; 59 | 60 | /* Publish mavlink messages into ROS topics (Added by Peixuan Shu)*/ 61 | void handle_message(const mavlink_message_t &msg); 62 | 63 | }; 64 | 65 | } 66 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/test/MatrixScalarMultiplicationTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixScalarMultiplicationTest, ScalarMultiplication) 40 | { 41 | float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; 42 | Matrix3f A(data); 43 | A = A * 2; 44 | float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; 45 | Matrix3f A_check(data_check); 46 | EXPECT_EQ(A, A_check); 47 | } 48 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/test/MatrixTransposeTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixTransposeTest, Transpose) 40 | { 41 | float data[6] = {1, 2, 3, 4, 5, 6}; 42 | Matrix A(data); 43 | Matrix A_T = A.transpose(); 44 | float data_check[6] = {1, 4, 2, 5, 3, 6}; 45 | Matrix A_T_check(data_check); 46 | EXPECT_EQ(A_T, A_T_check); 47 | } 48 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/test/MatrixHatveeTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixHatveeTest, Hatvee) 40 | { 41 | Euler euler(0.1f, 0.2f, 0.3f); 42 | Dcm R(euler); 43 | Dcm skew = R - R.T(); 44 | Vector3 w = skew.vee(); 45 | Vector3 w_check(0.1348f, 0.4170f, 0.5647f); 46 | 47 | EXPECT_EQ(w, w_check); 48 | EXPECT_EQ(skew, w.hat()); 49 | } 50 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/vehicle_angular_velocity.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_angular_velocity.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_angular_velocity_s { 44 | 45 | uint64_t timestamp; 46 | uint64_t timestamp_sample; 47 | float xyz[3]; 48 | uint8_t _padding0[4]; // required for logger 49 | 50 | }; 51 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/vehicle_rates_setpoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_rates_setpoint.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_rates_setpoint_s { 44 | 45 | uint64_t timestamp; 46 | float roll; 47 | float pitch; 48 | float yaw; 49 | float thrust_body[3]; 50 | }; 51 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/vehicle_constraints.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_constraints.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_constraints_s { 44 | 45 | uint64_t timestamp; 46 | float speed_up; 47 | float speed_down; 48 | bool want_takeoff; 49 | uint8_t _padding0[7]; // required for logger 50 | 51 | 52 | }; 53 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/test/MatrixScalarMultiplicationTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixScalarMultiplicationTest, ScalarMultiplication) 40 | { 41 | float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; 42 | Matrix3f A(data); 43 | A = A * 2; 44 | float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; 45 | Matrix3f A_check(data_check); 46 | EXPECT_EQ(A, A_check); 47 | } 48 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/test/MatrixHatveeTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixHatveeTest, Hatvee) 40 | { 41 | Euler euler(0.1f, 0.2f, 0.3f); 42 | Dcm R(euler); 43 | Dcm skew = R - R.T(); 44 | Vector3 w = skew.vee(); 45 | Vector3 w_check(0.1348f, 0.4170f, 0.5647f); 46 | 47 | EXPECT_EQ(w, w_check); 48 | EXPECT_EQ(skew, w.hat()); 49 | } 50 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/test/MatrixTransposeTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixTransposeTest, Transpose) 40 | { 41 | float data[6] = {1, 2, 3, 4, 5, 6}; 42 | Matrix A(data); 43 | Matrix A_T = A.transpose(); 44 | float data_check[6] = {1, 4, 2, 5, 3, 6}; 45 | Matrix A_T_check(data_check); 46 | EXPECT_EQ(A_T, A_T_check); 47 | } 48 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/vehicle_attitude.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_attitude.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_attitude_s { 44 | 45 | uint64_t timestamp; 46 | uint64_t timestamp_sample; 47 | float q[4]; 48 | float delta_q_reset[4]; 49 | uint8_t quat_reset_counter; 50 | uint8_t _padding0[7]; // required for logger 51 | }; 52 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/test/MatrixUpperRightTriangleTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixUpperRightTriangleTest, UpperRightTriangle) 40 | { 41 | float data[9] = {1, 2, 3, 42 | 4, 5, 6, 43 | 7, 8, 10 44 | }; 45 | float urt[6] = {1, 2, 3, 5, 6, 10}; 46 | 47 | SquareMatrix A(data); 48 | 49 | for (size_t i = 0; i < 6; i++) { 50 | EXPECT_FLOAT_EQ(urt[i], A.upper_right_triangle()(i)); 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/vehicle_angular_velocity.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_angular_velocity.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_angular_velocity_s { 44 | 45 | uint64_t timestamp; 46 | uint64_t timestamp_sample; 47 | float xyz[3]; 48 | uint8_t _padding0[4]; // required for logger 49 | 50 | }; 51 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/actuator_armed.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/actuator_armed.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct actuator_armed_s { 44 | uint64_t timestamp; 45 | bool armed; 46 | bool prearmed; 47 | bool ready_to_arm; 48 | bool lockdown; 49 | bool manual_lockdown; 50 | bool force_failsafe; 51 | bool in_esc_calibration_mode; 52 | bool soft_stop; 53 | }; -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/vehicle_constraints.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_constraints.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_constraints_s { 44 | 45 | uint64_t timestamp; 46 | float speed_up; 47 | float speed_down; 48 | bool want_takeoff; 49 | uint8_t _padding0[7]; // required for logger 50 | 51 | 52 | }; 53 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/launch/drone_visualizer_multi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/home_position.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/home_position.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct home_position_s { 44 | uint64_t timestamp; 45 | double lat; 46 | double lon; 47 | float alt; 48 | float x; 49 | float y; 50 | float z; 51 | float yaw; 52 | bool valid_alt; 53 | bool valid_hpos; 54 | bool valid_lpos; 55 | bool manual_home; 56 | 57 | }; 58 | -------------------------------------------------------------------------------- /src/ugv_sim/launch/visualize_ugv_multi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/offboard_control_mode.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/offboard_control_mode.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct offboard_control_mode_s { 44 | 45 | uint64_t timestamp; 46 | bool position; 47 | bool velocity; 48 | bool acceleration; 49 | bool attitude; 50 | bool body_rate; 51 | bool actuator; 52 | uint8_t _padding0[2]; // required for logger 53 | 54 | }; 55 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/vehicle_attitude.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_attitude.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_attitude_s { 44 | 45 | uint64_t timestamp; 46 | uint64_t timestamp_sample; 47 | float q[4]; 48 | float delta_q_reset[4]; 49 | uint8_t quat_reset_counter; 50 | uint8_t _padding0[7]; // required for logger 51 | }; 52 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/matrix/test/MatrixUpperRightTriangleTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | TEST(MatrixUpperRightTriangleTest, UpperRightTriangle) 40 | { 41 | float data[9] = {1, 2, 3, 42 | 4, 5, 6, 43 | 7, 8, 10 44 | }; 45 | float urt[6] = {1, 2, 3, 5, 6, 10}; 46 | 47 | SquareMatrix A(data); 48 | 49 | for (size_t i = 0; i < 6; i++) { 50 | EXPECT_FLOAT_EQ(urt[i], A.upper_right_triangle()(i)); 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/actuator_armed.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/actuator_armed.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct actuator_armed_s { 44 | uint64_t timestamp; 45 | bool armed; 46 | bool prearmed; 47 | bool ready_to_arm; 48 | bool lockdown; 49 | bool manual_lockdown; 50 | bool force_failsafe; 51 | bool in_esc_calibration_mode; 52 | bool soft_stop; 53 | }; -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/home_position.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/home_position.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct home_position_s { 44 | uint64_t timestamp; 45 | double lat; 46 | double lon; 47 | float alt; 48 | float x; 49 | float y; 50 | float z; 51 | float yaw; 52 | bool valid_alt; 53 | bool valid_hpos; 54 | bool valid_lpos; 55 | bool manual_home; 56 | 57 | }; 58 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/mathlib/math/test/test.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2012 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file test.hpp 36 | * 37 | * Controller library code 38 | */ 39 | 40 | #pragma once 41 | 42 | //#include 43 | //#include 44 | //#include 45 | 46 | bool equal(float a, float b, float eps = 1e-5); 47 | 48 | bool greater_than(float a, float b); 49 | 50 | bool less_than(float a, float b); 51 | 52 | bool greater_than_or_equal(float a, float b); 53 | 54 | bool less_than_or_equal(float a, float b); 55 | 56 | void float2SigExp( 57 | const float &num, 58 | float &sig, 59 | int &exp); 60 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/uORB/topics/vehicle_air_data.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_air_data.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_air_data_s { 44 | uint64_t timestamp; 45 | uint64_t timestamp_sample; 46 | uint32_t baro_device_id; 47 | float baro_alt_meter; 48 | float baro_temp_celcius; 49 | float baro_pressure_pa; 50 | float rho; 51 | uint8_t calibration_count; 52 | uint8_t _padding0[3]; // required for logger 53 | }; -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/offboard_control_mode.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/offboard_control_mode.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct offboard_control_mode_s { 44 | 45 | uint64_t timestamp; 46 | bool position; 47 | bool velocity; 48 | bool acceleration; 49 | bool attitude; 50 | bool body_rate; 51 | bool actuator; 52 | uint8_t _padding0[2]; // required for logger 53 | 54 | }; 55 | -------------------------------------------------------------------------------- /src/tello_sim/launch/visualize_tello_multi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/mathlib/math/test/test.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2012 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file test.hpp 36 | * 37 | * Controller library code 38 | */ 39 | 40 | #pragma once 41 | 42 | //#include 43 | //#include 44 | //#include 45 | 46 | bool equal(float a, float b, float eps = 1e-5); 47 | 48 | bool greater_than(float a, float b); 49 | 50 | bool less_than(float a, float b); 51 | 52 | bool greater_than_or_equal(float a, float b); 53 | 54 | bool less_than_or_equal(float a, float b); 55 | 56 | void float2SigExp( 57 | const float &num, 58 | float &sig, 59 | int &exp); 60 | -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/px4_lib/uORB/topics/vehicle_air_data.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* Auto-generated by genmsg_cpp from file /home/spx/PX4-Autopilot/msg/vehicle_air_data.msg */ 35 | 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | struct vehicle_air_data_s { 44 | uint64_t timestamp; 45 | uint64_t timestamp_sample; 46 | uint32_t baro_device_id; 47 | float baro_alt_meter; 48 | float baro_temp_celcius; 49 | float baro_pressure_pa; 50 | float rho; 51 | uint8_t calibration_count; 52 | uint8_t _padding0[3]; // required for logger 53 | }; -------------------------------------------------------------------------------- /src/px4_rotor_sim/src/mavros_px4_quadrotor_sim/px4_modules/mavlink/mavlink_msg_list.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mavlink_msg_list.hpp 3 | * @author Peixuan Shu (shupeixuan@qq.com) 4 | * @brief Store the mavlink messages to simulate the send and receive of mavlink messages. 5 | * 6 | * Note: This program relies on 7 | * 8 | * @version 1.0 9 | * @date 2023-12-17 10 | * 11 | * @license BSD 3-Clause License 12 | * @copyright (c) 2023, Peixuan Shu 13 | * All rights reserved. 14 | * 15 | */ 16 | 17 | #ifndef __MAVLINK_MSG_LIST_HPP__ 18 | #define __MAVLINK_MSG_LIST_HPP__ 19 | 20 | #include // mavlink c headers from ros-noetic-mavlink 21 | 22 | #include // for std::vector 23 | #include // for std::array 24 | 25 | 26 | struct mavlink_info_s 27 | { 28 | bool updated; 29 | mavlink_message_t msg; 30 | }; 31 | 32 | 33 | namespace px4 34 | { 35 | 36 | enum class mavlink_stream_handle : uint16_t { 37 | ATTITUDE_QUATERNION, 38 | ATTITUDE_TARGET, 39 | HEARTBEAT, 40 | LOCAL_POSITION_NED, 41 | POSITION_TARGET_LOCAL_NED, 42 | SYS_STATUS, 43 | GLOBAL_POSITION_INT, 44 | GPS_GLOBAL_ORIGIN, 45 | //@TODO COMMAND_ACK for vehicle_command_ack 46 | ENUM_NUM // number of mavlink_stream 47 | }; 48 | #define MAVLINK_STREAM_NUM (int)px4::mavlink_stream_handle::ENUM_NUM // number of mavlink_receive 49 | /* Store the mavlink stream messages for arbitrary number of UAVs */ 50 | extern std::vector> mavlink_stream_lists; // declare global 51 | // // Store the streaming mavlink messages (declaring global) 52 | // extern mavlink_info_s mavlink_stream_list[MAVLINK_STREAM_NUM]; // declare global 53 | 54 | enum class mavlink_receive_handle : uint16_t { 55 | SET_POSITION_TARGET_LOCAL_NED, 56 | SET_POSITION_TARGET_GLOBAL_INT, 57 | SET_ATTITUDE_TARGET, 58 | SET_MODE, 59 | COMMAND_LONG, 60 | COMMAND_INT, 61 | SET_GPS_GLOBAL_ORIGIN, 62 | ENUM_NUM // number of mavlink_receive 63 | }; 64 | #define MAVLINK_RECEIVE_NUM (int)px4::mavlink_receive_handle::ENUM_NUM // number of mavlink_receive 65 | /* Store the mavlink stream messages for arbitrary number of UAVs */ 66 | extern std::vector> mavlink_receive_lists; // declare global 67 | // // Store the receiving mavlink messages 68 | // extern mavlink_info_s mavlink_receive_list[MAVLINK_RECEIVE_NUM]; // declare global 69 | 70 | void allocate_mavlink_message_storage(int expected_agent_num); 71 | 72 | } 73 | 74 | #endif -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/mathlib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | 34 | px4_add_library(mathlib 35 | math/test/test.cpp 36 | math/filter/LowPassFilter2p.hpp 37 | math/filter/MedianFilter.hpp 38 | math/filter/NotchFilter.hpp 39 | math/filter/second_order_reference_model.hpp 40 | ) 41 | 42 | px4_add_unit_gtest(SRC math/test/LowPassFilter2pVector3fTest.cpp LINKLIBS mathlib) 43 | px4_add_unit_gtest(SRC math/test/AlphaFilterTest.cpp) 44 | px4_add_unit_gtest(SRC math/test/MedianFilterTest.cpp) 45 | px4_add_unit_gtest(SRC math/test/NotchFilterTest.cpp) 46 | px4_add_unit_gtest(SRC math/test/second_order_reference_model_test.cpp) 47 | px4_add_unit_gtest(SRC math/FunctionsTest.cpp) 48 | px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp) 49 | -------------------------------------------------------------------------------- /src/fw_plane_sim/src/px4_lib/matrix/test/MatrixIntegralTest.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2022 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | using namespace matrix; 38 | 39 | Vector f(float t, const Matrix & /*y*/, const Matrix & /*u*/) 40 | { 41 | float v = -sin(t); 42 | return v * ones(); 43 | } 44 | 45 | TEST(MatrixIntegralTest, Integral) 46 | { 47 | Vector y = ones(); 48 | Vector u = ones(); 49 | float t0 = 0; 50 | float tf = 2; 51 | float h = 0.001f; 52 | integrate_rk4(f, y, u, t0, tf, h, y); 53 | float v = 1 + cos(tf) - cos(t0); 54 | EXPECT_EQ(y, (ones()*v)); 55 | } 56 | --------------------------------------------------------------------------------