├── .github └── workflows │ └── manual.yml ├── .gitignore ├── CMakeLists.txt ├── CODEOWNERS ├── README.md ├── animations ├── scenario1.gif ├── scenario2.gif ├── scenario3.gif ├── scenario4.gif └── scenario5.gif ├── config ├── 1_Intro.txt ├── 2_AttitudeControl.txt ├── 3_PositionControl.txt ├── 4_Nonidealities.txt ├── 5_TrajectoryFollow.txt ├── QuadControlParams.txt ├── QuadPhysicalParams.txt ├── Scenarios.txt ├── Simulation.txt ├── X_Scenarios.txt ├── X_TestManyQuads.txt ├── X_TestMavlink.txt ├── log │ └── .gitignore └── traj │ ├── CircleNoFF.txt │ ├── FigureEight.txt │ ├── FigureEightFF.txt │ ├── HelixNoFF.txt │ ├── HelixUpDownNoFF.txt │ ├── MakeCircleTrajectory.py │ ├── MakeHelixTrajectory.py │ ├── MakeHelixUpDownTrajectory.py │ ├── MakePeriodicTrajectory.py │ ├── MakeSpiralTrajectory.py │ ├── Origin.txt │ └── SpiralNoFF.txt ├── lib ├── freeglut │ ├── Copying.txt │ ├── Readme.txt │ ├── bin │ │ ├── freeglut.dll │ │ └── x64 │ │ │ └── freeglut.dll │ ├── include │ │ └── GL │ │ │ ├── freeglut.h │ │ │ ├── freeglut_ext.h │ │ │ ├── freeglut_std.h │ │ │ └── glut.h │ └── lib │ │ ├── freeglut.lib │ │ └── x64 │ │ └── freeglut.lib ├── matrix │ ├── AxisAngle.hpp │ ├── Dcm.hpp │ ├── Euler.hpp │ ├── LICENSE │ ├── Matrix.hpp │ ├── Quaternion.hpp │ ├── Scalar.hpp │ ├── SquareMatrix.hpp │ ├── Vector.hpp │ ├── Vector2.hpp │ ├── Vector3.hpp │ ├── filter.hpp │ ├── helper_functions.hpp │ ├── integration.hpp │ └── math.hpp └── mavlink │ ├── checksum.h │ ├── common │ ├── common.h │ ├── mavlink.h │ ├── mavlink_msg_actuator_control_target.h │ ├── mavlink_msg_adsb_vehicle.h │ ├── mavlink_msg_altitude.h │ ├── mavlink_msg_att_pos_mocap.h │ ├── mavlink_msg_attitude.h │ ├── mavlink_msg_attitude_quaternion.h │ ├── mavlink_msg_attitude_quaternion_cov.h │ ├── mavlink_msg_attitude_target.h │ ├── mavlink_msg_auth_key.h │ ├── mavlink_msg_autopilot_version.h │ ├── mavlink_msg_battery_status.h │ ├── mavlink_msg_button_change.h │ ├── mavlink_msg_camera_capture_status.h │ ├── mavlink_msg_camera_image_captured.h │ ├── mavlink_msg_camera_information.h │ ├── mavlink_msg_camera_settings.h │ ├── mavlink_msg_camera_trigger.h │ ├── mavlink_msg_change_operator_control.h │ ├── mavlink_msg_change_operator_control_ack.h │ ├── mavlink_msg_collision.h │ ├── mavlink_msg_command_ack.h │ ├── mavlink_msg_command_int.h │ ├── mavlink_msg_command_long.h │ ├── mavlink_msg_control_system_state.h │ ├── mavlink_msg_data_stream.h │ ├── mavlink_msg_data_transmission_handshake.h │ ├── mavlink_msg_debug.h │ ├── mavlink_msg_debug_vect.h │ ├── mavlink_msg_distance_sensor.h │ ├── mavlink_msg_encapsulated_data.h │ ├── mavlink_msg_estimator_status.h │ ├── mavlink_msg_extended_sys_state.h │ ├── mavlink_msg_file_transfer_protocol.h │ ├── mavlink_msg_flight_information.h │ ├── mavlink_msg_follow_target.h │ ├── mavlink_msg_global_position_int.h │ ├── mavlink_msg_global_position_int_cov.h │ ├── mavlink_msg_global_vision_position_estimate.h │ ├── mavlink_msg_gps2_raw.h │ ├── mavlink_msg_gps2_rtk.h │ ├── mavlink_msg_gps_global_origin.h │ ├── mavlink_msg_gps_inject_data.h │ ├── mavlink_msg_gps_input.h │ ├── mavlink_msg_gps_raw_int.h │ ├── mavlink_msg_gps_rtcm_data.h │ ├── mavlink_msg_gps_rtk.h │ ├── mavlink_msg_gps_status.h │ ├── mavlink_msg_heartbeat.h │ ├── mavlink_msg_high_latency.h │ ├── mavlink_msg_high_latency2.h │ ├── mavlink_msg_highres_imu.h │ ├── mavlink_msg_hil_actuator_controls.h │ ├── mavlink_msg_hil_controls.h │ ├── mavlink_msg_hil_gps.h │ ├── mavlink_msg_hil_optical_flow.h │ ├── mavlink_msg_hil_rc_inputs_raw.h │ ├── mavlink_msg_hil_sensor.h │ ├── mavlink_msg_hil_state.h │ ├── mavlink_msg_hil_state_quaternion.h │ ├── mavlink_msg_home_position.h │ ├── mavlink_msg_landing_target.h │ ├── mavlink_msg_local_position_ned.h │ ├── mavlink_msg_local_position_ned_cov.h │ ├── mavlink_msg_local_position_ned_system_global_offset.h │ ├── mavlink_msg_log_data.h │ ├── mavlink_msg_log_entry.h │ ├── mavlink_msg_log_erase.h │ ├── mavlink_msg_log_request_data.h │ ├── mavlink_msg_log_request_end.h │ ├── mavlink_msg_log_request_list.h │ ├── mavlink_msg_logging_ack.h │ ├── mavlink_msg_logging_data.h │ ├── mavlink_msg_logging_data_acked.h │ ├── mavlink_msg_manual_control.h │ ├── mavlink_msg_manual_setpoint.h │ ├── mavlink_msg_memory_vect.h │ ├── mavlink_msg_message_interval.h │ ├── mavlink_msg_mission_ack.h │ ├── mavlink_msg_mission_clear_all.h │ ├── mavlink_msg_mission_count.h │ ├── mavlink_msg_mission_current.h │ ├── mavlink_msg_mission_item.h │ ├── mavlink_msg_mission_item_int.h │ ├── mavlink_msg_mission_item_reached.h │ ├── mavlink_msg_mission_request.h │ ├── mavlink_msg_mission_request_int.h │ ├── mavlink_msg_mission_request_list.h │ ├── mavlink_msg_mission_request_partial_list.h │ ├── mavlink_msg_mission_set_current.h │ ├── mavlink_msg_mission_write_partial_list.h │ ├── mavlink_msg_mount_orientation.h │ ├── mavlink_msg_named_value_float.h │ ├── mavlink_msg_named_value_int.h │ ├── mavlink_msg_nav_controller_output.h │ ├── mavlink_msg_obstacle_distance.h │ ├── mavlink_msg_optical_flow.h │ ├── mavlink_msg_optical_flow_rad.h │ ├── mavlink_msg_param_ext_ack.h │ ├── mavlink_msg_param_ext_request_list.h │ ├── mavlink_msg_param_ext_request_read.h │ ├── mavlink_msg_param_ext_set.h │ ├── mavlink_msg_param_ext_value.h │ ├── mavlink_msg_param_map_rc.h │ ├── mavlink_msg_param_request_list.h │ ├── mavlink_msg_param_request_read.h │ ├── mavlink_msg_param_set.h │ ├── mavlink_msg_param_value.h │ ├── mavlink_msg_ping.h │ ├── mavlink_msg_play_tune.h │ ├── mavlink_msg_position_target_global_int.h │ ├── mavlink_msg_position_target_local_ned.h │ ├── mavlink_msg_power_status.h │ ├── mavlink_msg_protocol_version.h │ ├── mavlink_msg_radio_status.h │ ├── mavlink_msg_raw_imu.h │ ├── mavlink_msg_raw_pressure.h │ ├── mavlink_msg_rc_channels.h │ ├── mavlink_msg_rc_channels_override.h │ ├── mavlink_msg_rc_channels_raw.h │ ├── mavlink_msg_rc_channels_scaled.h │ ├── mavlink_msg_request_data_stream.h │ ├── mavlink_msg_resource_request.h │ ├── mavlink_msg_safety_allowed_area.h │ ├── mavlink_msg_safety_set_allowed_area.h │ ├── mavlink_msg_scaled_imu.h │ ├── mavlink_msg_scaled_imu2.h │ ├── mavlink_msg_scaled_imu3.h │ ├── mavlink_msg_scaled_pressure.h │ ├── mavlink_msg_scaled_pressure2.h │ ├── mavlink_msg_scaled_pressure3.h │ ├── mavlink_msg_serial_control.h │ ├── mavlink_msg_servo_output_raw.h │ ├── mavlink_msg_set_actuator_control_target.h │ ├── mavlink_msg_set_attitude_target.h │ ├── mavlink_msg_set_gps_global_origin.h │ ├── mavlink_msg_set_home_position.h │ ├── mavlink_msg_set_mode.h │ ├── mavlink_msg_set_position_target_global_int.h │ ├── mavlink_msg_set_position_target_local_ned.h │ ├── mavlink_msg_set_video_stream_settings.h │ ├── mavlink_msg_setup_signing.h │ ├── mavlink_msg_sim_state.h │ ├── mavlink_msg_statustext.h │ ├── mavlink_msg_storage_information.h │ ├── mavlink_msg_sys_status.h │ ├── mavlink_msg_system_time.h │ ├── mavlink_msg_terrain_check.h │ ├── mavlink_msg_terrain_data.h │ ├── mavlink_msg_terrain_report.h │ ├── mavlink_msg_terrain_request.h │ ├── mavlink_msg_timesync.h │ ├── mavlink_msg_uavcan_node_info.h │ ├── mavlink_msg_uavcan_node_status.h │ ├── mavlink_msg_v2_extension.h │ ├── mavlink_msg_vfr_hud.h │ ├── mavlink_msg_vibration.h │ ├── mavlink_msg_vicon_position_estimate.h │ ├── mavlink_msg_video_stream_information.h │ ├── mavlink_msg_vision_position_estimate.h │ ├── mavlink_msg_vision_speed_estimate.h │ ├── mavlink_msg_wifi_config_ap.h │ ├── mavlink_msg_wind_cov.h │ ├── testsuite.h │ └── version.h │ ├── mavlink_conversions.h │ ├── mavlink_get_info.h │ ├── mavlink_helpers.h │ ├── mavlink_sha256.h │ ├── mavlink_types.h │ └── protocol.h ├── project ├── CPPSim.pro ├── FCND-CPPSim.xcodeproj │ ├── project.pbxproj │ └── project.xcworkspace │ │ └── contents.xcworkspacedata ├── Simulator.sln ├── Simulator.vcxproj ├── Simulator.vcxproj.filters └── Simulator.vcxproj.user ├── src ├── BaseController.cpp ├── BaseController.h ├── Common.h ├── ControllerFactory.h ├── DataSource.h ├── Drawing │ ├── AbsThreshold.h │ ├── BaseAnalyzer.h │ ├── ColorUtils.cpp │ ├── ColorUtils.h │ ├── DrawingFuncs.cpp │ ├── DrawingFuncs.h │ ├── GLUTMenu.cpp │ ├── GLUTMenu.h │ ├── Graph.cpp │ ├── Graph.h │ ├── GraphManager.cpp │ ├── GraphManager.h │ ├── Visualizer_GLUT.cpp │ ├── Visualizer_GLUT.h │ └── WindowThreshold.h ├── Math │ ├── Angles.h │ ├── Constants.h │ ├── Geometry.cpp │ ├── Geometry.h │ ├── LowPassFilter.h │ ├── Mat3x3F.h │ ├── MathUtils.h │ ├── Quaternion.h │ ├── Random.cpp │ ├── Random.h │ ├── Transform3D.h │ ├── V3D.h │ ├── V3F.h │ └── V4D.h ├── MavlinkNode │ ├── MavlinkNode.cpp │ ├── MavlinkNode.h │ ├── MavlinkTranslation.cpp │ ├── MavlinkTranslation.h │ ├── PracticalSocket.cpp │ ├── PracticalSocket.h │ └── UDPPacket.h ├── QuadControl.cpp ├── QuadControl.h ├── Simulation │ ├── BaseDynamics.cpp │ ├── BaseDynamics.h │ ├── QuadDynamics.cpp │ ├── QuadDynamics.h │ ├── Simulator.cpp │ ├── Simulator.h │ ├── magnetometer.cpp │ ├── magnetometer.h │ ├── opticalflow.cpp │ ├── opticalflow.h │ ├── rangefinder.cpp │ └── rangefinder.h ├── Trajectory.cpp ├── Trajectory.h ├── Utility │ ├── Camera.cpp │ ├── Camera.h │ ├── FastDelegate.h │ ├── FixedQueue.h │ ├── Mutex.h │ ├── SimpleConfig.cpp │ ├── SimpleConfig.h │ ├── StringUtils.h │ ├── Timer.cpp │ └── Timer.h ├── VehicleDatatypes.h └── main.cpp └── x64.png /.github/workflows/manual.yml: -------------------------------------------------------------------------------- 1 | # Workflow to ensure whenever a Github PR is submitted, 2 | # a JIRA ticket gets created automatically. 3 | name: Manual Workflow 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on pull request events but only for the master branch 8 | pull_request_target: 9 | types: [opened, reopened] 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test-transition-issue: 16 | name: Convert Github Issue to Jira Issue 17 | runs-on: ubuntu-latest 18 | steps: 19 | - name: Checkout 20 | uses: actions/checkout@master 21 | 22 | - name: Login 23 | uses: atlassian/gajira-login@master 24 | env: 25 | JIRA_BASE_URL: ${{ secrets.JIRA_BASE_URL }} 26 | JIRA_USER_EMAIL: ${{ secrets.JIRA_USER_EMAIL }} 27 | JIRA_API_TOKEN: ${{ secrets.JIRA_API_TOKEN }} 28 | 29 | - name: Create NEW JIRA ticket 30 | id: create 31 | uses: atlassian/gajira-create@master 32 | with: 33 | project: CONUPDATE 34 | issuetype: Task 35 | summary: | 36 | Github PR [Assign the ND component] | Repo: ${{ github.repository }} | PR# ${{github.event.number}} 37 | description: | 38 | Repo link: https://github.com/${{ github.repository }} 39 | PR no. ${{ github.event.pull_request.number }} 40 | PR title: ${{ github.event.pull_request.title }} 41 | PR description: ${{ github.event.pull_request.description }} 42 | In addition, please resolve other issues, if any. 43 | fields: '{"components": [{"name":"Github PR"}], "customfield_16449":"https://classroom.udacity.com/", "customfield_16450":"Resolve the PR", "labels": ["github"], "priority":{"id": "4"}}' 44 | 45 | - name: Log created issue 46 | run: echo "Issue ${{ steps.create.outputs.issue }} was created" 47 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ## Simulator specific ignores 2 | 3 | config/LastScenario.txt 4 | 5 | ## Ignore Visual Studio temporary files, build results, and 6 | ## files generated by popular Visual Studio add-ons. 7 | 8 | # Build results 9 | [Dd]ebug/ 10 | [Dd]ebugPublic/ 11 | [Rr]elease/ 12 | [Rr]eleases/ 13 | x64/ 14 | x86/ 15 | bld/ 16 | [Bb]in/ 17 | [Oo]bj/ 18 | [Ll]og/ 19 | 20 | # Visual Studio 2015/2017 cache/options directory 21 | .vs/ 22 | 23 | # CLion/IntelliJ IDEA 24 | .idea/ 25 | 26 | 27 | # manual cmake build ignores 28 | build/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(CPPSim) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 6 | 7 | include_directories(src) 8 | include_directories(lib) 9 | 10 | FILE(GLOB SOURCES 11 | src/*.cpp 12 | src/Drawing/*.cpp 13 | src/Math/*.cpp 14 | src/Simulation/*.cpp 15 | src/Utility/*.cpp 16 | src/MavlinkNode/*.cpp 17 | src/MavlinkNode/*.h) 18 | 19 | FILE(GLOB HEADERS 20 | src/*.h 21 | src/Drawing/*.h 22 | src/Math/*.h 23 | src/Simulation/*.h 24 | src/Utility/*.h 25 | lib/matrix/*.hpp 26 | lib/mavlink/*.h 27 | lib/mavlink/common/*.h) 28 | 29 | find_package(Qt5Core REQUIRED) 30 | find_package(Qt5Network REQUIRED) 31 | find_package(Qt5Widgets REQUIRED) 32 | 33 | # /System/Library/Frameworks/GLUT.framework 34 | find_package(GLUT REQUIRED) 35 | include_directories(${GLUT_INCLUDE_DIR}) 36 | # /System/Library/Frameworks/OpenGL.framework 37 | find_package(OpenGL REQUIRED) 38 | include_directories(${OPENGL_INCLUDE_DIR}) 39 | 40 | #find_package(GL REQUIRED) 41 | #find_package(pthread REQUIRED) 42 | 43 | add_executable(CPPSim 44 | ${SOURCES} 45 | ${HEADERS} 46 | ) 47 | 48 | target_link_libraries(CPPSim 49 | Qt5::Core 50 | Qt5::Network 51 | Qt5::Widgets 52 | ${GLUT_LIBRARIES} 53 | ${OPENGL_LIBRARIES} 54 | pthread 55 | ) 56 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @udacity/active-public-content -------------------------------------------------------------------------------- /animations/scenario1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/animations/scenario1.gif -------------------------------------------------------------------------------- /animations/scenario2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/animations/scenario2.gif -------------------------------------------------------------------------------- /animations/scenario3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/animations/scenario3.gif -------------------------------------------------------------------------------- /animations/scenario4.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/animations/scenario4.gif -------------------------------------------------------------------------------- /animations/scenario5.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/animations/scenario5.gif -------------------------------------------------------------------------------- /config/1_Intro.txt: -------------------------------------------------------------------------------- 1 | # Introduction: open-loop hover 2 | 3 | INCLUDE QuadPhysicalParams.txt 4 | 5 | # simulation setup 6 | Sim.RunMode = Repeat 7 | Sim.EndTime = 1 8 | Sim.Vehicle1 = Quad 9 | 10 | # Controller selection 11 | Quad.ControlType = QuadControl 12 | Quad.ControlConfig = QuadControlParams 13 | 14 | # reference trajectory (just the starting position) 15 | QuadControlParams.Trajectory=0,0,-1 16 | 17 | # initial conditions 18 | Quad.InitialPos=0,0,-1 19 | Quad.InitialVel=0,0,0 20 | Quad.InitialYPR=0,0,0 21 | Quad.InitialOmega=0,0,0 22 | 23 | # graphing commands 24 | Commands.1=AddGraph1.Quad.Pos.Z 25 | Commands.2=AddGraph1.WindowThreshold(Quad.PosFollowErr,.5,.8) 26 | 27 | INCLUDE QuadControlParams.txt 28 | INCLUDE Simulation.txt 29 | 30 | # make sure the controller is off 31 | [QuadControlParams] 32 | kpPosXY = 0 33 | kpPosZ = 0 34 | kpVelXY = 0 35 | kpVelZ = 0 36 | kpBank = 0 37 | kpYaw = 0 38 | kpPQR = 0,0,0 -------------------------------------------------------------------------------- /config/2_AttitudeControl.txt: -------------------------------------------------------------------------------- 1 | # Attitude control 2 | 3 | INCLUDE QuadPhysicalParams.txt 4 | 5 | # simulation setup 6 | Sim.RunMode = Repeat 7 | Sim.EndTime = 1 8 | Sim.Vehicle1 = Quad 9 | 10 | # Controller selection 11 | Quad.ControlType = QuadControl 12 | Quad.ControlConfig = QuadControlParams 13 | 14 | # reference trajectory (just the starting position) 15 | QuadControlParams.Trajectory=0,0,-1 16 | 17 | # initial conditions 18 | Quad.InitialPos=0,0,-1 19 | Quad.InitialVel=0,0,0 20 | Quad.InitialYPR=0,0,0 21 | Quad.InitialOmega=30,0,0 22 | 23 | # graphing commands 24 | Commands.1=AddGraph1.Quad.Roll 25 | Commands.2=AddGraph1.AbsThreshold(Quad.Roll,0.03,0.1) 26 | Commands.3=AddGraph2.Quad.Omega.X 27 | Commands.4=AddGraph2.AbsThreshold(Quad.Omega.X,3,0.1) 28 | Commands.5=AddGraph1.WindowThreshold(Quad.Roll,0.025,0.75) 29 | Commands.6=AddGraph2.WindowThreshold(Quad.Omega.X,2.5,0.75) 30 | 31 | INCLUDE QuadControlParams.txt 32 | INCLUDE Simulation.txt 33 | 34 | # make sure position control is turned off here 35 | [QuadControlParams] 36 | kpPosXY = 0 37 | kpPosZ = 0 38 | kpVelXY = 0 39 | kpVelZ = 0 -------------------------------------------------------------------------------- /config/3_PositionControl.txt: -------------------------------------------------------------------------------- 1 | # Hover at the initial point using full 3D control 2 | 3 | INCLUDE QuadPhysicalParams.txt 4 | 5 | # simulation setup 6 | Sim.RunMode = Repeat 7 | Sim.EndTime = 2 8 | Sim.Vehicle1 = Quad1 9 | Sim.Vehicle2 = Quad2 10 | 11 | # Controller selection 12 | Quad.ControlType = QuadControl 13 | Quad.ControlConfig = QuadControlParams 14 | 15 | # reference trajectory (just the starting position) 16 | QuadControlParams.Trajectory=0,0,-1 17 | 18 | # initial conditions 19 | Quad.InitialPos=0.5,0,-1 20 | Quad.InitialVel=0,0,0 21 | Quad.InitialYPR=0,0,0 22 | Quad.InitialOmega=0,0,0 23 | 24 | # graphing commands 25 | Commands.1=AddGraph1.Quad1.Pos.X 26 | Commands.2=AddGraph1.AbsThreshold(Quad1.Pos.X,0.05,0.5) 27 | Commands.3=AddGraph1.Quad2.Pos.X 28 | Commands.4=AddGraph2.Quad1.Yaw 29 | Commands.5=AddGraph2.Quad2.Yaw 30 | Commands.6=AddGraph1.WindowThreshold(Quad1.Pos.X,0.1,1.25) 31 | Commands.7=AddGraph1.WindowThreshold(Quad2.Pos.X,0.1,1.25) 32 | Commands.8=AddGraph2.WindowThreshold(Quad2.Yaw,0.1,1.0) 33 | 34 | INCLUDE QuadControlParams.txt 35 | INCLUDE Simulation.txt 36 | 37 | [Quad1:Quad] 38 | InitialPos=0.5,1,-1 39 | TrajectoryOffset=0,1,0 40 | 41 | [Quad2:Quad] 42 | InitialPos=0.5,-1,-1 43 | InitialYPR=.7,0,0 44 | TrajectoryOffset=0,-1,0 -------------------------------------------------------------------------------- /config/4_Nonidealities.txt: -------------------------------------------------------------------------------- 1 | # Hover at the initial point using full 3D control 2 | 3 | INCLUDE QuadPhysicalParams.txt 4 | 5 | # simulation setup 6 | Sim.RunMode = Repeat 7 | Sim.EndTime = 3 8 | Sim.Vehicle1 = Quad1 9 | Sim.Vehicle2 = Quad2 10 | Sim.Vehicle3 = Quad3 11 | 12 | # Controller selection 13 | Quad.ControlType = QuadControl 14 | Quad.ControlConfig = QuadControlParams 15 | 16 | # reference trajectory (just the starting position) 17 | QuadControlParams.Trajectory=0,0,-1 18 | #QuadControlParams.Trajectory=traj/CircleNoFF.txt 19 | 20 | # initial conditions 21 | Quad.InitialPos=1,0,-.5 22 | Quad.InitialVel=0,0,0 23 | Quad.InitialYPR=0,0,0 24 | Quad.InitialOmega=0,0,0 25 | 26 | # graphing commands 27 | Commands.1=AddGraph1.Quad1.PosFollowErr 28 | Commands.2=AddGraph1.Quad2.PosFollowErr 29 | Commands.3=AddGraph1.Quad3.PosFollowErr 30 | Commands.4=Toggle.RefTrajectory 31 | Commands.5=Toggle.ActualTrajectory 32 | Commands.6=AddGraph1.WindowThreshold(Quad1.PosFollowErr,0.1,1.5) 33 | Commands.7=AddGraph1.WindowThreshold(Quad2.PosFollowErr,0.1,1.5) 34 | Commands.8=AddGraph1.WindowThreshold(Quad3.PosFollowErr,0.1,1.5) 35 | 36 | INCLUDE QuadControlParams.txt 37 | INCLUDE Simulation.txt 38 | 39 | # Vehicle-specific config 40 | [Quad1:Quad] 41 | InitialPos=-2,1,-1 42 | TrajectoryOffset = 0,1,0 43 | Mass = .8 44 | 45 | [Quad2:Quad] 46 | InitialPos=-2,0,-1 47 | TrajectoryOffset=0,0,0 48 | 49 | [Quad3:Quad] 50 | InitialPos=-2,-1,-1 51 | TrajectoryOffset=0,-1,0 52 | cy=-.05 -------------------------------------------------------------------------------- /config/5_TrajectoryFollow.txt: -------------------------------------------------------------------------------- 1 | # Hover at the initial point using full 3D control 2 | 3 | INCLUDE QuadPhysicalParams.txt 4 | 5 | # simulation setup 6 | Sim.RunMode = Repeat 7 | Sim.EndTime = 10 8 | Sim.Vehicle1 = Quad1 9 | Sim.Vehicle2 = Quad2 10 | 11 | # Controller selection 12 | Quad.ControlType = QuadControl 13 | Quad.ControlConfig = QuadControlParams 14 | 15 | # reference trajectory (just the starting position) 16 | QuadControlParams.Trajectory=traj/FigureEight.txt 17 | 18 | # graphing commands 19 | Commands.1=AddGraph1.Quad1.PosFollowErr 20 | Commands.2=AddGraph1.Quad2.PosFollowErr 21 | Commands.3=Toggle.RefTrajectory 22 | Commands.4=Toggle.ActualTrajectory 23 | Commands.5=AddGraph1.WindowThreshold(Quad2.PosFollowErr,.25,3) 24 | 25 | INCLUDE QuadControlParams.txt 26 | INCLUDE Simulation.txt 27 | 28 | # Vehicle-specific config 29 | [Quad1:Quad] 30 | InitialPos=0,1,-1 31 | TrajectoryOffset = 0,1.5,0 32 | 33 | [Quad2:Quad] 34 | InitialPos=0,-1,-1 35 | TrajectoryOffset=0,-1.5,0 36 | ControlConfig = QuadControlParamsFF 37 | 38 | [QuadControlParamsFF:QuadControlParams] 39 | Trajectory=traj/FigureEightFF.txt -------------------------------------------------------------------------------- /config/QuadControlParams.txt: -------------------------------------------------------------------------------- 1 | ############################## SLR SIMPLECONFIG ############################ 2 | # this is a comment. [X] is a namespace. [X:Y] initializes X from Y 3 | # Namespace and parameter names are not case-sensitive 4 | # X=Y sets X to Y. Y may be a string, float, or list of 3 floats 5 | ############################################################################ 6 | 7 | [QuadControlParams] 8 | 9 | UseIdealEstimator=1 10 | 11 | # Physical properties 12 | Mass = 0.4 13 | L = 0.17 14 | Ixx = 0.0023 15 | Iyy = 0.0023 16 | Izz = 0.0046 17 | kappa = 0.016 18 | minMotorThrust = .1 19 | maxMotorThrust = 4.5 20 | 21 | # Position control gains 22 | kpPosXY = 1 23 | kpPosZ = 1 24 | KiPosZ = 20 25 | 26 | # Velocity control gains 27 | kpVelXY = 4 28 | kpVelZ = 4 29 | 30 | # Angle control gains 31 | kpBank = 5 32 | kpYaw = 1 33 | 34 | # Angle rate gains 35 | kpPQR = 23, 23, 5 36 | 37 | # limits 38 | maxAscentRate = 5 39 | maxDescentRate = 2 40 | maxSpeedXY = 5 41 | maxHorizAccel = 12 42 | maxTiltAngle = .7 43 | -------------------------------------------------------------------------------- /config/QuadPhysicalParams.txt: -------------------------------------------------------------------------------- 1 | ############################## SLR SIMPLECONFIG ############################ 2 | # # or / or // begins a comment line 3 | # [X] <- namespace. Any parameter name Y after that becomes "X.Y". 4 | # Namespace and parameter names are not case-sensitive 5 | # X=Y defines a parameter 'X' to have value 'Y' 6 | # Y may be a string, float, or comma-separated list of several numbers 7 | ############################################################################ 8 | 9 | # Simulated quadcopter physical parameters 10 | 11 | [Quad] 12 | # mass [kg] 13 | Mass = 0.5 14 | 15 | # distance from vehicle origin to motors [m] 16 | L = 0.17 17 | 18 | # offset center-of-mass to vehicle body origin [m] 19 | cx = 0 20 | cy = 0 21 | 22 | # moments of inertia (assumed diagonal) [kg m2] 23 | Ixx = 0.0023 24 | Iyy = 0.0023 25 | Izz = 0.0046 26 | 27 | # time constant for props ramping up & down [s] 28 | tauaUp = 0.01 29 | tauaDown = 0.02 30 | 31 | # motor min/max thrust [N] 32 | minMotorThrust = 0.1 33 | maxMotorThrust = 4.5 34 | 35 | # ratio between thrust [N] and torque due to drag [N m] 36 | # torque = kappa * thrust 37 | kappa = 0.016 -------------------------------------------------------------------------------- /config/Scenarios.txt: -------------------------------------------------------------------------------- 1 | 1_Intro 2 | 2_AttitudeControl 3 | 3_PositionControl 4 | 4_Nonidealities 5 | 5_TrajectoryFollow -------------------------------------------------------------------------------- /config/Simulation.txt: -------------------------------------------------------------------------------- 1 | ############################## SLR SIMPLECONFIG ############################ 2 | # # or / or // begins a comment line 3 | # [X] <- namespace. Any parameter name Y after that becomes "X.Y". 4 | # Namespace and parameter names are not case-sensitive 5 | # X=Y defines a parameter 'X' to have value 'Y' 6 | # Y may be a string, float, or comma-separated list of several numbers 7 | ############################################################################ 8 | 9 | [Sim] 10 | 11 | # 5ms simulation steps 12 | Timestep = 0.001 13 | 14 | # Record vehicle state to this file 15 | # comment out to disable 16 | LoggedStateFile = log/LoggedState.txt 17 | 18 | # Space constraints 19 | xMin = -5 20 | yMin = -5 21 | xMax = 5 22 | yMax = 5 23 | bottom = 0 24 | top = 10 25 | 26 | # Simulated noise 27 | gyroNoiseInt = 0.00001 28 | rotDisturbanceInt = 0.00001 29 | xyzDisturbanceInt = 0.00001 30 | rotDisturbanceBW = 2 31 | xyzDisturbanceBW = 2 32 | 33 | [Quad] 34 | randomMotorForceMag = .25 35 | trajectoryLogStepTime = .05 -------------------------------------------------------------------------------- /config/X_Scenarios.txt: -------------------------------------------------------------------------------- 1 | X_TestMavlink 2 | X_TestManyQuads -------------------------------------------------------------------------------- /config/X_TestManyQuads.txt: -------------------------------------------------------------------------------- 1 | # Hover at the initial point using full 3D control 2 | 3 | INCLUDE QuadPhysicalParams.txt 4 | 5 | # simulation setup 6 | Sim.RunMode = Repeat 7 | Sim.EndTime = 10 8 | Sim.Vehicle1 = Quad1 9 | Sim.Vehicle2 = Quad2 10 | Sim.Vehicle3 = Quad3 11 | Sim.Vehicle4 = Quad4 12 | Sim.Vehicle5 = Quad5 13 | Sim.Vehicle6 = Quad6 14 | Sim.Vehicle7 = Quad7 15 | Sim.Vehicle8 = Quad8 16 | Sim.Vehicle9 = Quad9 17 | 18 | # Controller selection 19 | Quad.ControlType = QuadControl 20 | Quad.ControlConfig = QuadControlParams 21 | 22 | # reference trajectory 23 | QuadControlParams.Trajectory=traj/FigureEightFF.txt 24 | 25 | # graphing commands 26 | Commands.1=AddGraph1.Sim.DrawTime 27 | Commands.2=Toggle.ActualTrajectory 28 | 29 | INCLUDE QuadControlParams.txt 30 | INCLUDE Simulation.txt 31 | 32 | [] 33 | Quad.trajectoryLogStepTime = .025 34 | 35 | # Vehicle-specific config 36 | [Quad1:Quad] 37 | TrajectoryTimeOffset = 0 38 | TrajectoryOffset = 0, 0, 0 39 | [Quad2:Quad] 40 | TrajectoryTimeOffset = .1 41 | TrajectoryOffset = .1,0,0 42 | [Quad3:Quad] 43 | TrajectoryTimeOffset = .2 44 | TrajectoryOffset = .2,0,0 45 | [Quad4:Quad] 46 | TrajectoryTimeOffset = .3 47 | TrajectoryOffset = .3,0,0 48 | [Quad5:Quad] 49 | TrajectoryTimeOffset = .4 50 | TrajectoryOffset = .4,0,0 51 | [Quad6:Quad] 52 | TrajectoryTimeOffset = .5 53 | TrajectoryOffset = .5,0,0 54 | [Quad7:Quad] 55 | TrajectoryTimeOffset = .6 56 | TrajectoryOffset = .6,0,0 57 | [Quad8:Quad] 58 | TrajectoryTimeOffset = .7 59 | TrajectoryOffset = .7,0,0 60 | [Quad9:Quad] 61 | TrajectoryTimeOffset = .8 62 | TrajectoryOffset = .8,0,0 -------------------------------------------------------------------------------- /config/X_TestMavlink.txt: -------------------------------------------------------------------------------- 1 | # Hover at the initial point using full 3D control 2 | 3 | INCLUDE 5_TrajectoryFollow.txt 4 | 5 | [] 6 | Mavlink.Enable=1 -------------------------------------------------------------------------------- /config/log/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/config/log/.gitignore -------------------------------------------------------------------------------- /config/traj/CircleNoFF.txt: -------------------------------------------------------------------------------- 1 | 0.000,0.000,1.500,-1 2 | 0.020,0.047,1.499,-1 3 | 0.040,0.094,1.497,-1 4 | 0.060,0.141,1.493,-1 5 | 0.080,0.188,1.488,-1 6 | 0.100,0.235,1.482,-1 7 | 0.120,0.281,1.473,-1 8 | 0.140,0.327,1.464,-1 9 | 0.160,0.373,1.453,-1 10 | 0.180,0.418,1.440,-1 11 | 0.200,0.464,1.427,-1 12 | 0.220,0.508,1.411,-1 13 | 0.240,0.552,1.395,-1 14 | 0.260,0.596,1.377,-1 15 | 0.280,0.639,1.357,-1 16 | 0.300,0.681,1.337,-1 17 | 0.320,0.723,1.314,-1 18 | 0.340,0.764,1.291,-1 19 | 0.360,0.804,1.266,-1 20 | 0.380,0.843,1.241,-1 21 | 0.400,0.882,1.214,-1 22 | 0.420,0.919,1.185,-1 23 | 0.440,0.956,1.156,-1 24 | 0.460,0.992,1.125,-1 25 | 0.480,1.027,1.093,-1 26 | 0.500,1.061,1.061,-1 27 | 0.520,1.093,1.027,-1 28 | 0.540,1.125,0.992,-1 29 | 0.560,1.156,0.956,-1 30 | 0.580,1.185,0.919,-1 31 | 0.600,1.214,0.882,-1 32 | 0.620,1.241,0.843,-1 33 | 0.640,1.266,0.804,-1 34 | 0.660,1.291,0.764,-1 35 | 0.680,1.314,0.723,-1 36 | 0.700,1.337,0.681,-1 37 | 0.720,1.357,0.639,-1 38 | 0.740,1.377,0.596,-1 39 | 0.760,1.395,0.552,-1 40 | 0.780,1.411,0.508,-1 41 | 0.800,1.427,0.464,-1 42 | 0.820,1.440,0.418,-1 43 | 0.840,1.453,0.373,-1 44 | 0.860,1.464,0.327,-1 45 | 0.880,1.473,0.281,-1 46 | 0.900,1.482,0.235,-1 47 | 0.920,1.488,0.188,-1 48 | 0.940,1.493,0.141,-1 49 | 0.960,1.497,0.094,-1 50 | 0.980,1.499,0.047,-1 51 | 1.000,1.500,-0.000,-1 52 | 1.020,1.499,-0.047,-1 53 | 1.040,1.497,-0.094,-1 54 | 1.060,1.493,-0.141,-1 55 | 1.080,1.488,-0.188,-1 56 | 1.100,1.482,-0.235,-1 57 | 1.120,1.473,-0.281,-1 58 | 1.140,1.464,-0.327,-1 59 | 1.160,1.453,-0.373,-1 60 | 1.180,1.440,-0.418,-1 61 | 1.200,1.427,-0.464,-1 62 | 1.220,1.411,-0.508,-1 63 | 1.240,1.395,-0.552,-1 64 | 1.260,1.377,-0.596,-1 65 | 1.280,1.357,-0.639,-1 66 | 1.300,1.337,-0.681,-1 67 | 1.320,1.314,-0.723,-1 68 | 1.340,1.291,-0.764,-1 69 | 1.360,1.266,-0.804,-1 70 | 1.380,1.241,-0.843,-1 71 | 1.400,1.214,-0.882,-1 72 | 1.420,1.185,-0.919,-1 73 | 1.440,1.156,-0.956,-1 74 | 1.460,1.125,-0.992,-1 75 | 1.480,1.093,-1.027,-1 76 | 1.500,1.061,-1.061,-1 77 | 1.520,1.027,-1.093,-1 78 | 1.540,0.992,-1.125,-1 79 | 1.560,0.956,-1.156,-1 80 | 1.580,0.919,-1.185,-1 81 | 1.600,0.882,-1.214,-1 82 | 1.620,0.843,-1.241,-1 83 | 1.640,0.804,-1.266,-1 84 | 1.660,0.764,-1.291,-1 85 | 1.680,0.723,-1.314,-1 86 | 1.700,0.681,-1.337,-1 87 | 1.720,0.639,-1.357,-1 88 | 1.740,0.596,-1.377,-1 89 | 1.760,0.552,-1.395,-1 90 | 1.780,0.508,-1.411,-1 91 | 1.800,0.464,-1.427,-1 92 | 1.820,0.418,-1.440,-1 93 | 1.840,0.373,-1.453,-1 94 | 1.860,0.327,-1.464,-1 95 | 1.880,0.281,-1.473,-1 96 | 1.900,0.235,-1.482,-1 97 | 1.920,0.188,-1.488,-1 98 | 1.940,0.141,-1.493,-1 99 | 1.960,0.094,-1.497,-1 100 | 1.980,0.047,-1.499,-1 101 | 2.000,-0.000,-1.500,-1 102 | 2.020,-0.047,-1.499,-1 103 | 2.040,-0.094,-1.497,-1 104 | 2.060,-0.141,-1.493,-1 105 | 2.080,-0.188,-1.488,-1 106 | 2.100,-0.235,-1.482,-1 107 | 2.120,-0.281,-1.473,-1 108 | 2.140,-0.327,-1.464,-1 109 | 2.160,-0.373,-1.453,-1 110 | 2.180,-0.418,-1.440,-1 111 | 2.200,-0.464,-1.427,-1 112 | 2.220,-0.508,-1.411,-1 113 | 2.240,-0.552,-1.395,-1 114 | 2.260,-0.596,-1.377,-1 115 | 2.280,-0.639,-1.357,-1 116 | 2.300,-0.681,-1.337,-1 117 | 2.320,-0.723,-1.314,-1 118 | 2.340,-0.764,-1.291,-1 119 | 2.360,-0.804,-1.266,-1 120 | 2.380,-0.843,-1.241,-1 121 | 2.400,-0.882,-1.214,-1 122 | 2.420,-0.919,-1.185,-1 123 | 2.440,-0.956,-1.156,-1 124 | 2.460,-0.992,-1.125,-1 125 | 2.480,-1.027,-1.093,-1 126 | 2.500,-1.061,-1.061,-1 127 | 2.520,-1.093,-1.027,-1 128 | 2.540,-1.125,-0.992,-1 129 | 2.560,-1.156,-0.956,-1 130 | 2.580,-1.185,-0.919,-1 131 | 2.600,-1.214,-0.882,-1 132 | 2.620,-1.241,-0.843,-1 133 | 2.640,-1.266,-0.804,-1 134 | 2.660,-1.291,-0.764,-1 135 | 2.680,-1.314,-0.723,-1 136 | 2.700,-1.337,-0.681,-1 137 | 2.720,-1.357,-0.639,-1 138 | 2.740,-1.377,-0.596,-1 139 | 2.760,-1.395,-0.552,-1 140 | 2.780,-1.411,-0.508,-1 141 | 2.800,-1.427,-0.464,-1 142 | 2.820,-1.440,-0.418,-1 143 | 2.840,-1.453,-0.373,-1 144 | 2.860,-1.464,-0.327,-1 145 | 2.880,-1.473,-0.281,-1 146 | 2.900,-1.482,-0.235,-1 147 | 2.920,-1.488,-0.188,-1 148 | 2.940,-1.493,-0.141,-1 149 | 2.960,-1.497,-0.094,-1 150 | 2.980,-1.499,-0.047,-1 151 | 3.000,-1.500,0.000,-1 152 | 3.020,-1.499,0.047,-1 153 | 3.040,-1.497,0.094,-1 154 | 3.060,-1.493,0.141,-1 155 | 3.080,-1.488,0.188,-1 156 | 3.100,-1.482,0.235,-1 157 | 3.120,-1.473,0.281,-1 158 | 3.140,-1.464,0.327,-1 159 | 3.160,-1.453,0.373,-1 160 | 3.180,-1.440,0.418,-1 161 | 3.200,-1.427,0.464,-1 162 | 3.220,-1.411,0.508,-1 163 | 3.240,-1.395,0.552,-1 164 | 3.260,-1.377,0.596,-1 165 | 3.280,-1.357,0.639,-1 166 | 3.300,-1.337,0.681,-1 167 | 3.320,-1.314,0.723,-1 168 | 3.340,-1.291,0.764,-1 169 | 3.360,-1.266,0.804,-1 170 | 3.380,-1.241,0.843,-1 171 | 3.400,-1.214,0.882,-1 172 | 3.420,-1.185,0.919,-1 173 | 3.440,-1.156,0.956,-1 174 | 3.460,-1.125,0.992,-1 175 | 3.480,-1.093,1.027,-1 176 | 3.500,-1.061,1.061,-1 177 | 3.520,-1.027,1.093,-1 178 | 3.540,-0.992,1.125,-1 179 | 3.560,-0.956,1.156,-1 180 | 3.580,-0.919,1.185,-1 181 | 3.600,-0.882,1.214,-1 182 | 3.620,-0.843,1.241,-1 183 | 3.640,-0.804,1.266,-1 184 | 3.660,-0.764,1.291,-1 185 | 3.680,-0.723,1.314,-1 186 | 3.700,-0.681,1.337,-1 187 | 3.720,-0.639,1.357,-1 188 | 3.740,-0.596,1.377,-1 189 | 3.760,-0.552,1.395,-1 190 | 3.780,-0.508,1.411,-1 191 | 3.800,-0.464,1.427,-1 192 | 3.820,-0.418,1.440,-1 193 | 3.840,-0.373,1.453,-1 194 | 3.860,-0.327,1.464,-1 195 | 3.880,-0.281,1.473,-1 196 | 3.900,-0.235,1.482,-1 197 | 3.920,-0.188,1.488,-1 198 | 3.940,-0.141,1.493,-1 199 | 3.960,-0.094,1.497,-1 200 | 3.980,-0.047,1.499,-1 201 | -------------------------------------------------------------------------------- /config/traj/HelixNoFF.txt: -------------------------------------------------------------------------------- 1 | 0.000,0.000,1.500,-1.000 2 | 0.100,0.047,1.499,-1.010 3 | 0.200,0.094,1.497,-1.020 4 | 0.300,0.141,1.493,-1.030 5 | 0.400,0.188,1.488,-1.040 6 | 0.500,0.235,1.482,-1.050 7 | 0.600,0.281,1.473,-1.060 8 | 0.700,0.327,1.464,-1.070 9 | 0.800,0.373,1.453,-1.080 10 | 0.900,0.418,1.440,-1.090 11 | 1.000,0.464,1.427,-1.100 12 | 1.100,0.508,1.411,-1.110 13 | 1.200,0.552,1.395,-1.120 14 | 1.300,0.596,1.377,-1.130 15 | 1.400,0.639,1.357,-1.140 16 | 1.500,0.681,1.337,-1.150 17 | 1.600,0.723,1.314,-1.160 18 | 1.700,0.764,1.291,-1.170 19 | 1.800,0.804,1.266,-1.180 20 | 1.900,0.843,1.241,-1.190 21 | 2.000,0.882,1.214,-1.200 22 | 2.100,0.919,1.185,-1.210 23 | 2.200,0.956,1.156,-1.220 24 | 2.300,0.992,1.125,-1.230 25 | 2.400,1.027,1.093,-1.240 26 | 2.500,1.061,1.061,-1.250 27 | 2.600,1.093,1.027,-1.260 28 | 2.700,1.125,0.992,-1.270 29 | 2.800,1.156,0.956,-1.280 30 | 2.900,1.185,0.919,-1.290 31 | 3.000,1.214,0.882,-1.300 32 | 3.100,1.241,0.843,-1.310 33 | 3.200,1.266,0.804,-1.320 34 | 3.300,1.291,0.764,-1.330 35 | 3.400,1.314,0.723,-1.340 36 | 3.500,1.337,0.681,-1.350 37 | 3.600,1.357,0.639,-1.360 38 | 3.700,1.377,0.596,-1.370 39 | 3.800,1.395,0.552,-1.380 40 | 3.900,1.411,0.508,-1.390 41 | 4.000,1.427,0.464,-1.400 42 | 4.100,1.440,0.418,-1.410 43 | 4.200,1.453,0.373,-1.420 44 | 4.300,1.464,0.327,-1.430 45 | 4.400,1.473,0.281,-1.440 46 | 4.500,1.482,0.235,-1.450 47 | 4.600,1.488,0.188,-1.460 48 | 4.700,1.493,0.141,-1.470 49 | 4.800,1.497,0.094,-1.480 50 | 4.900,1.499,0.047,-1.490 51 | 5.000,1.500,0.000,-1.500 52 | 5.100,1.499,-0.047,-1.510 53 | 5.200,1.497,-0.094,-1.520 54 | 5.300,1.493,-0.141,-1.530 55 | 5.400,1.488,-0.188,-1.540 56 | 5.500,1.482,-0.235,-1.550 57 | 5.600,1.473,-0.281,-1.560 58 | 5.700,1.464,-0.327,-1.570 59 | 5.800,1.453,-0.373,-1.580 60 | 5.900,1.440,-0.418,-1.590 61 | 6.000,1.427,-0.464,-1.600 62 | 6.100,1.411,-0.508,-1.610 63 | 6.200,1.395,-0.552,-1.620 64 | 6.300,1.377,-0.596,-1.630 65 | 6.400,1.357,-0.639,-1.640 66 | 6.500,1.337,-0.681,-1.650 67 | 6.600,1.314,-0.723,-1.660 68 | 6.700,1.291,-0.764,-1.670 69 | 6.800,1.266,-0.804,-1.680 70 | 6.900,1.241,-0.843,-1.690 71 | 7.000,1.214,-0.882,-1.700 72 | 7.100,1.185,-0.919,-1.710 73 | 7.200,1.156,-0.956,-1.720 74 | 7.300,1.125,-0.992,-1.730 75 | 7.400,1.093,-1.027,-1.740 76 | 7.500,1.061,-1.061,-1.750 77 | 7.600,1.027,-1.093,-1.760 78 | 7.700,0.992,-1.125,-1.770 79 | 7.800,0.956,-1.156,-1.780 80 | 7.900,0.919,-1.185,-1.790 81 | 8.000,0.882,-1.214,-1.800 82 | 8.100,0.843,-1.241,-1.810 83 | 8.200,0.804,-1.266,-1.820 84 | 8.300,0.764,-1.291,-1.830 85 | 8.400,0.723,-1.314,-1.840 86 | 8.500,0.681,-1.337,-1.850 87 | 8.600,0.639,-1.357,-1.860 88 | 8.700,0.596,-1.377,-1.870 89 | 8.800,0.552,-1.395,-1.880 90 | 8.900,0.508,-1.411,-1.890 91 | 9.000,0.464,-1.427,-1.900 92 | 9.100,0.418,-1.440,-1.910 93 | 9.200,0.373,-1.453,-1.920 94 | 9.300,0.327,-1.464,-1.930 95 | 9.400,0.281,-1.473,-1.940 96 | 9.500,0.235,-1.482,-1.950 97 | 9.600,0.188,-1.488,-1.960 98 | 9.700,0.141,-1.493,-1.970 99 | 9.800,0.094,-1.497,-1.980 100 | 9.900,0.047,-1.499,-1.990 101 | 10.000,0.000,-1.500,-2.000 102 | 10.100,-0.047,-1.499,-2.010 103 | 10.200,-0.094,-1.497,-2.020 104 | 10.300,-0.141,-1.493,-2.030 105 | 10.400,-0.188,-1.488,-2.040 106 | 10.500,-0.235,-1.482,-2.050 107 | 10.600,-0.281,-1.473,-2.060 108 | 10.700,-0.327,-1.464,-2.070 109 | 10.800,-0.373,-1.453,-2.080 110 | 10.900,-0.418,-1.440,-2.090 111 | 11.000,-0.464,-1.427,-2.100 112 | 11.100,-0.508,-1.411,-2.110 113 | 11.200,-0.552,-1.395,-2.120 114 | 11.300,-0.596,-1.377,-2.130 115 | 11.400,-0.639,-1.357,-2.140 116 | 11.500,-0.681,-1.337,-2.150 117 | 11.600,-0.723,-1.314,-2.160 118 | 11.700,-0.764,-1.291,-2.170 119 | 11.800,-0.804,-1.266,-2.180 120 | 11.900,-0.843,-1.241,-2.190 121 | 12.000,-0.882,-1.214,-2.200 122 | 12.100,-0.919,-1.185,-2.210 123 | 12.200,-0.956,-1.156,-2.220 124 | 12.300,-0.992,-1.125,-2.230 125 | 12.400,-1.027,-1.093,-2.240 126 | 12.500,-1.061,-1.061,-2.250 127 | 12.600,-1.093,-1.027,-2.260 128 | 12.700,-1.125,-0.992,-2.270 129 | 12.800,-1.156,-0.956,-2.280 130 | 12.900,-1.185,-0.919,-2.290 131 | 13.000,-1.214,-0.882,-2.300 132 | 13.100,-1.241,-0.843,-2.310 133 | 13.200,-1.266,-0.804,-2.320 134 | 13.300,-1.291,-0.764,-2.330 135 | 13.400,-1.314,-0.723,-2.340 136 | 13.500,-1.337,-0.681,-2.350 137 | 13.600,-1.357,-0.639,-2.360 138 | 13.700,-1.377,-0.596,-2.370 139 | 13.800,-1.395,-0.552,-2.380 140 | 13.900,-1.411,-0.508,-2.390 141 | 14.000,-1.427,-0.464,-2.400 142 | 14.100,-1.440,-0.418,-2.410 143 | 14.200,-1.453,-0.373,-2.420 144 | 14.300,-1.464,-0.327,-2.430 145 | 14.400,-1.473,-0.281,-2.440 146 | 14.500,-1.482,-0.235,-2.450 147 | 14.600,-1.488,-0.188,-2.460 148 | 14.700,-1.493,-0.141,-2.470 149 | 14.800,-1.497,-0.094,-2.480 150 | 14.900,-1.499,-0.047,-2.490 151 | 15.000,-1.500,-0.000,-2.500 152 | 15.100,-1.499,0.047,-2.510 153 | 15.200,-1.497,0.094,-2.520 154 | 15.300,-1.493,0.141,-2.530 155 | 15.400,-1.488,0.188,-2.540 156 | 15.500,-1.482,0.235,-2.550 157 | 15.600,-1.473,0.281,-2.560 158 | 15.700,-1.464,0.327,-2.570 159 | 15.800,-1.453,0.373,-2.580 160 | 15.900,-1.440,0.418,-2.590 161 | 16.000,-1.427,0.464,-2.600 162 | 16.100,-1.411,0.508,-2.610 163 | 16.200,-1.395,0.552,-2.620 164 | 16.300,-1.377,0.596,-2.630 165 | 16.400,-1.357,0.639,-2.640 166 | 16.500,-1.337,0.681,-2.650 167 | 16.600,-1.314,0.723,-2.660 168 | 16.700,-1.291,0.764,-2.670 169 | 16.800,-1.266,0.804,-2.680 170 | 16.900,-1.241,0.843,-2.690 171 | 17.000,-1.214,0.882,-2.700 172 | 17.100,-1.185,0.919,-2.710 173 | 17.200,-1.156,0.956,-2.720 174 | 17.300,-1.125,0.992,-2.730 175 | 17.400,-1.093,1.027,-2.740 176 | 17.500,-1.061,1.061,-2.750 177 | 17.600,-1.027,1.093,-2.760 178 | 17.700,-0.992,1.125,-2.770 179 | 17.800,-0.956,1.156,-2.780 180 | 17.900,-0.919,1.185,-2.790 181 | 18.000,-0.882,1.214,-2.800 182 | 18.100,-0.843,1.241,-2.810 183 | 18.200,-0.804,1.266,-2.820 184 | 18.300,-0.764,1.291,-2.830 185 | 18.400,-0.723,1.314,-2.840 186 | 18.500,-0.681,1.337,-2.850 187 | 18.600,-0.639,1.357,-2.860 188 | 18.700,-0.596,1.377,-2.870 189 | 18.800,-0.552,1.395,-2.880 190 | 18.900,-0.508,1.411,-2.890 191 | 19.000,-0.464,1.427,-2.900 192 | 19.100,-0.418,1.440,-2.910 193 | 19.200,-0.373,1.453,-2.920 194 | 19.300,-0.327,1.464,-2.930 195 | 19.400,-0.281,1.473,-2.940 196 | 19.500,-0.235,1.482,-2.950 197 | 19.600,-0.188,1.488,-2.960 198 | 19.700,-0.141,1.493,-2.970 199 | 19.800,-0.094,1.497,-2.980 200 | 19.900,-0.047,1.499,-2.990 201 | -------------------------------------------------------------------------------- /config/traj/MakeCircleTrajectory.py: -------------------------------------------------------------------------------- 1 | import math; 2 | 3 | def fmt(value): 4 | return "%.3f" % value 5 | 6 | period = 4 7 | radius = 1.5 8 | timestep = 0.02 9 | maxtime = period*1 10 | 11 | with open('CircleNoFF.txt', 'w') as the_file: 12 | t=0; 13 | while t <= maxtime: 14 | x = math.sin(t * 2 * math.pi / period) * radius; 15 | y = math.cos(t * 2 * math.pi / period) * radius; 16 | the_file.write(fmt(t) + "," + fmt(x) + "," + fmt(y) + "," + "-1\n"); 17 | t += timestep; 18 | 19 | -------------------------------------------------------------------------------- /config/traj/MakeHelixTrajectory.py: -------------------------------------------------------------------------------- 1 | import math; 2 | 3 | def fmt(value): 4 | return "%.3f" % value 5 | 6 | period = 20 7 | radius = 1.5 8 | timestep = 0.1 9 | maxtime = period*1 10 | z = -1 11 | 12 | with open('HelixNoFF.txt', 'w') as the_file: 13 | t=0; 14 | while t <= maxtime: 15 | x = math.sin(t * 2 * math.pi / period) * radius; 16 | y = math.cos(t * 2 * math.pi / period) * radius; 17 | the_file.write(fmt(t) + "," + fmt(x) + "," + fmt(y) + "," + fmt(z) + "\n"); 18 | t += timestep; 19 | z -= 0.01 20 | -------------------------------------------------------------------------------- /config/traj/MakeHelixUpDownTrajectory.py: -------------------------------------------------------------------------------- 1 | import math; 2 | 3 | def fmt(value): 4 | return "%.3f" % value 5 | 6 | period = 40 7 | radius = 1.5 8 | timestep = 0.1 9 | maxtime = period*1 10 | z = -1 11 | 12 | with open('HelixUpDownNoFF.txt', 'w') as the_file: 13 | t=0; 14 | while t <= maxtime/2: 15 | x = math.sin(t * 2 * math.pi / period) * radius; 16 | y = math.cos(t * 2 * math.pi / period) * radius; 17 | the_file.write(fmt(t) + "," + fmt(x) + "," + fmt(y) + "," + fmt(z) + "\n"); 18 | t += timestep; 19 | z -= 0.01 20 | while t>maxtime/2 and t <= maxtime: 21 | x = math.sin(t * 2 * math.pi / period) * radius; 22 | y = math.cos(t * 2 * math.pi / period) * radius; 23 | the_file.write(fmt(t) + "," + fmt(x) + "," + fmt(y) + "," + fmt(z) + "\n"); 24 | t += timestep; 25 | z += 0.01 26 | 27 | -------------------------------------------------------------------------------- /config/traj/MakePeriodicTrajectory.py: -------------------------------------------------------------------------------- 1 | import math; 2 | 3 | def fmt(value): 4 | return "%.3f" % value 5 | 6 | period = [4, 2, 4] 7 | radius = 1.5 8 | timestep = 0.02 9 | maxtime = max(period)*3 10 | timemult = [1, 1, 1] 11 | phase=[0,0,0] 12 | amp = [1,0.4,.5] 13 | center = [0, 0, -2] 14 | 15 | with open('FigureEight.txt', 'w') as the_file: 16 | t=0; 17 | px = 0; 18 | py = 0; 19 | pz = 0; 20 | while t <= maxtime: 21 | x = math.sin(t * 2 * math.pi / period[0] + phase[0]) * radius * amp[0] + center[0]; 22 | y = math.sin(t * 2 * math.pi / period[1] + phase[1]) * radius * amp[1] + center[1]; 23 | z = math.sin(t * 2 * math.pi / period[2] + phase[2]) * radius * amp[2] + center[2]; 24 | the_file.write(fmt(t) + "," + fmt(x) + "," + fmt(y) + "," + fmt(z)); 25 | vx = 0; 26 | vy = 0; 27 | vz = 0; 28 | ######## BEGIN STUDENT CODE 29 | 30 | ######## END STUDENT CODE 31 | the_file.write("," + fmt(vx) + "," + fmt(vy) + "," + fmt(vz)); 32 | ######## EXAMPLE SOLUTION 33 | #the_file.write("," + fmt((x-px)/timestep) + "," + fmt((y-py)/timestep) + "," + fmt((z-pz)/timestep)); 34 | #px = x; 35 | #py = y; 36 | #pz = z; 37 | ######## END EXAMPLE SOLUTION 38 | 39 | the_file.write("\n"); 40 | 41 | t += timestep; 42 | 43 | -------------------------------------------------------------------------------- /config/traj/MakeSpiralTrajectory.py: -------------------------------------------------------------------------------- 1 | import math; 2 | 3 | def fmt(value): 4 | return "%.3f" % value 5 | 6 | period = 4 7 | radius = 0.5 8 | timestep = 0.05 9 | maxtime = period*1 10 | 11 | with open('SpiralNoFF.txt', 'w') as the_file: 12 | t=0; 13 | while t <= maxtime: 14 | x = math.sin(t * 2 * math.pi / period) * radius; 15 | y = math.cos(t * 2 * math.pi / period) * radius; 16 | the_file.write(fmt(t) + "," + fmt(x) + "," + fmt(y) + "," + "-1\n"); 17 | t += timestep; 18 | radius += 0.01 19 | -------------------------------------------------------------------------------- /config/traj/Origin.txt: -------------------------------------------------------------------------------- 1 | 0,0,0,-1 -------------------------------------------------------------------------------- /config/traj/SpiralNoFF.txt: -------------------------------------------------------------------------------- 1 | 0.000,0.000,0.500,-1 2 | 0.050,0.040,0.508,-1 3 | 0.100,0.081,0.514,-1 4 | 0.150,0.124,0.515,-1 5 | 0.200,0.167,0.514,-1 6 | 0.250,0.210,0.508,-1 7 | 0.300,0.254,0.499,-1 8 | 0.350,0.298,0.486,-1 9 | 0.400,0.341,0.469,-1 10 | 0.450,0.383,0.449,-1 11 | 0.500,0.424,0.424,-1 12 | 0.550,0.464,0.396,-1 13 | 0.600,0.502,0.364,-1 14 | 0.650,0.537,0.329,-1 15 | 0.700,0.570,0.291,-1 16 | 0.750,0.601,0.249,-1 17 | 0.800,0.628,0.204,-1 18 | 0.850,0.651,0.156,-1 19 | 0.900,0.672,0.106,-1 20 | 0.950,0.688,0.054,-1 21 | 1.000,0.700,-0.000,-1 22 | 1.050,0.708,-0.056,-1 23 | 1.100,0.711,-0.113,-1 24 | 1.150,0.710,-0.170,-1 25 | 1.200,0.704,-0.229,-1 26 | 1.250,0.693,-0.287,-1 27 | 1.300,0.677,-0.345,-1 28 | 1.350,0.657,-0.402,-1 29 | 1.400,0.631,-0.458,-1 30 | 1.450,0.601,-0.513,-1 31 | 1.500,0.566,-0.566,-1 32 | 1.550,0.526,-0.616,-1 33 | 1.600,0.482,-0.663,-1 34 | 1.650,0.434,-0.708,-1 35 | 1.700,0.381,-0.748,-1 36 | 1.750,0.325,-0.785,-1 37 | 1.800,0.266,-0.818,-1 38 | 1.850,0.203,-0.846,-1 39 | 1.900,0.138,-0.869,-1 40 | 1.950,0.070,-0.887,-1 41 | 2.000,-0.000,-0.900,-1 42 | 2.050,-0.071,-0.907,-1 43 | 2.100,-0.144,-0.909,-1 44 | 2.150,-0.217,-0.904,-1 45 | 2.200,-0.290,-0.894,-1 46 | 2.250,-0.364,-0.878,-1 47 | 2.300,-0.436,-0.855,-1 48 | 2.350,-0.507,-0.827,-1 49 | 2.400,-0.576,-0.793,-1 50 | 2.450,-0.643,-0.753,-1 51 | 2.500,-0.707,-0.707,-1 52 | 2.550,-0.768,-0.656,-1 53 | 2.600,-0.825,-0.600,-1 54 | 2.650,-0.878,-0.538,-1 55 | 2.700,-0.927,-0.472,-1 56 | 2.750,-0.970,-0.402,-1 57 | 2.800,-1.008,-0.328,-1 58 | 2.850,-1.040,-0.250,-1 59 | 2.900,-1.067,-0.169,-1 60 | 2.950,-1.087,-0.086,-1 61 | 3.000,-1.100,-0.000,-1 62 | 3.050,-1.107,0.087,-1 63 | 3.100,-1.106,0.175,-1 64 | 3.150,-1.099,0.264,-1 65 | 3.200,-1.084,0.352,-1 66 | 3.250,-1.062,0.440,-1 67 | 3.300,-1.034,0.527,-1 68 | 3.350,-0.998,0.611,-1 69 | 3.400,-0.955,0.694,-1 70 | 3.450,-0.905,0.773,-1 71 | 3.500,-0.849,0.849,-1 72 | 3.550,-0.786,0.920,-1 73 | 3.600,-0.717,0.987,-1 74 | 3.650,-0.643,1.049,-1 75 | 3.700,-0.563,1.105,-1 76 | 3.750,-0.478,1.155,-1 77 | 3.800,-0.389,1.198,-1 78 | 3.850,-0.296,1.235,-1 79 | 3.900,-0.200,1.264,-1 80 | 3.950,-0.101,1.286,-1 81 | 4.000,-0.000,1.300,-1 82 | -------------------------------------------------------------------------------- /lib/freeglut/Copying.txt: -------------------------------------------------------------------------------- 1 | 2 | Freeglut Copyright 3 | ------------------ 4 | 5 | Freeglut code without an explicit copyright is covered by the following 6 | copyright: 7 | 8 | Copyright (c) 1999-2000 Pawel W. Olszta. All Rights Reserved. 9 | Permission is hereby granted, free of charge, to any person obtaining a copy 10 | of this software and associated documentation files (the "Software"), to deal 11 | in the Software without restriction, including without limitation the rights 12 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | copies or substantial portions of the Software. 14 | 15 | The above copyright notice and this permission notice shall be included in 16 | all copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 21 | PAWEL W. OLSZTA BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 22 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 23 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 24 | 25 | Except as contained in this notice, the name of Pawel W. Olszta shall not be 26 | used in advertising or otherwise to promote the sale, use or other dealings 27 | in this Software without prior written authorization from Pawel W. Olszta. 28 | -------------------------------------------------------------------------------- /lib/freeglut/Readme.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/lib/freeglut/Readme.txt -------------------------------------------------------------------------------- /lib/freeglut/bin/freeglut.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/lib/freeglut/bin/freeglut.dll -------------------------------------------------------------------------------- /lib/freeglut/bin/x64/freeglut.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/lib/freeglut/bin/x64/freeglut.dll -------------------------------------------------------------------------------- /lib/freeglut/include/GL/freeglut.h: -------------------------------------------------------------------------------- 1 | #ifndef __FREEGLUT_H__ 2 | #define __FREEGLUT_H__ 3 | 4 | /* 5 | * freeglut.h 6 | * 7 | * The freeglut library include file 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 10 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 11 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 12 | * PAWEL W. OLSZTA BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 13 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 14 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 15 | */ 16 | 17 | #include "freeglut_std.h" 18 | #include "freeglut_ext.h" 19 | 20 | /*** END OF FILE ***/ 21 | 22 | #endif /* __FREEGLUT_H__ */ 23 | -------------------------------------------------------------------------------- /lib/freeglut/include/GL/glut.h: -------------------------------------------------------------------------------- 1 | #ifndef __GLUT_H__ 2 | #define __GLUT_H__ 3 | 4 | /* 5 | * glut.h 6 | * 7 | * The freeglut library include file 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 10 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 11 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 12 | * PAWEL W. OLSZTA BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 13 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 14 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 15 | */ 16 | 17 | #include "freeglut_std.h" 18 | 19 | /*** END OF FILE ***/ 20 | 21 | #endif /* __GLUT_H__ */ 22 | -------------------------------------------------------------------------------- /lib/freeglut/lib/freeglut.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/lib/freeglut/lib/freeglut.lib -------------------------------------------------------------------------------- /lib/freeglut/lib/x64/freeglut.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/lib/freeglut/lib/x64/freeglut.lib -------------------------------------------------------------------------------- /lib/matrix/AxisAngle.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file AxisAngle.hpp 3 | * 4 | * @author James Goppert 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "math.hpp" 10 | #include "helper_functions.hpp" 11 | 12 | namespace matrix 13 | { 14 | 15 | template 16 | class Dcm; 17 | 18 | template 19 | class Euler; 20 | 21 | template 22 | class AxisAngle; 23 | 24 | /** 25 | * AxisAngle class 26 | * 27 | * The rotation between two coordinate frames is 28 | * described by this class. 29 | */ 30 | template 31 | class AxisAngle : public Vector 32 | { 33 | public: 34 | virtual ~AxisAngle() {}; 35 | 36 | typedef Matrix Matrix31; 37 | 38 | /** 39 | * Constructor from array 40 | * 41 | * @param data_ array 42 | */ 43 | AxisAngle(const Type *data_) : 44 | Vector(data_) 45 | { 46 | } 47 | 48 | /** 49 | * Standard constructor 50 | */ 51 | AxisAngle() : 52 | Vector() 53 | { 54 | } 55 | 56 | /** 57 | * Constructor from Matrix31 58 | * 59 | * @param other Matrix31 to copy 60 | */ 61 | AxisAngle(const Matrix31 &other) : 62 | Vector(other) 63 | { 64 | } 65 | 66 | /** 67 | * Constructor from quaternion 68 | * 69 | * This sets the instance from a quaternion representing coordinate transformation from 70 | * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described 71 | * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. 72 | * 73 | * @param q quaternion 74 | */ 75 | AxisAngle(const Quaternion &q) : 76 | Vector() 77 | { 78 | AxisAngle &v = *this; 79 | Type ang = (Type)2.0f*acosf(q(0)); 80 | Type mag = sinf(ang/2.0f); 81 | if (fabs(ang) < 1e-10f) { 82 | v(0) = 0; 83 | v(1) = 0; 84 | v(2) = 0; 85 | } else { 86 | v(0) = ang*q(1)/mag; 87 | v(1) = ang*q(2)/mag; 88 | v(2) = ang*q(3)/mag; 89 | } 90 | } 91 | 92 | /** 93 | * Constructor from dcm 94 | * 95 | * Instance is initialized from a dcm representing coordinate transformation 96 | * from frame 2 to frame 1. 97 | * 98 | * @param dcm dcm to set quaternion to 99 | */ 100 | AxisAngle(const Dcm &dcm) : 101 | Vector() 102 | { 103 | AxisAngle &v = *this; 104 | v = Quaternion(dcm); 105 | } 106 | 107 | /** 108 | * Constructor from euler angles 109 | * 110 | * This sets the instance to a quaternion representing coordinate transformation from 111 | * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described 112 | * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. 113 | * 114 | * @param euler euler angle instance 115 | */ 116 | AxisAngle(const Euler &euler) : 117 | Vector() 118 | { 119 | AxisAngle &v = *this; 120 | v = Quaternion(euler); 121 | } 122 | 123 | /** 124 | * Constructor from 3 axis angle values (unit vector * angle) 125 | * 126 | * @param x r_x*angle 127 | * @param y r_y*angle 128 | * @param z r_z*angle 129 | */ 130 | AxisAngle(Type x, Type y, Type z) : 131 | Vector() 132 | { 133 | AxisAngle &v = *this; 134 | v(0) = x; 135 | v(1) = y; 136 | v(2) = z; 137 | } 138 | 139 | /** 140 | * Constructor from axis and angle 141 | * 142 | * @param axis An axis of rotation, normalized if not unit length 143 | * @param angle The amount to rotate 144 | */ 145 | AxisAngle(const Matrix31 & axis_, Type angle_) : 146 | Vector() 147 | { 148 | AxisAngle &v = *this; 149 | // make sure axis is a unit vector 150 | Vector a = axis_; 151 | a = a.unit(); 152 | v(0) = a(0)*angle_; 153 | v(1) = a(1)*angle_; 154 | v(2) = a(2)*angle_; 155 | } 156 | 157 | 158 | Vector axis() { 159 | return Vector::unit(); 160 | } 161 | 162 | Type angle() { 163 | return Vector::norm(); 164 | } 165 | }; 166 | 167 | typedef AxisAngle AxisAnglef; 168 | 169 | } // namespace matrix 170 | 171 | /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ 172 | -------------------------------------------------------------------------------- /lib/matrix/Dcm.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Dcm.hpp 3 | * 4 | * A direction cosine matrix class. 5 | * All rotations and axis systems follow the right-hand rule. 6 | * 7 | * This library uses the convention that premultiplying a three dimensional 8 | * vector represented in coordinate system 1 will apply a rotation from coordinate system 9 | * 1 to coordinate system 2 to the vector. 10 | * Likewise, a matrix instance of this class also represents a coordinate transformation 11 | * from frame 2 to frame 1. 12 | * 13 | * @author James Goppert 14 | */ 15 | 16 | #pragma once 17 | 18 | #include "math.hpp" 19 | 20 | 21 | namespace matrix 22 | { 23 | 24 | template 25 | class Quaternion; 26 | 27 | template 28 | class Euler; 29 | 30 | template 31 | class AxisAngle; 32 | 33 | 34 | /** 35 | * Direction cosine matrix class 36 | * 37 | * The rotation between two coordinate frames is 38 | * described by this class. 39 | */ 40 | template 41 | class Dcm : public Matrix 42 | { 43 | public: 44 | virtual ~Dcm() {}; 45 | 46 | typedef Matrix Vector3; 47 | 48 | /** 49 | * Standard constructor 50 | * 51 | * Initializes to identity 52 | */ 53 | Dcm() : Matrix() 54 | { 55 | (*this) = eye(); 56 | } 57 | 58 | /** 59 | * Constructor from array 60 | * 61 | * @param _data pointer to array 62 | */ 63 | Dcm(const Type *data_) : Matrix(data_) 64 | { 65 | } 66 | 67 | /** 68 | * Copy constructor 69 | * 70 | * @param other Matrix33 to set dcm to 71 | */ 72 | Dcm(const Matrix &other) : Matrix(other) 73 | { 74 | } 75 | 76 | /** 77 | * Constructor from quaternion 78 | * 79 | * Instance is initialized from quaternion representing 80 | * coordinate transformation from frame 2 to frame 1. 81 | * 82 | * @param q quaternion to set dcm to 83 | */ 84 | Dcm(const Quaternion &q) 85 | { 86 | Dcm &dcm = *this; 87 | Type a = q(0); 88 | Type b = q(1); 89 | Type c = q(2); 90 | Type d = q(3); 91 | Type aSq = a * a; 92 | Type bSq = b * b; 93 | Type cSq = c * c; 94 | Type dSq = d * d; 95 | dcm(0, 0) = aSq + bSq - cSq - dSq; 96 | dcm(0, 1) = 2 * (b * c - a * d); 97 | dcm(0, 2) = 2 * (a * c + b * d); 98 | dcm(1, 0) = 2 * (b * c + a * d); 99 | dcm(1, 1) = aSq - bSq + cSq - dSq; 100 | dcm(1, 2) = 2 * (c * d - a * b); 101 | dcm(2, 0) = 2 * (b * d - a * c); 102 | dcm(2, 1) = 2 * (a * b + c * d); 103 | dcm(2, 2) = aSq - bSq - cSq + dSq; 104 | } 105 | 106 | /** 107 | * Constructor from euler angles 108 | * 109 | * This sets the transformation matrix from frame 2 to frame 1 where the rotation 110 | * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence. 111 | * 112 | * 113 | * @param euler euler angle instance 114 | */ 115 | Dcm(const Euler &euler) 116 | { 117 | Dcm &dcm = *this; 118 | Type cosPhi = Type(cos(euler.phi())); 119 | Type sinPhi = Type(sin(euler.phi())); 120 | Type cosThe = Type(cos(euler.theta())); 121 | Type sinThe = Type(sin(euler.theta())); 122 | Type cosPsi = Type(cos(euler.psi())); 123 | Type sinPsi = Type(sin(euler.psi())); 124 | 125 | dcm(0, 0) = cosThe * cosPsi; 126 | dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; 127 | dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; 128 | 129 | dcm(1, 0) = cosThe * sinPsi; 130 | dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; 131 | dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; 132 | 133 | dcm(2, 0) = -sinThe; 134 | dcm(2, 1) = sinPhi * cosThe; 135 | dcm(2, 2) = cosPhi * cosThe; 136 | } 137 | 138 | 139 | /** 140 | * Constructor from axis angle 141 | * 142 | * This sets the transformation matrix from frame 2 to frame 1 where the rotation 143 | * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence. 144 | * 145 | * 146 | * @param euler euler angle instance 147 | */ 148 | Dcm(const AxisAngle &aa) 149 | { 150 | Dcm &dcm = *this; 151 | dcm = Quaternion(aa); 152 | } 153 | 154 | Vector vee() const // inverse to Vector.hat() operation 155 | { 156 | const Dcm &A(*this); 157 | Vector v; 158 | v(0) = -A(1, 2); 159 | v(1) = A(0, 2); 160 | v(2) = -A(0, 1); 161 | return v; 162 | } 163 | 164 | void renormalize() 165 | { 166 | /* renormalize rows */ 167 | for (int row = 0; row < 3; row++) { 168 | matrix::Vector3f rvec(this->_data[row]); 169 | this->setRow(row, rvec.normalized()); 170 | } 171 | } 172 | }; 173 | 174 | typedef Dcm Dcmf; 175 | 176 | } // namespace matrix 177 | 178 | /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ 179 | -------------------------------------------------------------------------------- /lib/matrix/Euler.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Euler.hpp 3 | * 4 | * All rotations and axis systems follow the right-hand rule 5 | * 6 | * An instance of this class defines a rotation from coordinate frame 1 to coordinate frame 2. 7 | * It follows the convention of a 3-2-1 intrinsic Tait-Bryan rotation sequence. 8 | * In order to go from frame 1 to frame 2 we apply the following rotations consecutively. 9 | * 1) We rotate about our initial Z axis by an angle of _psi. 10 | * 2) We rotate about the newly created Y' axis by an angle of _theta. 11 | * 3) We rotate about the newly created X'' axis by an angle of _phi. 12 | * 13 | * @author James Goppert 14 | */ 15 | 16 | #pragma once 17 | 18 | #include "math.hpp" 19 | 20 | #ifndef M_PI 21 | #define M_PI (3.14159265358979323846f) 22 | #endif 23 | 24 | namespace matrix 25 | { 26 | 27 | template 28 | class Dcm; 29 | 30 | template 31 | class Quaternion; 32 | 33 | /** 34 | * Euler angles class 35 | * 36 | * This class describes the rotation from frame 1 37 | * to frame 2 via 3-2-1 intrinsic Tait-Bryan rotation sequence. 38 | */ 39 | template 40 | class Euler : public Vector 41 | { 42 | public: 43 | virtual ~Euler() {}; 44 | 45 | /** 46 | * Standard constructor 47 | */ 48 | Euler() : Vector() 49 | { 50 | } 51 | 52 | /** 53 | * Copy constructor 54 | * 55 | * @param other vector to copy 56 | */ 57 | Euler(const Vector &other) : 58 | Vector(other) 59 | { 60 | } 61 | 62 | /** 63 | * Constructor from Matrix31 64 | * 65 | * @param other Matrix31 to copy 66 | */ 67 | Euler(const Matrix &other) : 68 | Vector(other) 69 | { 70 | } 71 | 72 | /** 73 | * Constructor from euler angles 74 | * 75 | * Instance is initialized from an 3-2-1 intrinsic Tait-Bryan 76 | * rotation sequence representing transformation from frame 1 77 | * to frame 2. 78 | * 79 | * @param phi_ rotation angle about X axis 80 | * @param theta_ rotation angle about Y axis 81 | * @param psi_ rotation angle about Z axis 82 | */ 83 | Euler(Type phi_, Type theta_, Type psi_) : Vector() 84 | { 85 | phi() = phi_; 86 | theta() = theta_; 87 | psi() = psi_; 88 | } 89 | 90 | /** 91 | * Constructor from DCM matrix 92 | * 93 | * Instance is set from Dcm representing transformation from 94 | * frame 2 to frame 1. 95 | * This instance will hold the angles defining the 3-2-1 intrinsic 96 | * Tait-Bryan rotation sequence from frame 1 to frame 2. 97 | * 98 | * @param dcm Direction cosine matrix 99 | */ 100 | Euler(const Dcm &dcm) : Vector() 101 | { 102 | Type phi_val = Type(atan2(dcm(2, 1), dcm(2, 2))); 103 | Type theta_val = Type(asin(-dcm(2, 0))); 104 | Type psi_val = Type(atan2(dcm(1, 0), dcm(0, 0))); 105 | Type pi = Type(M_PI); 106 | 107 | if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) { 108 | phi_val = Type(0.0); 109 | psi_val = Type(atan2(dcm(1, 2), dcm(0, 2))); 110 | 111 | } else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) { 112 | phi_val = Type(0.0); 113 | psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2))); 114 | } 115 | 116 | phi() = phi_val; 117 | theta() = theta_val; 118 | psi() = psi_val; 119 | } 120 | 121 | /** 122 | * Constructor from quaternion instance. 123 | * 124 | * Instance is set from a quaternion representing transformation 125 | * from frame 2 to frame 1. 126 | * This instance will hold the angles defining the 3-2-1 intrinsic 127 | * Tait-Bryan rotation sequence from frame 1 to frame 2. 128 | * 129 | * @param q quaternion 130 | */ 131 | Euler(const Quaternion &q) : 132 | Vector() 133 | { 134 | *this = Euler(Dcm(q)); 135 | } 136 | 137 | inline Type phi() const 138 | { 139 | return (*this)(0); 140 | } 141 | inline Type theta() const 142 | { 143 | return (*this)(1); 144 | } 145 | inline Type psi() const 146 | { 147 | return (*this)(2); 148 | } 149 | 150 | inline Type &phi() 151 | { 152 | return (*this)(0); 153 | } 154 | inline Type &theta() 155 | { 156 | return (*this)(1); 157 | } 158 | inline Type &psi() 159 | { 160 | return (*this)(2); 161 | } 162 | 163 | }; 164 | 165 | typedef Euler Eulerf; 166 | 167 | } // namespace matrix 168 | 169 | /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ 170 | -------------------------------------------------------------------------------- /lib/matrix/LICENSE: -------------------------------------------------------------------------------- 1 | The Matrix library is licensed under a permissive 3-clause BSD license. Contributions must be made under the same license. 2 | 3 | Copyright (c) 2015, Matrix Development Team. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of matrix nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | -------------------------------------------------------------------------------- /lib/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 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "math.hpp" 18 | 19 | namespace matrix 20 | { 21 | 22 | template 23 | class Scalar 24 | { 25 | public: 26 | virtual ~Scalar() {}; 27 | 28 | Scalar() : _value() 29 | { 30 | } 31 | 32 | Scalar(const Matrix & other) 33 | { 34 | _value = other(0,0); 35 | } 36 | 37 | Scalar(Type other) 38 | { 39 | _value = other; 40 | } 41 | 42 | operator Type &() 43 | { 44 | return _value; 45 | } 46 | 47 | operator Type const &() const 48 | { 49 | return _value; 50 | } 51 | 52 | operator Matrix() const { 53 | Matrix m; 54 | m(0, 0) = _value; 55 | return m; 56 | } 57 | 58 | operator Vector() const { 59 | Vector m; 60 | m(0) = _value; 61 | return m; 62 | } 63 | 64 | private: 65 | Type _value; 66 | 67 | }; 68 | 69 | typedef Scalar Scalarf; 70 | 71 | } // namespace matrix 72 | 73 | /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ 74 | -------------------------------------------------------------------------------- /lib/matrix/SquareMatrix.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SquareMatrix.hpp 3 | * 4 | * A square matrix 5 | * 6 | * @author James Goppert 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "math.hpp" 18 | 19 | namespace matrix 20 | { 21 | 22 | template 23 | class Matrix; 24 | 25 | template 26 | class Vector; 27 | 28 | template 29 | class SquareMatrix : public Matrix 30 | { 31 | public: 32 | SquareMatrix() : 33 | Matrix() 34 | { 35 | } 36 | 37 | SquareMatrix(const Type *data_) : 38 | Matrix(data_) 39 | { 40 | } 41 | 42 | SquareMatrix(const Matrix &other) : 43 | Matrix(other) 44 | { 45 | } 46 | 47 | // inverse alias 48 | inline SquareMatrix I() const 49 | { 50 | return inv(*this); 51 | } 52 | 53 | Vector diag() const 54 | { 55 | Vector res; 56 | const SquareMatrix &self = *this; 57 | 58 | for (size_t i = 0; i < M; i++) { 59 | res(i) = self(i, i); 60 | } 61 | return res; 62 | } 63 | 64 | Type trace() const 65 | { 66 | Type res = 0; 67 | const SquareMatrix &self = *this; 68 | 69 | for (size_t i = 0; i < M; i++) { 70 | res += self(i, i); 71 | } 72 | return res; 73 | } 74 | 75 | }; 76 | 77 | typedef SquareMatrix SquareMatrix3f; 78 | 79 | template 80 | SquareMatrix eye() { 81 | SquareMatrix m; 82 | m.setIdentity(); 83 | return m; 84 | } 85 | 86 | template 87 | SquareMatrix diag(Vector d) { 88 | SquareMatrix m; 89 | for (size_t i=0; i 96 | SquareMatrix expm(const Matrix & A, size_t order=5) 97 | { 98 | SquareMatrix res; 99 | SquareMatrix A_pow = A; 100 | res.setIdentity(); 101 | size_t i_factorial = 1; 102 | for (size_t i=1; i<=order; i++) { 103 | i_factorial *= i; 104 | res += A_pow / Type(i_factorial); 105 | A_pow *= A_pow; 106 | } 107 | 108 | return res; 109 | } 110 | 111 | /** 112 | * inverse based on LU factorization with partial pivotting 113 | */ 114 | template 115 | SquareMatrix inv(const SquareMatrix & A) 116 | { 117 | SquareMatrix L; 118 | L.setIdentity(); 119 | SquareMatrix U = A; 120 | SquareMatrix P; 121 | P.setIdentity(); 122 | 123 | //printf("A:\n"); A.print(); 124 | 125 | // for all diagonal elements 126 | for (size_t n = 0; n < M; n++) { 127 | 128 | // if diagonal is zero, swap with row below 129 | if (fabsf(U(n, n)) < 1e-8f) { 130 | //printf("trying pivot for row %d\n",n); 131 | for (size_t i = 0; i < M; i++) { 132 | if (i == n) { 133 | continue; 134 | } 135 | 136 | //printf("\ttrying row %d\n",i); 137 | if (fabsf(U(i, n)) > 1e-8f) { 138 | //printf("swapped %d\n",i); 139 | U.swapRows(i, n); 140 | P.swapRows(i, n); 141 | } 142 | } 143 | } 144 | 145 | #ifdef MATRIX_ASSERT 146 | //printf("A:\n"); A.print(); 147 | //printf("U:\n"); U.print(); 148 | //printf("P:\n"); P.print(); 149 | //fflush(stdout); 150 | ASSERT(fabsf(U(n, n)) > 1e-8f); 151 | #endif 152 | 153 | // failsafe, return zero matrix 154 | if (fabsf(U(n, n)) < 1e-8f) { 155 | SquareMatrix zero; 156 | zero.setZero(); 157 | return zero; 158 | } 159 | 160 | // for all rows below diagonal 161 | for (size_t i = (n + 1); i < M; i++) { 162 | L(i, n) = U(i, n) / U(n, n); 163 | 164 | // add i-th row and n-th row 165 | // multiplied by: -a(i,n)/a(n,n) 166 | for (size_t k = n; k < M; k++) { 167 | U(i, k) -= L(i, n) * U(n, k); 168 | } 169 | } 170 | } 171 | 172 | //printf("L:\n"); L.print(); 173 | //printf("U:\n"); U.print(); 174 | 175 | // solve LY=P*I for Y by forward subst 176 | SquareMatrix Y = P; 177 | 178 | // for all columns of Y 179 | for (size_t c = 0; c < M; c++) { 180 | // for all rows of L 181 | for (size_t i = 0; i < M; i++) { 182 | // for all columns of L 183 | for (size_t j = 0; j < i; j++) { 184 | // for all existing y 185 | // subtract the component they 186 | // contribute to the solution 187 | Y(i, c) -= L(i, j) * Y(j, c); 188 | } 189 | 190 | // divide by the factor 191 | // on current 192 | // term to be solved 193 | // Y(i,c) /= L(i,i); 194 | // but L(i,i) = 1.0 195 | } 196 | } 197 | 198 | //printf("Y:\n"); Y.print(); 199 | 200 | // solve Ux=y for x by back subst 201 | SquareMatrix X = Y; 202 | 203 | // for all columns of X 204 | for (size_t c = 0; c < M; c++) { 205 | // for all rows of U 206 | for (size_t k = 0; k < M; k++) { 207 | // have to go in reverse order 208 | size_t i = M - 1 - k; 209 | 210 | // for all columns of U 211 | for (size_t j = i + 1; j < M; j++) { 212 | // for all existing x 213 | // subtract the component they 214 | // contribute to the solution 215 | X(i, c) -= U(i, j) * X(j, c); 216 | } 217 | 218 | // divide by the factor 219 | // on current 220 | // term to be solved 221 | X(i, c) /= U(i, i); 222 | } 223 | } 224 | 225 | //printf("X:\n"); X.print(); 226 | return X; 227 | } 228 | 229 | typedef SquareMatrix Matrix3f; 230 | 231 | } // namespace matrix 232 | 233 | /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ 234 | -------------------------------------------------------------------------------- /lib/matrix/Vector.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Vector.hpp 3 | * 4 | * Vector class. 5 | * 6 | * @author James Goppert 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include "math.hpp" 14 | 15 | namespace matrix 16 | { 17 | 18 | template 19 | class Matrix; 20 | 21 | template 22 | class Vector : public Matrix 23 | { 24 | public: 25 | virtual ~Vector() {}; 26 | 27 | typedef Matrix MatrixM1; 28 | 29 | Vector() : MatrixM1() 30 | { 31 | } 32 | 33 | Vector(const MatrixM1 & other) : 34 | MatrixM1(other) 35 | { 36 | } 37 | 38 | Vector(const Type *data_) : 39 | MatrixM1(data_) 40 | { 41 | } 42 | 43 | inline Type operator()(size_t i) const 44 | { 45 | const MatrixM1 &v = *this; 46 | return v(i, 0); 47 | } 48 | 49 | inline Type &operator()(size_t i) 50 | { 51 | MatrixM1 &v = *this; 52 | return v(i, 0); 53 | } 54 | 55 | Type dot(const MatrixM1 & b) const { 56 | const Vector &a(*this); 57 | Type r = 0; 58 | for (size_t i = 0; i 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 | typedef Matrix Matrix21; 25 | 26 | virtual ~Vector2() {}; 27 | 28 | Vector2() : 29 | Vector() 30 | { 31 | } 32 | 33 | Vector2(const Matrix21 & other) : 34 | Vector(other) 35 | { 36 | } 37 | 38 | Vector2(const Type *data_) : 39 | Vector(data_) 40 | { 41 | } 42 | 43 | Vector2(Type x, Type y) : Vector() 44 | { 45 | Vector2 &v(*this); 46 | v(0) = x; 47 | v(1) = y; 48 | } 49 | 50 | Type cross(const Matrix21 & b) const { 51 | const Vector2 &a(*this); 52 | return a(0)*b(1, 0) - a(1)*b(0, 0); 53 | } 54 | 55 | Type operator%(const Matrix21 & b) const { 56 | return (*this).cross(b); 57 | } 58 | 59 | }; 60 | 61 | typedef Vector2 Vector2f; 62 | 63 | } // namespace matrix 64 | 65 | /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ 66 | -------------------------------------------------------------------------------- /lib/matrix/Vector3.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Vector3.hpp 3 | * 4 | * 3D 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 Matrix; 18 | 19 | template 20 | class Vector; 21 | 22 | template 23 | class Dcm; 24 | 25 | template 26 | class Vector3 : public Vector 27 | { 28 | public: 29 | 30 | typedef Matrix Matrix31; 31 | 32 | virtual ~Vector3() {}; 33 | 34 | Vector3() : 35 | Vector() 36 | { 37 | } 38 | 39 | Vector3(const Matrix31 & other) : 40 | Vector(other) 41 | { 42 | } 43 | 44 | Vector3(const Type *data_) : 45 | Vector(data_) 46 | { 47 | } 48 | 49 | Vector3(Type x, Type y, Type z) : Vector() 50 | { 51 | Vector3 &v(*this); 52 | v(0) = x; 53 | v(1) = y; 54 | v(2) = z; 55 | } 56 | 57 | Vector3 cross(const Matrix31 & b) const { 58 | const Vector3 &a(*this); 59 | Vector3 c; 60 | c(0) = a(1)*b(2,0) - a(2)*b(1,0); 61 | c(1) = -a(0)*b(2,0) + a(2)*b(0,0); 62 | c(2) = a(0)*b(1,0) - a(1)*b(0,0); 63 | return c; 64 | } 65 | 66 | /** 67 | * Override matrix ops so Vector3 type is returned 68 | */ 69 | 70 | inline Vector3 operator+(Vector3 other) const 71 | { 72 | return Matrix31::operator+(other); 73 | } 74 | 75 | inline Vector3 operator-(Vector3 other) const 76 | { 77 | return Matrix31::operator-(other); 78 | } 79 | 80 | inline Vector3 operator-() const 81 | { 82 | return Matrix31::operator-(); 83 | } 84 | 85 | inline Vector3 operator*(Type scalar) const 86 | { 87 | return Matrix31::operator*(scalar); 88 | } 89 | 90 | inline Type operator*(Vector3 b) const 91 | { 92 | return Vector::operator*(b); 93 | } 94 | 95 | inline Vector3 operator%(const Matrix31 & b) const { 96 | return (*this).cross(b); 97 | } 98 | 99 | /** 100 | * Override vector ops so Vector3 type is returned 101 | */ 102 | inline Vector3 unit() const { 103 | return Vector3(Vector::unit()); 104 | } 105 | 106 | inline Vector3 normalized() const { 107 | return unit(); 108 | } 109 | 110 | 111 | Dcm hat() const { // inverse to Dcm.vee() operation 112 | const Vector3 &v(*this); 113 | Dcm A; 114 | A(0,0) = 0; 115 | A(0,1) = -v(2); 116 | A(0,2) = v(1); 117 | A(1,0) = v(2); 118 | A(1,1) = 0; 119 | A(1,2) = -v(0); 120 | A(2,0) = -v(1); 121 | A(2,1) = v(0); 122 | A(2,2) = 0; 123 | return A; 124 | } 125 | 126 | }; 127 | 128 | typedef Vector3 Vector3f; 129 | 130 | } // namespace matrix 131 | 132 | /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ 133 | -------------------------------------------------------------------------------- /lib/matrix/filter.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "math.hpp" 4 | 5 | namespace matrix { 6 | 7 | template 8 | int kalman_correct( 9 | const Matrix & P, 10 | const Matrix & C, 11 | const Matrix & R, 12 | const Matrix &r, 13 | Matrix & dx, 14 | Matrix & dP, 15 | Type & beta 16 | ) 17 | { 18 | SquareMatrix S_I = SquareMatrix(C*P*C.T() + R).I(); 19 | Matrix K = P*C.T()*S_I; 20 | dx = K*r; 21 | beta = Scalar(r.T()*S_I*r); 22 | dP = K*C*P*(-1); 23 | return 0; 24 | } 25 | 26 | } // namespace matrix 27 | -------------------------------------------------------------------------------- /lib/matrix/helper_functions.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "math.hpp" 4 | 5 | // grody hack - this should go once C++11 is supported 6 | // on all platforms. 7 | #if defined (__PX4_NUTTX) || defined (__PX4_QURT) 8 | #include 9 | #else 10 | #include 11 | #endif 12 | 13 | namespace matrix 14 | { 15 | 16 | template 17 | Type wrap_pi(Type x) 18 | { 19 | #if defined (__PX4_NUTTX) || defined (__PX4_QURT) 20 | if (!isfinite(x)) { 21 | #else 22 | if (!std::isfinite(x)) { 23 | #endif 24 | return x; 25 | } 26 | 27 | while (x >= (Type)M_PI) { 28 | x -= (Type)(2.0 * M_PI); 29 | 30 | } 31 | 32 | while (x < (Type)(-M_PI)) { 33 | x += (Type)(2.0 * M_PI); 34 | 35 | } 36 | 37 | return x; 38 | } 39 | 40 | 41 | }; 42 | -------------------------------------------------------------------------------- /lib/matrix/integration.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "math.hpp" 4 | 5 | namespace matrix { 6 | 7 | template 8 | int integrate_rk4( 9 | Vector (*f)(Type, const Matrix &x, const Matrix & u), 10 | const Matrix & y0, 11 | const Matrix & u, 12 | Type t0, 13 | Type tf, 14 | Type h0, 15 | Matrix & y1 16 | ) 17 | { 18 | // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods 19 | Type t1 = t0; 20 | y1 = y0; 21 | Type h = h0; 22 | Vector k1, k2, k3, k4; 23 | if (tf < t0) return -1; // make sure t1 > t0 24 | while (t1 < tf) { 25 | if (t1 + h0 < tf) { 26 | h = h0; 27 | } else { 28 | h = tf - t1; 29 | } 30 | k1 = f(t1, y1, u); 31 | k2 = f(t1 + h/2, y1 + k1*h/2, u); 32 | k3 = f(t1 + h/2, y1 + k2*h/2, u); 33 | k4 = f(t1 + h, y1 + k3*h, u); 34 | y1 += (k1 + k2*2 + k3*2 + k4)*(h/6); 35 | t1 += h; 36 | } 37 | return 0; 38 | } 39 | 40 | } // namespace matrix 41 | 42 | // vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : 43 | -------------------------------------------------------------------------------- /lib/matrix/math.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef __PX4_QURT 4 | #include "dspal_math.h" 5 | #endif 6 | #include "Matrix.hpp" 7 | #include "SquareMatrix.hpp" 8 | #include "Vector.hpp" 9 | #include "Vector2.hpp" 10 | #include "Vector3.hpp" 11 | #include "Euler.hpp" 12 | #include "Dcm.hpp" 13 | #include "Scalar.hpp" 14 | #include "Quaternion.hpp" 15 | #include "AxisAngle.hpp" 16 | -------------------------------------------------------------------------------- /lib/mavlink/checksum.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(MAVLINK_USE_CXX_NAMESPACE) 4 | namespace mavlink { 5 | #elif defined(__cplusplus) 6 | extern "C" { 7 | #endif 8 | 9 | // Visual Studio versions before 2010 don't have stdint.h, so we just error out. 10 | #if (defined _MSC_VER) && (_MSC_VER < 1600) 11 | #error "The C-MAVLink implementation requires Visual Studio 2010 or greater" 12 | #endif 13 | 14 | #include 15 | 16 | /** 17 | * 18 | * CALCULATE THE CHECKSUM 19 | * 20 | */ 21 | 22 | #define X25_INIT_CRC 0xffff 23 | #define X25_VALIDATE_CRC 0xf0b8 24 | 25 | #ifndef HAVE_CRC_ACCUMULATE 26 | /** 27 | * @brief Accumulate the X.25 CRC by adding one char at a time. 28 | * 29 | * The checksum function adds the hash of one char at a time to the 30 | * 16 bit checksum (uint16_t). 31 | * 32 | * @param data new char to hash 33 | * @param crcAccum the already accumulated checksum 34 | **/ 35 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 36 | { 37 | /*Accumulate one byte of data into the CRC*/ 38 | uint8_t tmp; 39 | 40 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 41 | tmp ^= (tmp<<4); 42 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 43 | } 44 | #endif 45 | 46 | 47 | /** 48 | * @brief Initiliaze the buffer for the X.25 CRC 49 | * 50 | * @param crcAccum the 16 bit X.25 CRC 51 | */ 52 | static inline void crc_init(uint16_t* crcAccum) 53 | { 54 | *crcAccum = X25_INIT_CRC; 55 | } 56 | 57 | 58 | /** 59 | * @brief Calculates the X.25 checksum on a byte buffer 60 | * 61 | * @param pBuffer buffer containing the byte array to hash 62 | * @param length length of the byte array 63 | * @return the checksum over the buffer bytes 64 | **/ 65 | static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) 66 | { 67 | uint16_t crcTmp; 68 | crc_init(&crcTmp); 69 | while (length--) { 70 | crc_accumulate(*pBuffer++, &crcTmp); 71 | } 72 | return crcTmp; 73 | } 74 | 75 | 76 | /** 77 | * @brief Accumulate the X.25 CRC by adding an array of bytes 78 | * 79 | * The checksum function adds the hash of one char at a time to the 80 | * 16 bit checksum (uint16_t). 81 | * 82 | * @param data new bytes to hash 83 | * @param crcAccum the already accumulated checksum 84 | **/ 85 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) 86 | { 87 | const uint8_t *p = (const uint8_t *)pBuffer; 88 | while (length--) { 89 | crc_accumulate(*p++, crcAccum); 90 | } 91 | } 92 | 93 | #if defined(MAVLINK_USE_CXX_NAMESPACE) || defined(__cplusplus) 94 | } 95 | #endif 96 | -------------------------------------------------------------------------------- /lib/mavlink/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_IDX 1 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 | -------------------------------------------------------------------------------- /lib/mavlink/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 "Mon Feb 19 2018" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /lib/mavlink/mavlink_get_info.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef MAVLINK_USE_MESSAGE_INFO 4 | #define MAVLINK_HAVE_GET_MESSAGE_INFO 5 | 6 | /* 7 | return the message_info struct for a message 8 | */ 9 | MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_id(uint32_t msgid) 10 | { 11 | static const mavlink_message_info_t mavlink_message_info[] = MAVLINK_MESSAGE_INFO; 12 | /* 13 | use a bisection search to find the right entry. A perfect hash may be better 14 | Note that this assumes the table is sorted with primary key msgid 15 | */ 16 | uint32_t low=0, high=sizeof(mavlink_message_info)/sizeof(mavlink_message_info[0]); 17 | while (low < high) { 18 | uint32_t mid = (low+1+high)/2; 19 | if (msgid < mavlink_message_info[mid].msgid) { 20 | high = mid-1; 21 | continue; 22 | } 23 | if (msgid > mavlink_message_info[mid].msgid) { 24 | low = mid; 25 | continue; 26 | } 27 | low = mid; 28 | break; 29 | } 30 | if (mavlink_message_info[low].msgid == msgid) { 31 | return &mavlink_message_info[low]; 32 | } 33 | return NULL; 34 | } 35 | 36 | /* 37 | return the message_info struct for a message 38 | */ 39 | MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg) 40 | { 41 | return mavlink_get_message_info_by_id(msg->msgid); 42 | } 43 | 44 | /* 45 | return the message_info struct for a message 46 | */ 47 | MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_name(const char *name) 48 | { 49 | static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES; 50 | /* 51 | use a bisection search to find the right entry. A perfect hash may be better 52 | Note that this assumes the table is sorted with primary key name 53 | */ 54 | uint32_t low=0, high=sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); 55 | while (low < high) { 56 | uint32_t mid = (low+1+high)/2; 57 | int cmp = strcmp(mavlink_message_names[mid].name, name); 58 | if (cmp == 0) { 59 | return mavlink_get_message_info_by_id(mavlink_message_names[mid].msgid); 60 | } 61 | if (cmp > 0) { 62 | high = mid-1; 63 | } else { 64 | low = mid; 65 | } 66 | } 67 | return NULL; 68 | } 69 | #endif // MAVLINK_USE_MESSAGE_INFO 70 | 71 | 72 | -------------------------------------------------------------------------------- /lib/mavlink/mavlink_sha256.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/lib/mavlink/mavlink_sha256.h -------------------------------------------------------------------------------- /project/CPPSim.pro: -------------------------------------------------------------------------------- 1 | TEMPLATE = app 2 | CONFIG += c++11 console 3 | CONFIG -= qt 4 | 5 | TARGET = CPPSim 6 | 7 | INCLUDEPATH += ../src 8 | INCLUDEPATH += ../lib 9 | 10 | SOURCES += ../src/*.cpp 11 | SOURCES += ../src/Drawing/*.cpp 12 | SOURCES += ../src/Math/*.cpp 13 | SOURCES += ../src/Simulation/*.cpp 14 | SOURCES += ../src/Utility/*.cpp 15 | SOURCES += ../src/MavlinkNode/*.cpp 16 | 17 | HEADERS += ../src/*.h 18 | HEADERS += ../src/Drawing/*.h 19 | HEADERS += ../src/Math/*.h 20 | HEADERS += ../src/Simulation/*.h 21 | HEADERS += ../src/Utility/*.h 22 | SOURCES += ../src/MavlinkNode/*.h 23 | HEADERS += ../lib/matrix/*.hpp 24 | HEADERS += ../lib/mavlink/*.h 25 | HEADERS += ../lib/mavlink/common/*.h 26 | 27 | LIBS += -lglut -lGLU -lGL -lpthread 28 | 29 | QMAKE_CXXFLAGS += -Wno-unused-parameter -Wno-unused-local-typedefs 30 | -------------------------------------------------------------------------------- /project/FCND-CPPSim.xcodeproj/project.xcworkspace/contents.xcworkspacedata: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /project/Simulator.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 15 4 | VisualStudioVersion = 15.0.27004.2009 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Simulator", "Simulator.vcxproj", "{D8435B4A-B444-4C1B-ABC8-6D1C779B268D}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|x64 = Debug|x64 11 | Debug|x86 = Debug|x86 12 | Release|x64 = Release|x64 13 | Release|x86 = Release|x86 14 | EndGlobalSection 15 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 16 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Debug|x64.ActiveCfg = Debug|x64 17 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Debug|x64.Build.0 = Debug|x64 18 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Debug|x86.ActiveCfg = Debug|Win32 19 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Debug|x86.Build.0 = Debug|Win32 20 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Release|x64.ActiveCfg = Release|x64 21 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Release|x64.Build.0 = Release|x64 22 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Release|x86.ActiveCfg = Release|Win32 23 | {D8435B4A-B444-4C1B-ABC8-6D1C779B268D}.Release|x86.Build.0 = Release|Win32 24 | EndGlobalSection 25 | GlobalSection(SolutionProperties) = preSolution 26 | HideSolutionNode = FALSE 27 | EndGlobalSection 28 | GlobalSection(ExtensibilityGlobals) = postSolution 29 | SolutionGuid = {E7D836DB-9779-49B1-AA5A-615391D3881C} 30 | EndGlobalSection 31 | EndGlobal 32 | -------------------------------------------------------------------------------- /project/Simulator.vcxproj.user: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | -------------------------------------------------------------------------------- /src/BaseController.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "BaseController.h" 3 | #ifndef __PX4_NUTTX 4 | #include "Utility/SimpleConfig.h" 5 | #endif 6 | #include "Utility/StringUtils.h" 7 | using namespace SLR; 8 | 9 | BaseController::BaseController(string config) 10 | { 11 | _config = config; 12 | Init(); 13 | } 14 | 15 | void BaseController::Init() 16 | { 17 | #ifndef __PX4_NUTTX 18 | ParamsHandle config = SimpleConfig::GetInstance(); 19 | 20 | optFlowX = 0; 21 | optFlowY = 0; 22 | mass = config->Get(_config+".Mass", 1.f); 23 | L = config->Get(_config+".L", 0.1f); 24 | Ixx = config->Get(_config+".Ixx", 0.001f); 25 | Iyy = config->Get(_config + ".Iyy", 0.001f); 26 | Izz = config->Get(_config + ".Izz", 0.002f); 27 | kappa = config->Get(_config + ".kappa", 0.01f); 28 | 29 | trajectory.Clear(); 30 | string trajFile = config->Get(_config + ".Trajectory",""); 31 | if (!trajectory.ReadFile(string("../config/")+trajFile)) 32 | { 33 | TrajectoryPoint tmp; 34 | tmp.position = config->Get(_config + ".Trajectory", V3F()); 35 | trajectory.AddTrajectoryPoint(tmp); 36 | } 37 | #else 38 | 39 | 40 | #endif 41 | } 42 | 43 | void BaseController::Reset() 44 | { 45 | // Reinitialise the physical parameters 46 | Init(); 47 | } 48 | 49 | void BaseController::OnSensor_IMU(V3F accel, V3F gyros) 50 | { 51 | // todo 52 | } 53 | 54 | void BaseController::OnSensor_OpticalFlow(float x, float y) 55 | { 56 | optFlowX = x; 57 | optFlowY = y; 58 | } 59 | 60 | void BaseController::OnSensor_Range(float z) 61 | { 62 | range = z; 63 | } 64 | 65 | // Allows the simulator to provide perfect state data to the controller 66 | void BaseController::OverrideEstimates(V3F pos, V3F vel, Quaternion attitude, V3F omega) 67 | { 68 | estAtt = attitude; 69 | estOmega = omega; 70 | estPos = pos; 71 | estVel = vel; 72 | } 73 | 74 | TrajectoryPoint BaseController::GetNextTrajectoryPoint(float mission_time) 75 | { 76 | TrajectoryPoint pt = trajectory.NextTrajectoryPoint(mission_time + _trajectoryTimeOffset); 77 | pt.position += _trajectoryOffset; 78 | return pt; 79 | } 80 | 81 | // Access functions for graphing variables 82 | bool BaseController::GetData(const string& name, float& ret) const 83 | { 84 | if (name.find_first_of(".") == string::npos) return false; 85 | string leftPart = LeftOf(name, '.'); 86 | string rightPart = RightOf(name, '.'); 87 | 88 | if (ToUpper(leftPart) == ToUpper(_config)) 89 | { 90 | #define GETTER_HELPER(A,B) if (SLR::ToUpper(rightPart) == SLR::ToUpper(A)){ ret=(B); return true; } 91 | // UDACITY CONVENTION 92 | GETTER_HELPER("Ref.X", curTrajPoint.position.x); 93 | GETTER_HELPER("Ref.Y", curTrajPoint.position.y); 94 | GETTER_HELPER("Ref.Z", curTrajPoint.position.z); 95 | #undef GETTER_HELPER 96 | } 97 | return false; 98 | } 99 | 100 | vector BaseController::GetFields() const 101 | { 102 | vector ret; 103 | ret.push_back(_config + ".Ref.X"); 104 | ret.push_back(_config + ".Ref.Y"); 105 | ret.push_back(_config + ".Ref.Z"); 106 | return ret; 107 | } 108 | -------------------------------------------------------------------------------- /src/BaseController.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "DataSource.h" 5 | #include "VehicleDatatypes.h" 6 | #include "Trajectory.h" 7 | using namespace SLR; 8 | using namespace std; 9 | 10 | #ifndef __PX4_NUTTX 11 | class BaseController; 12 | typedef shared_ptr ControllerHandle; 13 | #endif 14 | 15 | class BaseController : public DataSource 16 | { 17 | public: 18 | BaseController(string config); 19 | virtual ~BaseController() {}; 20 | 21 | virtual void RunEstimation() {}; 22 | virtual VehicleCommand RunControl(float dt, float sim_time) { return VehicleCommand(); }; 23 | 24 | virtual void Init(); 25 | virtual void Reset(); 26 | 27 | void OnSensor_IMU(V3F accel, V3F gyros); 28 | void OnSensor_OpticalFlow(float x, float y); 29 | void OnSensor_Range(float z); 30 | void OnSensor_GPS(V3D LLA); 31 | void OnSensor_Magnetometer(V3F); 32 | 33 | TrajectoryPoint GetNextTrajectoryPoint(float mission_time); 34 | 35 | // Allows the simulator to provide perfect state data to the controller 36 | void OverrideEstimates(V3F pos, V3F vel, Quaternion attitude, V3F omega); 37 | 38 | // Access functions for graphing variables 39 | virtual bool GetData(const string& name, float& ret) const; 40 | virtual vector GetFields() const; 41 | 42 | void SetTrajectoryOffset(V3F trajOffset) { _trajectoryOffset = trajOffset; } 43 | void SetTrajTimeOffset(float timeOffset) { _trajectoryTimeOffset = timeOffset; } 44 | 45 | // system parameters params 46 | float mass; // mass 47 | float L; // length of arm from centre of quadrocopter to motor 48 | float Ixx, Iyy, Izz; // mass moment of inertia / second moment of inertia 49 | float kappa; // torque (Nm) produced by motor per N of thrust produced 50 | 51 | // controller input (reference state) 52 | int mode; 53 | VehicleCommand cmd; 54 | 55 | // Estimator state 56 | Quaternion estAtt; 57 | V3F estVel; 58 | V3F estPos; 59 | V3F estOmega; 60 | 61 | // measurements 62 | float optFlowX, optFlowY; 63 | V3F gyros; 64 | V3F accels; 65 | float range; 66 | 67 | Trajectory trajectory; 68 | TrajectoryPoint curTrajPoint; 69 | string _config; 70 | 71 | V3F _trajectoryOffset; 72 | float _trajectoryTimeOffset; 73 | }; 74 | 75 | -------------------------------------------------------------------------------- /src/Common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Common.h 4 | 5 | // The purpose of this file is to improve the overall sanity of the other code 6 | // by making compilers behave 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #ifndef __PX4_NUTTX 15 | #include 16 | #endif 17 | 18 | #define SLR_ERROR0(A) SLR::PrintError(__FUNCTION__,__LINE__,A) 19 | #define SLR_ERROR1(A,B) SLR::PrintError(__FUNCTION__,__LINE__,A,B) 20 | #define SLR_ERROR2(A,B,C) SLR::PrintError(__FUNCTION__,__LINE__,A,B,C) 21 | #define SLR_ERROR3(A,B,C,D) SLR::PrintError(__FUNCTION__,__LINE__,A,B,C,D) 22 | #define SLR_ERROR4(A,B,C,D,E) SLR::PrintError(__FUNCTION__,__LINE__,A,B,C,D,E) 23 | #define SLR_ERROR5(A,B,C,D,E,F) SLR::PrintError(__FUNCTION__,__LINE__,A,B,C,D,E,F) 24 | #define SLR_ERROR6(A,B,C,D,E,F,G) SLR::PrintError(__FUNCTION__,__LINE__,A,B,C,D,E,F,G) 25 | 26 | #define SLR_ERROR0S(S,A) SLR::PrintError(S,__LINE__,A) 27 | #define SLR_ERROR1S(S,A,B) SLR::PrintError(S,__LINE__,A,B) 28 | #define SLR_ERROR2S(S,A,B,C) SLR::PrintError(S,__LINE__,A,B,C) 29 | #define SLR_ERROR3S(S,A,B,C,D) SLR::PrintError(S,__LINE__,A,B,C,D) 30 | #define SLR_ERROR4S(S,A,B,C,D,E) SLR::PrintError(S,__LINE__,A,B,C,D,E) 31 | #define SLR_ERROR5S(S,A,B,C,D,E,F) SLR::PrintError(S,__LINE__,A,B,C,D,E,F) 32 | #define SLR_ERROR6S(S,A,B,C,D,E,F,G) SLR::PrintError(S,__LINE__,A,B,C,D,E,F,G) 33 | 34 | #define SLR_WARNING0(A) SLR::PrintWarning(__FUNCTION__,__LINE__,A) 35 | #define SLR_WARNING1(A,B) SLR::PrintWarning(__FUNCTION__,__LINE__,A,B) 36 | #define SLR_WARNING2(A,B,C) SLR::PrintWarning(__FUNCTION__,__LINE__,A,B,C) 37 | #define SLR_WARNING3(A,B,C,D) SLR::PrintWarning(__FUNCTION__,__LINE__,A,B,C,D) 38 | #define SLR_WARNING4(A,B,C,D,E) SLR::PrintWarning(__FUNCTION__,__LINE__,A,B,C,D,E) 39 | #define SLR_WARNING5(A,B,C,D,E,F) SLR::PrintWarning(__FUNCTION__,__LINE__,A,B,C,D,E,F) 40 | 41 | #ifndef _WIN32 42 | // not technically 100% correct, but lets us move on with our lives 43 | #define sprintf_s snprintf 44 | #define vsprintf_s vsnprintf 45 | #else 46 | #ifndef _SCL_SECURE_NO_WARNINGS 47 | #define _SCL_SECURE_NO_WARNINGS 48 | #endif 49 | #pragma warning(disable: 4996) //strcpy unsafe 50 | #endif 51 | 52 | namespace SLR { 53 | inline void TimestampString(char* buf, int cnt, const char* format="%y.%m.%d.%H.%M.%S") 54 | { 55 | #ifndef __PX4_NUTTX 56 | // timestamp 57 | struct tm newTime; 58 | time_t szClock; 59 | time(&szClock); 60 | #ifdef _WIN32 61 | _timeb tstruct; 62 | _ftime(&tstruct); 63 | localtime_s(&newTime, &szClock); 64 | #else 65 | // linux and apple 66 | timeb tstruct; 67 | ftime(&tstruct); 68 | localtime_r(&szClock,&newTime); 69 | #endif 70 | strftime(buf, cnt, format, &newTime); 71 | sprintf_s(buf, cnt, "%s.%03d", buf, tstruct.millitm); 72 | #else 73 | if(cnt>0) 74 | { 75 | buf[0]=0; 76 | } 77 | 78 | #endif 79 | } 80 | 81 | inline void PrintError(const char* funcName, const int lineNum, const char* format, ...) 82 | { 83 | char tsBuf[100]; tsBuf[99]=0; 84 | char buf[512]; buf[511] = 0; 85 | char buf2[2048]; buf2[2047] = 0; 86 | char buf3[2560]; buf3[2559] = 0; 87 | 88 | TimestampString(tsBuf,99,"%d-%H.%M.%S"); 89 | 90 | // error location 91 | sprintf_s(buf, 511, "%s ERR %s(%d) : ", tsBuf, funcName, lineNum); 92 | 93 | va_list args; 94 | va_start(args, format); 95 | vsprintf_s(buf2, 2047, format, args); 96 | va_end(args); 97 | 98 | // push everything out to stderr 99 | sprintf_s(buf3, 2047, "%s%s\n", buf, buf2); 100 | fprintf(stderr,"%s",buf3); 101 | fflush(stderr); 102 | } 103 | 104 | inline void PrintWarning(const char* funcName, const int lineNum, const char* format, ...) 105 | { 106 | char tsBuf[100]; tsBuf[99]=0; 107 | char buf[512]; buf[511] = 0; 108 | char buf2[2048]; buf2[2047] = 0; 109 | char buf3[2560]; buf3[2559] = 0; 110 | 111 | TimestampString(tsBuf,99,"%d-%H.%M.%S"); 112 | 113 | // error location 114 | sprintf_s(buf, 511, "%s WRN %s(%d) : ", tsBuf, funcName, lineNum); 115 | 116 | va_list args; 117 | va_start(args, format); 118 | vsprintf_s(buf2, 2047, format, args); 119 | va_end(args); 120 | 121 | // push everything out to stderr 122 | sprintf_s(buf3, 2047, "%s%s\n", buf, buf2); 123 | fprintf(stderr,"%s",buf3); 124 | fflush(stderr); 125 | } 126 | 127 | 128 | } //namespace SLR 129 | 130 | #ifdef _WIN32 131 | 132 | 133 | namespace SLR { 134 | inline void PrintError(const char* funcName, const int lineNum, const char* format, ...); 135 | inline void PrintWarning(const char* funcName, const int lineNum, const char* format, ...); 136 | } 137 | 138 | 139 | 140 | // boost::numeric::ublas shortcuts 141 | #define BNU boost::numeric::ublas 142 | #define BNUV BNU::bounded_vector 143 | #define BNUM BNU::c_matrix 144 | 145 | #if _MSC_VER>=1600 // visual studio 2010 146 | #include 147 | #include 148 | using std::shared_ptr; 149 | using std::weak_ptr; 150 | using std::placeholders::_1; 151 | #define TR1 std 152 | #else 153 | #include 154 | #include 155 | #include 156 | #include 157 | #include "Common/Math/Attitude.h" 158 | #include "Common/Math/Transform3D.h" 159 | using boost::shared_ptr; 160 | using boost::weak_ptr; 161 | using boost::static_pointer_cast; 162 | #define TR1 boost 163 | #endif 164 | 165 | #include 166 | #include 167 | 168 | 169 | #else 170 | // not _WIN32 171 | #include 172 | #include 173 | #include 174 | inline void Sleep(int msec) 175 | { 176 | usleep(msec*1000); 177 | } 178 | 179 | #include 180 | #include 181 | using std::shared_ptr; 182 | using std::weak_ptr; 183 | using std::placeholders::_1; 184 | 185 | #ifndef __PX4_NUTTX 186 | #include 187 | inline bool _isnan(const double& v){return std::isnan(v);} 188 | inline bool _isnan(const float& v){return std::isnan(v);} 189 | #endif 190 | 191 | #endif // #ifdef _WIN32 192 | 193 | #include "Math/Constants.h" 194 | #include "Math/V3F.h" 195 | #include "Math/V3D.h" 196 | -------------------------------------------------------------------------------- /src/ControllerFactory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "QuadControl.h" 4 | 5 | inline ControllerHandle CreateController(string controllerType, string config) 6 | { 7 | ControllerHandle ret; 8 | 9 | if (controllerType == "QuadControl") 10 | { 11 | ret.reset(new QuadControl(config)); 12 | } 13 | 14 | return ret; 15 | } -------------------------------------------------------------------------------- /src/DataSource.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | using std::string; 6 | using std::vector; 7 | 8 | class DataSource 9 | { 10 | public: 11 | virtual bool GetData(const string& name, float& ret) const 12 | { 13 | return false; 14 | } 15 | virtual vector GetFields() const 16 | { 17 | return vector(); 18 | } 19 | }; -------------------------------------------------------------------------------- /src/Drawing/AbsThreshold.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // "Absolute threshold" trigger/detector 4 | // Detects if the absolute value of a signal goes below a certain threshold for at least a given time 5 | // and plots the detection point (different color if quiet time met) 6 | 7 | #include "BaseAnalyzer.h" 8 | 9 | class AbsThreshold : public BaseAnalyzer 10 | { 11 | public: 12 | AbsThreshold(string var, float thresh, float quietTime) 13 | { 14 | _var = var; 15 | _thresh = thresh; 16 | _quietTime = quietTime; 17 | Reset(); 18 | } 19 | 20 | void Reset() 21 | { 22 | _lastTimeAboveThresh = numeric_limits::infinity(); 23 | _triggered = false; 24 | } 25 | 26 | void Update(double time, std::vector >& sources) 27 | { 28 | for (unsigned int j = 0; j < sources.size(); j++) 29 | { 30 | float tmp; 31 | if (sources[j]->GetData(_var, tmp)) 32 | { 33 | OnNewData((float)time, tmp); 34 | break; 35 | } 36 | } 37 | } 38 | 39 | void OnNewData(float time, float meas) 40 | { 41 | if (_triggered) 42 | { 43 | return; 44 | } 45 | 46 | if (_lastTimeAboveThresh == numeric_limits::infinity()) 47 | { 48 | _lastTimeAboveThresh = time; 49 | } 50 | 51 | if (fabs(meas) > _thresh) 52 | { 53 | _lastTimeAboveThresh = time; 54 | } 55 | 56 | if ((time - _lastTimeAboveThresh) > _quietTime) 57 | { 58 | _triggered = true; 59 | } 60 | } 61 | 62 | // Draws horizontal threshold bands 63 | // and detection marker/time 64 | void Draw(float minX, float maxX, float minY, float maxY) 65 | { 66 | glColor3f(.1f, .2f, .1f); 67 | 68 | if (_thresh > minY && _thresh < maxY) 69 | { 70 | glBegin(GL_LINES); 71 | glVertex2f(minX, _thresh); 72 | glVertex2f(maxX, _thresh); 73 | glEnd(); 74 | } 75 | 76 | if ((-_thresh) > minY && (-_thresh) < maxY) 77 | { 78 | glBegin(GL_LINES); 79 | glVertex2f(minX, -_thresh); 80 | glVertex2f(maxX, -_thresh); 81 | glEnd(); 82 | } 83 | 84 | if (_lastTimeAboveThresh == numeric_limits::infinity() || _lastTimeAboveThreshmaxX) 85 | { 86 | return; 87 | } 88 | 89 | if (_triggered) 90 | { 91 | glColor3f(0, 1, 0); 92 | } 93 | else 94 | { 95 | glColor3f(.2f, .4f, .2f); 96 | } 97 | glBegin(GL_LINES); 98 | glVertex2f(_lastTimeAboveThresh, minY); 99 | glVertex2f(_lastTimeAboveThresh, maxY); 100 | glEnd(); 101 | 102 | char buf[100]; 103 | sprintf_s(buf, 100, "t_set = %.3lf", _lastTimeAboveThresh); 104 | DrawStrokeText(buf, _lastTimeAboveThresh + (maxX - minX)*.05f , minY + (maxY - minY) / 2.f, 0, 1.2f, (maxX - minX) / 2.f, (maxY - minY) / 2.f *2.f); 105 | } 106 | 107 | bool _triggered; 108 | string _var; 109 | float _lastTimeAboveThresh; 110 | float _thresh, _quietTime; 111 | }; -------------------------------------------------------------------------------- /src/Drawing/BaseAnalyzer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | class BaseAnalyzer 4 | { 5 | public: 6 | 7 | virtual void Reset() {}; 8 | virtual void Update(double time, std::vector >& sources) {}; 9 | virtual void Draw(float minX, float maxX, float minY, float maxY) {} 10 | }; -------------------------------------------------------------------------------- /src/Drawing/ColorUtils.cpp: -------------------------------------------------------------------------------- 1 | #include "../Common.h" 2 | #include "ColorUtils.h" 3 | 4 | // h from 0-259 5 | // s from 0 to 1 6 | // v from 0 to 1 7 | // http://www.pymolwiki.org/index.php/Color_Objects 8 | V3F HSVtoRGB( float h, float s, float v ) 9 | { 10 | float r,g,b; 11 | int i; 12 | float f, p, q, t; 13 | if( s == 0 ) { 14 | // achromatic (grey) 15 | return V3F(v,v,v); 16 | } 17 | h /= 60; // sector 0 to 5 18 | i = (int)floor( h ); 19 | f = h - i; // factorial part of h 20 | p = v * ( 1 - s ); 21 | q = v * ( 1 - s * f ); 22 | t = v * ( 1 - s * ( 1 - f ) ); 23 | switch( i ) { 24 | case 0: 25 | r = v; 26 | g = t; 27 | b = p; 28 | break; 29 | case 1: 30 | r = q; 31 | g = v; 32 | b = p; 33 | break; 34 | case 2: 35 | r = p; 36 | g = v; 37 | b = t; 38 | break; 39 | case 3: 40 | r = p; 41 | g = q; 42 | b = v; 43 | break; 44 | case 4: 45 | r = t; 46 | g = p; 47 | b = v; 48 | break; 49 | default: // case 5: 50 | r = v; 51 | g = p; 52 | b = q; 53 | break; 54 | } 55 | return V3F(r,g,b); 56 | } 57 | 58 | // returns BGR!! 59 | V3F FalseColorBGR(float v, float intensityMult) 60 | { 61 | if(v<=.5f) 62 | { 63 | float b = (1.0f-v*2.0f); 64 | float g = 1.0f-b; 65 | float norm = (b+g)/(2.0f); 66 | b = b*intensityMult/norm; 67 | g = g*intensityMult/norm; 68 | return V3F(CONSTRAIN(b,0,1.0f),CONSTRAIN(g,0,1.0f),0); 69 | } 70 | else 71 | { 72 | float r = (2.0f*(v-.5f)); 73 | float g = 1.0f-r; 74 | float norm = (r+g)/(2.0f); 75 | r = r*intensityMult/norm; 76 | g = g*intensityMult/norm; 77 | return V3F(0,CONSTRAIN(g,0,1.0f),CONSTRAIN(r,0,1.0f)); 78 | } 79 | } 80 | 81 | V3F FalseColorRGB(float v, float intensityMult) 82 | { 83 | V3F bgr = FalseColorBGR(v, intensityMult); 84 | return V3F(bgr[2], bgr[1], bgr[0]); 85 | } 86 | 87 | V3F FalseColor_RedGreen(float v, float intensityMult) 88 | { 89 | V3F ret; // rgb 90 | ret[0] = CONSTRAIN(1.0f-v,0,1); 91 | ret[1] = CONSTRAIN(v,0,1); 92 | return ret.norm()*intensityMult; 93 | } 94 | 95 | void SetConsoleColor(unsigned char attr) 96 | { 97 | #ifdef _WIN32 98 | SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE), attr); 99 | #endif 100 | } 101 | 102 | void ResetConsoleColor() 103 | { 104 | #ifdef _WIN32 105 | SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE), FOREGROUND_BLUE|FOREGROUND_GREEN|FOREGROUND_RED); 106 | #endif 107 | } 108 | -------------------------------------------------------------------------------- /src/Drawing/ColorUtils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../Math/V3D.h" 4 | 5 | // h from 0-259 6 | // s from 0 to 1 7 | // v from 0 to 1 8 | V3F HSVtoRGB( float h, float s, float v ); 9 | 10 | // False color (color gradient) functions 11 | V3F FalseColorBGR(float v, float intensityMult=1.0); 12 | V3F FalseColorRGB(float v, float intensityMult = 1.0); 13 | V3F FalseColor_RedGreen(float v, float intensityMult=1.0); 14 | 15 | #ifdef _WIN32 16 | // Sets the stdout text color. 17 | // Example of use: SetConsoleColor(FOREGROUND_BLUE|BACKGROUND_INTENSITY) 18 | namespace SLR{ 19 | const uint16_t CONSOLE_FG_RED = FOREGROUND_RED | FOREGROUND_INTENSITY; 20 | const uint16_t CONSOLE_FG_GREEN = FOREGROUND_GREEN | FOREGROUND_INTENSITY; 21 | const uint16_t CONSOLE_FG_YELLOW = FOREGROUND_GREEN | FOREGROUND_RED | FOREGROUND_INTENSITY; 22 | const uint16_t CONSOLE_FG_DARK_YELLOW = FOREGROUND_GREEN | FOREGROUND_RED; 23 | const uint16_t CONSOLE_FG_DARK_GREEN = FOREGROUND_GREEN; 24 | const uint16_t CONSOLE_FG_BLUE = FOREGROUND_BLUE | FOREGROUND_INTENSITY; 25 | const uint16_t CONSOLE_FG_DARK_BLUE = FOREGROUND_BLUE; 26 | const uint16_t CONSOLE_FG_DARK_TEAL = FOREGROUND_BLUE | FOREGROUND_GREEN; 27 | const uint16_t CONSOLE_FG_TEAL = FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_INTENSITY; 28 | const uint16_t CONSOLE_FG_GRAY = FOREGROUND_RED | FOREGROUND_BLUE | FOREGROUND_GREEN; 29 | const uint16_t CONSOLE_FG_WHITE = FOREGROUND_RED | FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_INTENSITY; 30 | } 31 | #endif 32 | void SetConsoleColor(unsigned char attr); 33 | void ResetConsoleColor(); 34 | -------------------------------------------------------------------------------- /src/Drawing/DrawingFuncs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef __APPLE__ 4 | #include 5 | #include 6 | #include 7 | #include 8 | #else 9 | #include 10 | #endif 11 | 12 | #include "Math/Quaternion.h" 13 | 14 | 15 | void GLCube(V3F center, V3F dims, int cnt=3); 16 | void GLRectangle(V3F center, V3F normal, V3F up, float width, float height, int numX, int numY); 17 | void DrawX3D(V3D markingColor, V3D bodyColor=V3D(.2,.2,.2), double alpha=1, bool solidPart=true, bool transPart=true, GLUquadricObj *glQuadric=NULL); 18 | void DrawQuarterX3D(bool front, V3D markingColor, V3D bodyColor, double alpha, GLUquadricObj *glQuadric, float armLength); 19 | void GLCross(const V3F& center, const V3F& dims, bool gl_begin_line=true); 20 | void DrawQuarterX3D_TransparentPart(double alpha, GLUquadricObj *glQuadric, float armLength); 21 | void DrawStrokeText(const char* str, float x, float y, float z, float lineWidth, float scaleX=1, float scaleY=1); 22 | 23 | using SLR::Quaternion; 24 | 25 | namespace SLR { 26 | 27 | // Utility class for helping with OpenGL drawing 28 | 29 | class OpenGLDrawer 30 | { 31 | public: 32 | OpenGLDrawer(); 33 | ~OpenGLDrawer(); 34 | 35 | void PushLighting(); 36 | void PopLighting(); 37 | void SetLighting(bool enable); 38 | 39 | void DrawQuadrotor2(V3F pos, Quaternion att, V3F color, V3F centerOffset, float centerScale, float armLength); 40 | 41 | void DrawArrow(double len, double r1, double r2, double arrowLen); 42 | void DrawArrow(V3D from, V3D to, V3D color); 43 | 44 | GLUquadricObj* Quadric() { return _glQuadric; }; 45 | 46 | V3D cameraPos; 47 | 48 | protected: 49 | GLUquadricObj* _glQuadric; 50 | }; 51 | 52 | } // namespace SLR 53 | -------------------------------------------------------------------------------- /src/Drawing/GLUTMenu.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "GLUTMenu.h" 3 | #include 4 | #include "DrawingFuncs.h" 5 | 6 | using std::vector; 7 | 8 | 9 | GLUTMenu* _g_GLUTMenu = NULL; 10 | 11 | void _g_OnMenu(int code) 12 | { 13 | if (_g_GLUTMenu != NULL) 14 | { 15 | _g_GLUTMenu->OnGLUTMenu(code); 16 | } 17 | } 18 | 19 | GLUTMenu::GLUTMenu() 20 | { 21 | _g_GLUTMenu = this; 22 | } 23 | 24 | GLUTMenu::~GLUTMenu() 25 | { 26 | _g_GLUTMenu = NULL; 27 | } 28 | 29 | void GLUTMenu::AddMenuEntry(const string& entry, const string& fullCommand, GLUTMenu::MenuEntry& top) 30 | { 31 | auto tmp = entry.find_first_of("."); 32 | if (tmp == string::npos) 33 | { 34 | top.children[entry] = MenuEntry(); 35 | top.children[entry].glutMenuEntryID = _menuItemCounter; 36 | _menuMap[_menuItemCounter++] = fullCommand; 37 | } 38 | else 39 | { 40 | string left = entry.substr(0, tmp); 41 | string right = entry.substr(tmp + 1); 42 | if (top.children.find(left) == top.children.end()) 43 | { 44 | top.children[left] = MenuEntry(); 45 | } 46 | AddMenuEntry(right, fullCommand, top.children[left]); 47 | } 48 | } 49 | 50 | GLUTMenu::MenuEntry GLUTMenu::StringListToMenuTree(const vector& strings) 51 | { 52 | MenuEntry top; 53 | for (unsigned int i = 0; i < strings.size(); i++) 54 | { 55 | AddMenuEntry(strings[i],strings[i],top); 56 | } 57 | return top; 58 | } 59 | 60 | void GLUTMenu::CreateGLUTMenus(MenuEntry& top) 61 | { 62 | if (top.children.empty()) 63 | return; 64 | 65 | for (auto i = top.children.begin(); i != top.children.end(); i++) 66 | { 67 | // create the submenus 68 | CreateGLUTMenus(i->second); 69 | } 70 | 71 | top.glutMenuHandle = glutCreateMenu(_g_OnMenu); 72 | for (auto i = top.children.begin(); i != top.children.end(); i++) 73 | { 74 | // add items or submenus 75 | if (i->second.glutMenuHandle == -1) 76 | { 77 | // non-submenu item 78 | glutAddMenuEntry(i->first.c_str(), i->second.glutMenuEntryID); 79 | } 80 | else 81 | { 82 | glutAddSubMenu(i->first.c_str(), i->second.glutMenuHandle); 83 | } 84 | } 85 | } 86 | 87 | void GLUTMenu::RemoveGLUTMenus(MenuEntry& top) 88 | { 89 | if (top.children.empty()) 90 | return; 91 | 92 | for (auto i = top.children.begin(); i != top.children.end(); i++) 93 | { 94 | // create the submenus 95 | RemoveGLUTMenus(i->second); 96 | } 97 | 98 | glutDestroyMenu(top.glutMenuHandle); 99 | 100 | } 101 | 102 | void GLUTMenu::CreateMenu(const vector& strings) 103 | { 104 | _menuItemCounter = 0; 105 | if (menuTree.glutMenuHandle >= 0) 106 | { 107 | RemoveGLUTMenus(menuTree); 108 | } 109 | menuTree = StringListToMenuTree(strings); 110 | 111 | CreateGLUTMenus(menuTree); 112 | 113 | glutAttachMenu(GLUT_RIGHT_BUTTON); 114 | } 115 | 116 | void GLUTMenu::OnGLUTMenu(int id) 117 | { 118 | if (OnMenu && _menuMap.find(id)!= _menuMap.end()) 119 | { 120 | OnMenu(_menuMap[id]); 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /src/Drawing/GLUTMenu.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Utility/FastDelegate.h" 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | using namespace fastdelegate; 10 | 11 | class GLUTMenu 12 | { 13 | public: 14 | GLUTMenu(); 15 | ~GLUTMenu(); 16 | 17 | void CreateMenu(const vector& strings); 18 | void OnGLUTMenu(int id); 19 | 20 | FastDelegate1 OnMenu; 21 | int _menuID; 22 | int _menuItemCounter; 23 | 24 | std::map _menuMap; 25 | 26 | bool IsActive(); 27 | 28 | protected: 29 | struct MenuEntry 30 | { 31 | MenuEntry() : glutMenuHandle(-1) {} 32 | int glutMenuHandle; // if negative, not a menu itself. 33 | int glutMenuEntryID; 34 | map children; 35 | }; 36 | 37 | GLUTMenu::MenuEntry StringListToMenuTree(const vector& strings); 38 | void AddMenuEntry(const string& entry, const string& fullCommand, GLUTMenu::MenuEntry& top); 39 | void CreateGLUTMenus(MenuEntry& top); 40 | void RemoveGLUTMenus(MenuEntry& top); 41 | 42 | MenuEntry menuTree; 43 | }; 44 | -------------------------------------------------------------------------------- /src/Drawing/Graph.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | using namespace std; 6 | #include "../Utility/FixedQueue.h" 7 | 8 | class QuadDynamics; 9 | class DataSource; 10 | class BaseAnalyzer; 11 | 12 | class Graph 13 | { 14 | public: 15 | Graph(const char* name); 16 | void Reset(); 17 | void Clear(); 18 | void Update(double time, std::vector >& sources); 19 | 20 | void Draw(); 21 | void AddItem(string path); 22 | void AddSeries(string path, bool autoColor = true, V3F color = V3F()); 23 | void AddAbsThreshold(string path); 24 | void AddWindowThreshold(string path); 25 | bool IsSeriesPlotted(string path); 26 | void RemoveAllElements(); 27 | 28 | 29 | struct Series 30 | { 31 | Series(); 32 | V3F _color; 33 | string _yName; 34 | string _objName, _fieldName; 35 | FixedQueue x; 36 | FixedQueue y; 37 | void Clear() 38 | { 39 | x.reset(); 40 | y.reset(); 41 | } 42 | }; 43 | 44 | vector > _analyzers; 45 | 46 | void DrawSeries(Series& s); 47 | 48 | vector _series; 49 | string _name; 50 | }; -------------------------------------------------------------------------------- /src/Drawing/GraphManager.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "GraphManager.h" 3 | #include "../Utility/SimpleConfig.h" 4 | #include "../Utility/StringUtils.h" 5 | #include "DrawingFuncs.h" 6 | #include "DataSource.h" 7 | 8 | using namespace SLR; 9 | 10 | GraphManager* _g_GraphManager=NULL; 11 | 12 | void _g_OnGrapherDisplay() 13 | { 14 | if (_g_GraphManager != NULL) 15 | { 16 | _g_GraphManager->Paint(); 17 | } 18 | } 19 | 20 | void _g_OnGrapherReshape(int w, int h) 21 | { 22 | if (_g_GraphManager != NULL) 23 | { 24 | _g_GraphManager->Paint(); 25 | } 26 | } 27 | 28 | GraphManager::GraphManager(bool own_window) 29 | { 30 | ParamsHandle config = SimpleConfig::GetInstance(); 31 | 32 | _ownWindow = own_window; 33 | 34 | if (_ownWindow) 35 | { 36 | _g_GraphManager = this; 37 | 38 | glutInitWindowSize(500, 300); 39 | glutInitWindowPosition(0, 0); 40 | _glutWindowNum = glutCreateWindow("Grapher"); 41 | glutSetWindow(_glutWindowNum); 42 | 43 | glutReshapeFunc(&_g_OnGrapherReshape); 44 | glutDisplayFunc(&_g_OnGrapherDisplay); 45 | 46 | InitPaint(); 47 | } 48 | 49 | graph1.reset(new Graph("Graph1")); 50 | graph2.reset(new Graph("Graph1")); 51 | } 52 | 53 | GraphManager::~GraphManager() 54 | { 55 | graph1.reset(); 56 | graph2.reset(); 57 | Sleep(100); 58 | _g_GraphManager = NULL; 59 | } 60 | 61 | void GraphManager::Reset() 62 | { 63 | graph1->Reset(); 64 | graph2->Reset(); 65 | } 66 | 67 | void GraphManager::Clear() 68 | { 69 | graph1->Clear(); 70 | graph2->Clear(); 71 | } 72 | 73 | void GraphManager::UpdateData(double time) 74 | { 75 | if (graph1) 76 | { 77 | graph1->Update(time, _sources); 78 | } 79 | if (graph2) 80 | { 81 | graph2->Update(time, _sources); 82 | } 83 | } 84 | 85 | void GraphManager::DrawUpdate() 86 | { 87 | 88 | if (_ownWindow) 89 | { 90 | glutSetWindow(_glutWindowNum); 91 | glutPostRedisplay(); 92 | } 93 | } 94 | 95 | void GraphManager::InitPaint() 96 | { 97 | glClearColor(0.0, 0.0, 0.0, 0.0); // When screen cleared, use black. 98 | glShadeModel(GL_SMOOTH); // How the object color will be rendered smooth or flat 99 | } 100 | 101 | void GraphManager::Paint() 102 | { 103 | if (_ownWindow) 104 | { 105 | glutSetWindow(_glutWindowNum); 106 | 107 | int width = glutGet(GLUT_WINDOW_WIDTH); 108 | int height = glutGet(GLUT_WINDOW_HEIGHT); 109 | 110 | glViewport(0, 0, width, height); 111 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //Clear the screen 112 | } 113 | else 114 | { 115 | 116 | } 117 | 118 | if (graph1 && graph1->_series.size()) 119 | { 120 | glPushMatrix(); 121 | if (graph2 && graph2->_series.size()) 122 | { 123 | glTranslatef(0, .55f, 0); 124 | } 125 | else 126 | { 127 | glTranslatef(0, -.5f, 0); 128 | } 129 | glScalef(1, .5f, 1); 130 | 131 | glColor3f(0, 0, 0); 132 | glBegin(GL_QUADS); 133 | glVertex2f(-1, 1); 134 | glVertex2f(1, 1); 135 | glVertex2f(1, -1); 136 | glVertex2f(-1, -1); 137 | glEnd(); 138 | 139 | graph1->Draw(); 140 | glPopMatrix(); 141 | } 142 | 143 | if (graph2 && graph2->_series.size()) 144 | { 145 | glPushMatrix(); 146 | glTranslatef(0, -.5f, 0); 147 | glScalef(1, .5f, 1); 148 | 149 | glColor3f(0, 0, 0); 150 | glBegin(GL_QUADS); 151 | glVertex2f(-1, 1); 152 | glVertex2f(1, 1); 153 | glVertex2f(1, -1); 154 | glVertex2f(-1, -1); 155 | glEnd(); 156 | 157 | graph2->Draw(); 158 | glPopMatrix(); 159 | } 160 | 161 | glFlush(); // Render now 162 | 163 | if (_ownWindow) 164 | { 165 | glutSwapBuffers(); 166 | } 167 | } 168 | 169 | void GraphManager::RegisterDataSource(shared_ptr src) 170 | { 171 | _sources.push_back(src); 172 | } 173 | 174 | vector GraphManager::GetGraphableStrings() 175 | { 176 | vector ret; 177 | for (auto i = _sources.begin(); i != _sources.end(); i++) 178 | { 179 | vector s = (*i)->GetFields(); 180 | for (auto j = s.begin(); j != s.end(); j++) 181 | { 182 | ret.push_back("AddGraph1."+*j); 183 | ret.push_back("AddGraph2." + *j); 184 | } 185 | } 186 | return ret; 187 | } 188 | 189 | void GraphManager::AddGraph(string path) 190 | { 191 | if (path.find("AddGraph1.") == 0) 192 | { 193 | graph1->AddItem(path.substr(10)); 194 | } 195 | if (path.find("AddGraph2.") == 0) 196 | { 197 | graph2->AddItem(path.substr(10)); 198 | } 199 | } 200 | -------------------------------------------------------------------------------- /src/Drawing/GraphManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Graph.h" 4 | #include 5 | #include 6 | 7 | class DataSource; 8 | 9 | class GraphManager 10 | { 11 | public: 12 | GraphManager(bool own_window=true); 13 | ~GraphManager(); 14 | void Reset(); 15 | void Clear(); 16 | void UpdateData(double time); 17 | void DrawUpdate(); 18 | 19 | void AddGraph(string path); 20 | void InitPaint(); 21 | void Paint(); 22 | 23 | void RegisterDataSource(shared_ptr src); 24 | 25 | shared_ptr graph1, graph2; 26 | std::vector > _sources; 27 | 28 | std::vector GetGraphableStrings(); 29 | 30 | protected: 31 | int _glutWindowNum; 32 | bool _ownWindow; 33 | }; 34 | -------------------------------------------------------------------------------- /src/Drawing/Visualizer_GLUT.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Common.h" 4 | #include "Utility/Camera.h" 5 | #include 6 | #include 7 | 8 | #include "Math/Geometry.h" 9 | #include "Drawing/DrawingFuncs.h" 10 | #include "VehicleDatatypes.h" 11 | #include "Trajectory.h" 12 | #include "DataSource.h" 13 | #include "Drawing/GLUTMenu.h" 14 | 15 | using namespace std; 16 | 17 | class QuadDynamics; 18 | class GraphManager; 19 | 20 | class Visualizer_GLUT : public DataSource 21 | { 22 | 23 | public: 24 | Visualizer_GLUT(int *argcp, char **argv); 25 | ~Visualizer_GLUT(); 26 | 27 | void Reset(); 28 | 29 | void OnMouseClick(int button, int state, int x, int y); 30 | void OnMouseDoubleLClick(int x, int y); 31 | void OnMouseMove(int x, int y); 32 | void SetKeyboardFunc(void(*callback)(unsigned char, int, int)); 33 | 34 | void OnResize(int width, int height); 35 | void Paint(); 36 | void Update(); 37 | 38 | bool IsKeyDown(uint8_t key); 39 | bool IsSpecialKeyDown(int specialKey); 40 | 41 | void SetArrow(V3F begin, V3F end) 42 | { 43 | _arrowBegin = begin; 44 | _arrowEnd = end; 45 | } 46 | 47 | vector > quads; 48 | void VisualizeQuadCopter(shared_ptr quad); 49 | 50 | shared_ptr graph; 51 | 52 | shared_ptr followed_traj; 53 | 54 | void VisualizeTrajectory(const Trajectory& traj, bool drawPoints, V3F color, float alpha=1, V3F pointColor=V3F(.1f,.2f,1), V3F curPointColor=V3F(1,0,0), V3F offset=V3F(), int style=0); 55 | 56 | void InitializeMenu(const vector& strings); 57 | GLUTMenu _menu; 58 | bool _exiting; 59 | 60 | void OnMenu(string); 61 | 62 | protected: 63 | void initializeGL(int *argcp, char **argv); 64 | 65 | 66 | shared_ptr _glDraw; 67 | int _glutWindowNum; 68 | 69 | private: 70 | string _cameraTrackingMode; 71 | GLuint MakeVolumeCallList(); 72 | GLuint _volumeCallList; 73 | 74 | Camera _camera; 75 | 76 | Timer _start; 77 | 78 | V3F _arrowBegin, _arrowEnd; 79 | 80 | 81 | GLdouble modelMatrix[16],projMatrix[16]; 82 | GLint viewport[4]; 83 | 84 | 85 | bool _mouseLeftDown, _mouseRightDown, _mouseMiddleDown; 86 | 87 | 88 | protected: 89 | 90 | 91 | Timer _unlabeledMarkersTime; 92 | 93 | GLUquadricObj *glQuadric; 94 | 95 | bool _drawVolumeBoundaries; 96 | V3F _bgColorBottomRight, _bgColorBottomLeft, _bgColorTopRight, _bgColorTopLeft; 97 | void DrawBackground(); 98 | 99 | void DrawCoordinateReference(); 100 | V3D _refLoc; 101 | 102 | Timer _timeSinceLastPaint; 103 | 104 | void Draw(shared_ptr quad); 105 | void DrawTrajectories(shared_ptr quad); 106 | 107 | 108 | public: 109 | bool showPropCommands; 110 | bool showRefTrajectory, showActualTrajectory; 111 | bool paused; 112 | 113 | // data source functions 114 | virtual bool GetData(const string& name, float& ret) const; 115 | virtual vector GetFields() const; 116 | 117 | void OnMainTimer(); 118 | 119 | FastDelegate1 menuCallback; 120 | 121 | protected: 122 | V3D _doubleClickMousePoint; 123 | Timer _doubleClickTimer; 124 | 125 | SLR::LineD ScreenToPickVector(double x, double y); 126 | 127 | bool _objectSelected; 128 | int _selectedObjectIndex; 129 | V3F _selectedObjectPos; 130 | LowPassFilter _selectedObjectPos_filtered; 131 | 132 | Timer _lastDraw, _lastMainTimerEvent; 133 | float _draw_dt_ms, _timer_dt_ms, _last_draw_time_ms; 134 | 135 | // mouse 136 | int lastPosX, lastPosY; 137 | 138 | string _delayedScenarioLoader; 139 | }; 140 | 141 | -------------------------------------------------------------------------------- /src/Drawing/WindowThreshold.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // "Absolute threshold" trigger/detector 4 | // Detects if the absolute value of a signal goes below a certain threshold for at least a given time 5 | // and plots the detection point (different color if quiet time met) 6 | 7 | #include "BaseAnalyzer.h" 8 | 9 | class WindowThreshold : public BaseAnalyzer 10 | { 11 | public: 12 | WindowThreshold(string var, float thresh, float minWindow) 13 | { 14 | _var = var; 15 | _thresh = thresh; 16 | _minWindow = minWindow; 17 | _lastTime = 0; 18 | Reset(); 19 | } 20 | 21 | void Reset() 22 | { 23 | if (_lastTime != 0) 24 | { 25 | if (_active) 26 | { 27 | printf("PASS: ABS(%s) was less than %lf for at least %lf seconds\n", _var.c_str(), _thresh, _minWindow); 28 | } 29 | else 30 | { 31 | printf("FAIL: ABS(%s) was less than %lf for %lf seconds, which was less than %lf seconds\n", _var.c_str(), _thresh, _lastTime- _lastTimeAboveThresh, _minWindow); 32 | } 33 | } 34 | _lastTimeAboveThresh = numeric_limits::infinity(); 35 | _active = false; 36 | } 37 | 38 | void Update(double time, std::vector >& sources) 39 | { 40 | for (unsigned int j = 0; j < sources.size(); j++) 41 | { 42 | float tmp; 43 | if (sources[j]->GetData(_var, tmp)) 44 | { 45 | OnNewData((float)time, tmp); 46 | break; 47 | } 48 | } 49 | } 50 | 51 | void OnNewData(float time, float meas) 52 | { 53 | _lastTime = time; 54 | if (_active) 55 | { 56 | return; 57 | } 58 | 59 | if (_lastTimeAboveThresh == numeric_limits::infinity()) 60 | { 61 | _lastTimeAboveThresh = time; 62 | } 63 | 64 | if (fabs(meas) > _thresh) 65 | { 66 | _lastTimeAboveThresh = time; 67 | } 68 | 69 | if ((time - _lastTimeAboveThresh) > _minWindow) 70 | { 71 | _active = true; 72 | } 73 | } 74 | 75 | // Draws horizontal threshold bands 76 | // and detection marker/time 77 | void Draw(float minX, float maxX, float minY, float maxY) 78 | { 79 | glColor3f(.1f, .2f, .1f); 80 | 81 | if (_thresh > minY && _thresh < maxY) 82 | { 83 | glBegin(GL_LINES); 84 | glVertex2f(minX, _thresh); 85 | glVertex2f(maxX, _thresh); 86 | glEnd(); 87 | } 88 | 89 | if ((-_thresh) > minY && (-_thresh) < maxY) 90 | { 91 | glBegin(GL_LINES); 92 | glVertex2f(minX, -_thresh); 93 | glVertex2f(maxX, -_thresh); 94 | glEnd(); 95 | } 96 | 97 | if (_lastTimeAboveThresh == numeric_limits::infinity() || _lastTimeAboveThreshmaxX) 98 | { 99 | return; 100 | } 101 | 102 | if (_active) 103 | { 104 | glColor3f(0, 1, 0); 105 | glBegin(GL_LINE_STRIP); 106 | glVertex2f(_lastTimeAboveThresh, CONSTRAIN(_thresh,minY,maxY)); 107 | glVertex2f(_lastTime, CONSTRAIN(_thresh, minY, maxY)); 108 | glVertex2f(_lastTime, CONSTRAIN(-_thresh, minY, maxY)); 109 | glVertex2f(_lastTimeAboveThresh, CONSTRAIN(-_thresh, minY, maxY)); 110 | glVertex2f(_lastTimeAboveThresh, CONSTRAIN(_thresh, minY, maxY)); 111 | glEnd(); 112 | } 113 | else 114 | { 115 | glColor3f(.7f, .1f, .1f); 116 | if (_thresh > minY && _thresh < maxY) 117 | { 118 | glBegin(GL_LINES); 119 | glVertex2f(minX, _thresh); 120 | glVertex2f(maxX, _thresh); 121 | glEnd(); 122 | } 123 | if (-_thresh > minY && -_thresh < maxY) 124 | { 125 | glBegin(GL_LINES); 126 | glVertex2f(minX, -_thresh); 127 | glVertex2f(maxX, -_thresh); 128 | glEnd(); 129 | } 130 | } 131 | } 132 | 133 | bool _active; 134 | string _var; 135 | float _lastTimeAboveThresh; 136 | float _thresh, _minWindow; 137 | float _lastTime; 138 | }; -------------------------------------------------------------------------------- /src/Math/Angles.h: -------------------------------------------------------------------------------- 1 | // Angle utilities, super-lightweight-robotics library 2 | // License: BSD-3-clause 3 | 4 | #pragma once 5 | #include "Constants.h" 6 | 7 | // normalize angle to -pi to pi 8 | inline double AngleNormD(double angle) 9 | { 10 | angle = fmod(angle, (2.0*M_PI)); 11 | 12 | if (angle <= -M_PI) 13 | { 14 | angle += (2.0*M_PI); 15 | } 16 | else if (angle > M_PI) 17 | { 18 | angle -= (2.0*M_PI); 19 | } 20 | 21 | return angle; 22 | } 23 | 24 | // normalize angle to -pi to pi 25 | inline float AngleNormF(float angle) 26 | { 27 | angle = fmod(angle, (2.0f*(float)M_PI)); 28 | 29 | if (angle <= -(float)M_PI) 30 | { 31 | angle += (2.0f*(float)M_PI); 32 | } 33 | else if (angle > (float)M_PI) 34 | { 35 | angle -= (2.0f*(float)M_PI); 36 | } 37 | 38 | return angle; 39 | } -------------------------------------------------------------------------------- /src/Math/Constants.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define _USE_MATH_DEFINES 4 | #include 5 | 6 | #ifndef F_PI 7 | #define F_PI ((float)(M_PI)) 8 | #endif 9 | 10 | const double CONST_GRAVITY = 9.81; // gravity in [m/s^2] -------------------------------------------------------------------------------- /src/Math/Geometry.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "Geometry.h" 3 | 4 | namespace SLR{ 5 | 6 | V3D PlaneD::Intersect(LineD l) const 7 | { 8 | V3D a2b = (l._b-l._a); 9 | 10 | double denom = _normal.dot(a2b.norm()); 11 | if(denom==0) 12 | { 13 | // no intersection 14 | return V3D::Inf(); 15 | } 16 | 17 | double u = _normal.dot(_pt-l._a)/denom; 18 | return l._a+ a2b*u; 19 | } 20 | 21 | V3D PlaneD::Intersect(LineD l, double& distTowardsB) const 22 | { 23 | V3D a2b = (l._b-l._a); 24 | 25 | double denom = _normal.dot(a2b.norm()); 26 | if(denom==0) 27 | { 28 | // no intersection 29 | distTowardsB = numeric_limits::infinity(); 30 | return V3D::Inf(); 31 | } 32 | 33 | distTowardsB = _normal.dot(_pt-l._a)/denom; 34 | return l._a+ a2b*distTowardsB; 35 | } 36 | 37 | //http://local.wasp.uwa.edu.au/~pbourke/geometry/3planes/ 38 | V3D PlaneD::Intersect(const PlaneD& b, const PlaneD& c) const 39 | { 40 | double d1 = _pt.dot(_normal); 41 | double d2 = b._pt.dot(b._normal); 42 | double d3 = c._pt.dot(c._normal); 43 | const V3D& N1 = _normal; 44 | const V3D& N2 = b._normal; 45 | const V3D& N3 = c._normal; 46 | return (d1*(N2.cross(N3))+d2*(N3.cross(N1))+d3*(N1.cross(N2)))/(N1.dot(N2.cross(N3))); 47 | } 48 | 49 | //http://local.wasp.uwa.edu.au/~pbourke/geometry/planeplane/ 50 | //appears to work! :) 51 | LineD PlaneD::Intersect(PlaneD pl) const 52 | { 53 | V3D dir = _normal.cross(pl._normal); // direction of line of intersection 54 | if(dir.mag()==0) return LineD::Invalid(); 55 | 56 | double d1 = _pt.dot(_normal); 57 | double d2 = pl._pt.dot(pl._normal); 58 | 59 | double offDiagonal = _normal.dot(pl._normal); 60 | double det = 1 - offDiagonal * offDiagonal; 61 | double a = (d1 - d2 * offDiagonal) / det; 62 | double b = (d2 - d1 * offDiagonal) / det; 63 | 64 | V3D A = a * _normal + b * pl._normal; 65 | V3D B = A + dir; 66 | return LineD(A,B); 67 | } 68 | 69 | //http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm#Closest%20Point%20of%20Approach 70 | //http://pages.pacificcoast.net/~cazelais/251/distance.pdf 71 | V3D LineD::ClosestPt(const LineD& l) const 72 | { 73 | V3D u = _b-_a; 74 | V3D v = l._b-l._a; 75 | V3D w = _a-l._a; 76 | double a = u.dot(u); // always >= 0 77 | double b = u.dot(v); 78 | double c = v.dot(v); // always >= 0 79 | double d = u.dot(w); 80 | double e = v.dot(w); 81 | double D = a*c - b*b; // always >= 0 82 | double sc;//, tc; 83 | 84 | // compute the line parameters of the two closest points 85 | if (D < 1e-15) { // the lines are almost parallel 86 | sc = 0.0; 87 | //tc = (b>c ? d/b : e/c); // use the largest denominator 88 | } 89 | else { 90 | sc = (b*e - c*d) / D; 91 | //tc = (a*e - b*d) / D; 92 | } 93 | 94 | return _a+u*sc; 95 | 96 | 97 | 98 | /*// get the difference of the two closest points 99 | Vector dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc) 100 | 101 | return norm(dP); // return the closest distance*/ 102 | 103 | } 104 | 105 | V3D LineD::Dist(const V3D& pt) const 106 | { 107 | V3D v = _b - _a; 108 | V3D w = pt - _a; 109 | 110 | double c1 = w.dot(v); 111 | 112 | double c2 = v.dot(v); 113 | 114 | double b = c1 / c2; 115 | V3D closest(_a + b * v); 116 | return closest.dist(pt); 117 | } 118 | 119 | //http://softsurfer.com/Archive/algorithm_0102/algorithm_0102.htm#dist_Point_to_Segment%28%29 120 | V3D LineD::ClosestPt_Segment(const V3D& pt) const 121 | { 122 | V3D v = _b - _a; 123 | V3D w = pt - _a; 124 | 125 | double c1 = w.dot(v); 126 | if ( c1 <= 0 ) 127 | return _a; 128 | 129 | double c2 = v.dot(v); 130 | if ( c2 <= c1 ) 131 | return _b; 132 | 133 | double b = c1 / c2; 134 | return (_a + b * v); 135 | } 136 | 137 | 138 | bool Quad::ClosestPoint_InProjectionOnly(const V3D& pt, V3D& ret, double& signedDist) const 139 | { 140 | PlaneD plane(pts[0],pts[1],pts[2]); 141 | V3D p = plane.Project(pt); 142 | if(!IsInQuad(p)) return false; 143 | ret = p; 144 | signedDist = plane.Dist(pt); 145 | return true; 146 | } 147 | 148 | 149 | 150 | 151 | 152 | }; // namespace SLR 153 | -------------------------------------------------------------------------------- /src/Math/Geometry.h: -------------------------------------------------------------------------------- 1 | // Geometry utilities, super-lightweight-robotics library 2 | // License: BSD-3-clause 3 | 4 | #pragma once 5 | 6 | #include "Common.h" 7 | 8 | #include 9 | using std::vector; 10 | 11 | namespace SLR{ 12 | 13 | class LineD; 14 | 15 | // class for dealing with double-precision planes in 3D 16 | // internally the plane is represented as a normal and a point-on-plane 17 | class PlaneD 18 | { 19 | public: 20 | PlaneD(V3D pt=V3D(), V3D normal=V3D(0,0,1)) 21 | { 22 | _pt = pt; 23 | _normal = normal.norm(); 24 | } 25 | 26 | // points should be in COUNTERCLOCKWISE order 27 | PlaneD(V3D p0ccw, V3D p1ccw, V3D p2ccw) 28 | { 29 | _normal = (p1ccw-p0ccw).cross(p2ccw-p0ccw).norm(); 30 | _pt = p0ccw; 31 | } 32 | 33 | // ABCD format where plane is specified as Ax+By+Cz+D=0 34 | PlaneD(V3D normal, double d) 35 | { 36 | _normal = normal; 37 | _pt = -normal*d; 38 | } 39 | 40 | double D() const 41 | { 42 | return -_pt.dot(_normal); 43 | } 44 | 45 | // least-squares fit! 46 | static PlaneD LeastSquaresFit(vector& pts); 47 | 48 | // positive = plane normal points *toward* point 49 | // negative = plane normal points *away* from point 50 | double Dist(V3D pt) const 51 | { 52 | V3D p = pt-_pt; 53 | return p.norm().dot(_normal)*p.mag(); 54 | } 55 | 56 | V3D Normal() const {return _normal;} 57 | 58 | V3D Intersect(const PlaneD& b, const PlaneD& c) const; 59 | 60 | V3D Project(V3D pt) const 61 | { 62 | V3D p = pt-_pt; 63 | return pt - p.norm().dot(_normal)*p.mag()*_normal; 64 | } 65 | 66 | // given a 3d coordinate, leaves it alone if it's at least 'offset' in front of the plane. 67 | // Otherwise 'pushes' the input point in the direction orthogonal to the plane until it's 'offset' 68 | // in front of it and returns that coordinate. 69 | // (This is useful, for example, for getting a safe return point if a vehicle is too close to a plane) 70 | V3D PushOut(V3D pt, double offset=0) 71 | { 72 | V3D p = pt-_pt; 73 | double dist = p.norm().dot(_normal)*p.mag(); 74 | if(dist >= offset) return pt; 75 | return pt - p.norm().dot(_normal)*(p.mag()+offset)*_normal; 76 | } 77 | 78 | V3D Intersect(LineD l) const; 79 | V3D Intersect(LineD l, double& distTowardsB) const; 80 | 81 | // returns a line where two planes intersect 82 | // direction of line (a towards b) determined by RH rule 83 | // with *this* plane, and pl (i.e. *this cross pl) 84 | LineD Intersect(PlaneD pl) const; 85 | 86 | PlaneD Flip() const 87 | { 88 | return PlaneD(_pt,-_normal); 89 | } 90 | 91 | string ToString() const 92 | { 93 | char buf[200]; 94 | sprintf_s(buf,200,"pt={%.3lf %.3lf %.3lf} norm={%.3lf %.3lf %.3lf}", 95 | _pt.x,_pt.y,_pt.z,_normal.x,_normal.y,_normal.z); 96 | return string(buf); 97 | } 98 | 99 | protected: 100 | V3D _normal, _pt; 101 | }; 102 | 103 | class LineD 104 | { 105 | friend class PlaneD; 106 | public: 107 | LineD(V3D a=V3D(), V3D b=V3D(1,0,0)) 108 | { 109 | _a = a; 110 | _b = b; 111 | } 112 | 113 | static LineD Invalid() 114 | { 115 | return LineD(-V3D::Inf(),V3D::Inf()); 116 | } 117 | 118 | V3D ClosestPt_Segment(const V3D& pt) const; 119 | V3D ClosestPt(const LineD& l) const; 120 | V3D Dist(const V3D& pt) const; 121 | bool IsEndpoint(const V3D& pt) const{ return pt==_a || pt==_b;}; 122 | 123 | protected: 124 | V3D _a, _b; 125 | }; 126 | 127 | 128 | 129 | 130 | 131 | ////////////////////////////////////////////// 132 | 133 | // planar, convex quad 134 | class Quad 135 | { 136 | public: 137 | // point order: CCW about normal 138 | Quad(){}; 139 | Quad(V3D a, V3D b, V3D c, V3D d) 140 | { 141 | pts[0]=a; 142 | pts[1]=b; 143 | pts[2]=c; 144 | pts[3]=d; 145 | } 146 | Quad(const vector &_pts) 147 | { 148 | if(_pts.size()!=4) return; 149 | for(int i=0;i<4;i++) 150 | { 151 | pts[i]=_pts[i]; 152 | } 153 | } 154 | Quad(const vector &_pts) 155 | { 156 | if(_pts.size()!=4) return; 157 | for(int i=0;i<4;i++) 158 | { 159 | pts[i]=_pts[i]; 160 | } 161 | } 162 | 163 | bool IsInQuad(const V3D& pt) const 164 | { 165 | const double planarTolerance = 1e-10; 166 | // check that it's in-plane 167 | if(fabs(PlaneD(pts[0],pts[1],pts[2]).Dist(pt))>planarTolerance) return false; 168 | 169 | // possibly silly algorithm 170 | V3D v01 = (pts[1]-pts[0]).norm(); 171 | //V3D v02 = (pts[2]-pts[0]).norm(); 172 | V3D v03 = (pts[3]-pts[0]).norm(); 173 | V3D v21 = (pts[1]-pts[2]).norm(); 174 | V3D v23 = (pts[3]-pts[2]).norm(); 175 | 176 | V3D v0 = (pt-pts[0]).norm(); 177 | V3D v1 = (pt-pts[1]).norm(); 178 | V3D v2 = (pt-pts[2]).norm(); 179 | V3D v3 = (pt-pts[3]).norm(); 180 | 181 | // check that it lies on the right side of v01 182 | if(v03.cross(v01).dot(v0.cross(v01))<0) return false; 183 | 184 | // check that it lies on the right side of v12 185 | if((-v01).cross(-v21).dot(v1.cross(-v21))<0) return false; 186 | 187 | // check that it lies on the right side of v23 188 | if((v21).cross(v23).dot(v2.cross(v23))<0) return false; 189 | 190 | // check that it lies on the right side of v30 191 | if((-v23).cross(-v03).dot(v3.cross(-v03))<0) return false; 192 | 193 | return true; 194 | } 195 | 196 | // type = 0 if in-plane, 1 if edge, 2 if corner 197 | V3D ClosestPoint(const V3D& pt, double* signedDist=NULL, unsigned char* type=NULL) const 198 | { 199 | PlaneD plane(pts[0],pts[1],pts[2]); 200 | V3D p = plane.Project(pt); 201 | if(IsInQuad(p)) 202 | { 203 | if(type!=NULL) *type=0; 204 | if(signedDist!=NULL) *signedDist=plane.Dist(pt); 205 | return p; 206 | } 207 | LineD lines[4] = {LineD(pts[0],pts[1]),LineD(pts[1],pts[2]),LineD(pts[2],pts[3]),LineD(pts[3],pts[0])}; 208 | double dist = numeric_limits::infinity(); 209 | V3D ret; 210 | for(int i=0;i<4;i++) 211 | { 212 | V3D p = lines[i].ClosestPt_Segment(pt); 213 | if(p.dist(pt) 4 | class LowPassFilter 5 | { 6 | public: 7 | LowPassFilter(T_TimeConstant timeConstant=1, T initialVal=1) 8 | { 9 | _timeConstant = timeConstant; 10 | _val = initialVal; 11 | } 12 | 13 | T Read() const 14 | { 15 | return _val; 16 | } 17 | 18 | T Update(T meas, double dt) 19 | { 20 | T_TimeConstant alpha = dt / (_timeConstant + dt); 21 | _val = (T) (_val * (1.0-alpha) + meas*alpha); 22 | return _val; 23 | } 24 | 25 | void Reset(T meas) 26 | { 27 | _val = meas; 28 | } 29 | 30 | void SetTau(T_TimeConstant tau) 31 | { 32 | _timeConstant = tau; 33 | } 34 | 35 | protected: 36 | T _val; 37 | T_TimeConstant _timeConstant; 38 | }; -------------------------------------------------------------------------------- /src/Math/Mat3x3F.h: -------------------------------------------------------------------------------- 1 | // 3x3 Matrix of floats, super-lightweight-robotics library 2 | // License: BSD-3-clause 3 | 4 | #pragma once 5 | 6 | #include // swap 7 | #include 8 | #include "V3F.h" 9 | 10 | // class for operating with 3-by-3 matricies of floats 11 | // compatible with V3F! 12 | // assumes row-major ordering: 13 | // 0 1 2 14 | // 3 4 5 15 | // 6 7 8 16 | 17 | class Mat3x3F 18 | { 19 | public: 20 | Mat3x3F() 21 | { 22 | for(unsigned char i=0;i<9;i++) _v[i]=0; 23 | _v[0] = _v[4] = _v[8] = 1.0f; // diagonal! 24 | } 25 | 26 | Mat3x3F(const float v[9]) 27 | { 28 | for(unsigned char i=0;i<9;i++) _v[i]=v[i]; 29 | } 30 | 31 | static Mat3x3F Zeros() 32 | { 33 | float v[9] = {0,0,0,0,0,0,0,0,0}; 34 | return Mat3x3F(v); 35 | } 36 | 37 | static Mat3x3F Ident() 38 | { 39 | float v[9] = {1,0,0,0,1,0,0,0,1}; 40 | return Mat3x3F(v); 41 | } 42 | 43 | void Transpose() 44 | { 45 | std::swap(_v[1],_v[3]); 46 | std::swap(_v[2],_v[6]); 47 | std::swap(_v[5],_v[7]); 48 | } 49 | 50 | Mat3x3F RetTranspose() //Returns transpose but leaves original matrix unchanged 51 | { 52 | Mat3x3F res = *this; 53 | std::swap(res._v[1],res._v[3]); 54 | std::swap(res._v[2],res._v[6]); 55 | std::swap(res._v[5],res._v[7]); 56 | 57 | return res; 58 | } 59 | 60 | float Trace() { 61 | float res = _v[0] + _v[4] + _v[8]; 62 | return res; 63 | } 64 | 65 | Mat3x3F operator*(const Mat3x3F& b) 66 | { 67 | float res[9] = {0,0,0,0,0,0,0,0,0}; 68 | for(unsigned char r=0;r<3;r++) 69 | { 70 | for(unsigned char c=0;c<3;c++) 71 | { 72 | res[r*3+c] += _v[r*3]*b._v[0*3+c] + _v[r*3+1]*b._v[1*3+c] + _v[r*3+2]*b._v[2*3+c]; 73 | } 74 | } 75 | return Mat3x3F(res); 76 | } 77 | 78 | Mat3x3F operator*(const float& b) 79 | { 80 | float res[9] = {0,0,0,0,0,0,0,0,0}; 81 | for(unsigned char i=0;i<9;i++) res[i] = b*_v[i]; 82 | 83 | return Mat3x3F(res); 84 | } 85 | 86 | static Mat3x3F Rotation(V3F vec, float theta) 87 | { 88 | vec = vec.norm(); 89 | float c = cos(theta); 90 | float s = sin(theta); 91 | float t = (1.0f-c); 92 | float X = vec.x; float X2 = X*X; 93 | float Y = vec.y; float Y2 = Y*Y; 94 | float Z = vec.z; float Z2 = Z*Z; 95 | 96 | float v[9]; 97 | v[0] = t*X2 + c; 98 | v[1] = t*X*Y + s*Z; 99 | v[2] = t*X*Z - s*Y; 100 | v[3] = t*X*Y - s*Z; 101 | v[4] = t*Y2 + c; 102 | v[5] = t*Y*Z + s*X; 103 | v[6] = t*X*Z + s*Y; //Corrected from v[6] = t*X*Y + s*Y; Luzius; 7.10.2010 104 | v[7] = t*Y*Z - s*X; 105 | v[8] = t*Z2 + c; 106 | 107 | return Mat3x3F(v); 108 | } 109 | 110 | static Mat3x3F SkewSymmetric(const float a, const float b, const float c) 111 | { 112 | float v[9]; 113 | v[0] = 0.0f; 114 | v[1] = -c; 115 | v[2] = b; 116 | 117 | v[3] = c; 118 | v[4] = 0.0f; 119 | v[5] = -a; 120 | 121 | v[6] = -b; 122 | v[7] = a; 123 | v[8] = 0.0f; 124 | 125 | return Mat3x3F(v); 126 | } 127 | 128 | static Mat3x3F SkewSymmetric(const V3F& w){return Mat3x3F::SkewSymmetric(w.x,w.y,w.z);} 129 | 130 | static Mat3x3F OuterProduct(const V3F a, const V3F b)//a^T*b 131 | { 132 | float v[9]; 133 | v[0] = a.x*b.x; 134 | v[1] = a.x*b.y; 135 | v[2] = a.x*b.z; 136 | 137 | v[3] = a.y*b.x; 138 | v[4] = a.y*b.y; 139 | v[5] = a.y*b.z; 140 | 141 | v[6] = a.z*b.x; 142 | v[7] = a.z*b.y; 143 | v[8] = a.z*b.z; 144 | 145 | return Mat3x3F(v); 146 | } 147 | 148 | V3F operator*(const V3F& a) const 149 | { 150 | V3F ret; 151 | ret[0] = a[0]*_v[0] + a[1]*_v[1] + a[2]*_v[2]; 152 | ret[1] = a[0]*_v[3] + a[1]*_v[4] + a[2]*_v[5]; 153 | ret[2] = a[0]*_v[6] + a[1]*_v[7] + a[2]*_v[8]; 154 | return ret; 155 | } 156 | 157 | Mat3x3F operator+(const Mat3x3F& a) 158 | { 159 | Mat3x3F ret; 160 | for(unsigned char i=0;i<9;i++) 161 | { 162 | ret._v[i] = _v[i]+a._v[i]; 163 | } 164 | return ret; 165 | } 166 | 167 | 168 | Mat3x3F operator-(const Mat3x3F& a) 169 | { 170 | Mat3x3F ret; 171 | for(unsigned char i=0;i<9;i++) 172 | { 173 | ret._v[i] = _v[i]-a._v[i]; 174 | } 175 | return ret; 176 | } 177 | 178 | Mat3x3F operator/(const float a) 179 | { 180 | Mat3x3F ret; 181 | for(unsigned char i=0;i<9;i++) 182 | { 183 | ret._v[i] = _v[i]/a; 184 | } 185 | return ret; 186 | } 187 | 188 | float& operator()(unsigned char r, unsigned char c) 189 | { 190 | assert(r<3 && c<3); 191 | return _v[r*3+c]; 192 | } 193 | 194 | float& operator[](int i) 195 | { 196 | assert(i>=0 && i<9); 197 | return _v[i]; 198 | } 199 | 200 | const float& operator[](int i) const 201 | { 202 | assert(i>=0 && i<9); 203 | return _v[i]; 204 | } 205 | 206 | void SetValues(float v[9]) 207 | { 208 | for(unsigned char i=0;i<9;i++) _v[i]=v[i]; 209 | } 210 | 211 | void SetZero() 212 | { 213 | for(unsigned char i=0;i<9;i++) _v[i]=0; 214 | } 215 | 216 | Mat3x3F Pointdot(const Mat3x3F& b) 217 | { //multiply matrices elementwise (Matlab: A.*B) 218 | float res[9]= {0,0,0,0,0,0,0,0,0}; 219 | for(int i=0;i<9;i++) { 220 | res[i] = _v[i]*b._v[i]; 221 | } 222 | return Mat3x3F(res); 223 | } 224 | 225 | //cross product of vector with each column of a matrix, returns a matrix (vec x Mat) 226 | // side = 0 <-> vec x Mat 227 | // side = 1 <-> Mat x vec 228 | Mat3x3F Cross(const V3F vec, int side) 229 | { 230 | V3F tmp1(_v[0],_v[3],_v[6]); 231 | V3F tmp2(_v[1],_v[4],_v[7]); 232 | V3F tmp3(_v[2],_v[5],_v[8]); 233 | float v[9]; 234 | if(side==0) { 235 | tmp1 = vec.cross(tmp1); 236 | tmp2 = vec.cross(tmp2); 237 | tmp3 = vec.cross(tmp3); 238 | } 239 | else if(side==1) { 240 | tmp1 = tmp1.cross(vec); 241 | tmp2 = tmp2.cross(vec); 242 | tmp3 = tmp3.cross(vec); 243 | } 244 | tmp1.get(v[0],v[3],v[6]); 245 | tmp2.get(v[1],v[4],v[7]); 246 | tmp3.get(v[2],v[5],v[8]); 247 | return Mat3x3F(v); 248 | } 249 | 250 | float Determinant() 251 | { 252 | float res = 0; 253 | res += _v[0]*(_v[4]*_v[8]-_v[7]*_v[5]) 254 | - _v[3]*(_v[1]*_v[8]-_v[7]*_v[2]) 255 | + _v[6]*(_v[1]*_v[5]-_v[4]*_v[2]); 256 | 257 | return res; 258 | } 259 | 260 | Mat3x3F Inverse(void) 261 | { 262 | Mat3x3F res; 263 | float det = this->Determinant(); 264 | res[0] = (_v[4]*_v[8]-_v[7]*_v[5])/det; 265 | res[1] = -(_v[1]*_v[8]-_v[7]*_v[2])/det; 266 | res[2] = (_v[1]*_v[5]-_v[4]*_v[2])/det; 267 | 268 | res[3] = -(_v[3]*_v[8]-_v[6]*_v[5])/det; 269 | res[4] = (_v[0]*_v[8]-_v[6]*_v[2])/det; 270 | res[5] = -(_v[0]*_v[5]-_v[3]*_v[2])/det; 271 | 272 | res[6] = (_v[3]*_v[7]-_v[6]*_v[4])/det; 273 | res[7] = -(_v[0]*_v[7]-_v[6]*_v[1])/det; 274 | res[8] = (_v[0]*_v[4]-_v[3]*_v[1])/det; 275 | 276 | return res; 277 | 278 | } 279 | 280 | 281 | // 0 1 2 282 | // 3 4 5 283 | // 6 7 8 284 | 285 | 286 | protected: 287 | float _v[9]; 288 | }; 289 | 290 | 291 | -------------------------------------------------------------------------------- /src/Math/MathUtils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef MAX 4 | #define MAX(a,b) (((a)>(b))?(a):(b)) 5 | #endif 6 | 7 | #ifndef MIN 8 | #define MIN(a,b) (((a)<(b))?(a):(b)) 9 | #endif 10 | 11 | #ifndef CONSTRAIN 12 | #define CONSTRAIN(a,low,high) MAX((low),MIN((a),(high))) 13 | #endif -------------------------------------------------------------------------------- /src/Math/Random.cpp: -------------------------------------------------------------------------------- 1 | // Random Number Utilities, super-lightweight-robotics library 2 | // License: BSD-3-clause 3 | 4 | #include "../Common.h" 5 | #include "Random.h" 6 | #include 7 | 8 | // from num recipes in c++ chap7 9 | // a uniform random number generator 10 | double ran1(int &idum) 11 | { 12 | const int IA = 16807, IM = 2147483647, IQ = 127773, IR = 2836, NTAB = 32; 13 | const int NDIV = (1+ (IM-1)/NTAB); 14 | const double EPS = 3.0e-16, AM = 1.0/IM, RNMX = (1.0-EPS); 15 | static int iy = 0; 16 | static int iv[NTAB]; 17 | int j,k; 18 | double temp; 19 | 20 | if(idum<=0 || ! iy) // initialization 21 | { 22 | if(-idum<1) idum = 1; // prevent idum = 0 23 | else idum = -idum; 24 | for (j = NTAB+7; j>=0; j--) 25 | { 26 | k = idum/IQ; 27 | idum = IA*(idum-k*IQ)-IR*k; 28 | if(idum<0) idum += IM; 29 | if (jRNMX) return RNMX; 42 | else return temp; 43 | } 44 | 45 | // a gaussian random number generator from numerical recipes 46 | // need the uniform RV generator to work 47 | double gasdev(int &idum) 48 | { 49 | static int iset = 0; 50 | static double gset; 51 | double fac, rsq, v1,v2; 52 | 53 | if(idum<0) iset = 0; 54 | if(iset == 0) 55 | { 56 | do 57 | { 58 | v1=2.0*ran1(idum)-1.0; 59 | v2=2.0*ran1(idum)-1.0; 60 | rsq = v1*v1 + v2*v2; 61 | } while (rsq >= 1.0 || rsq == 0.0); 62 | fac = sqrt(-2.0 * log(rsq)/rsq); 63 | gset = v1*fac; 64 | iset = 1; 65 | return v2*fac; 66 | } 67 | else 68 | { 69 | iset = 0; 70 | return gset; 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /src/Math/Random.h: -------------------------------------------------------------------------------- 1 | // Random Number Utilities Header, super-lightweight-robotics library 2 | // License: BSD-3-clause 3 | 4 | #pragma once 5 | 6 | // random number routines from Numerical Recipes 7 | double gasdev(int &idum); 8 | double ran1(int &idum); 9 | 10 | inline float ran1_inRange(float min, float max, int& idum) 11 | { 12 | return (float)(ran1(idum)*(max-min)+min); 13 | } 14 | 15 | inline double ran1_inRange(double min, double max, int& idum) 16 | { 17 | return (double)(ran1(idum)*(max-min)+min); 18 | } -------------------------------------------------------------------------------- /src/Math/V3D.h: -------------------------------------------------------------------------------- 1 | // Vector, 3-Doubles, super-lightweight-robotics library 2 | // 2003-2018 sergei lupashin 3 | // License: BSD-3-clause 4 | 5 | #pragma once 6 | 7 | #ifndef _USE_MATH_DEFINES 8 | #define _USE_MATH_DEFINES 9 | #endif 10 | #include 11 | #include "MathUtils.h" 12 | #include 13 | #include 14 | using namespace std; 15 | 16 | #define NULL_VECTOR_D V3D(0.0,0.0,0.0) 17 | 18 | inline void normalize_3(double* v){ 19 | double mag = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); 20 | v[0] /= mag; 21 | v[1] /= mag; 22 | v[2] /= mag; 23 | } 24 | 25 | #pragma pack(push,1) 26 | 27 | class V3D{ 28 | public: 29 | double x,y,z; 30 | V3D():x(0),y(0),z(0){}; 31 | V3D(const int v):x(v),y(v),z(v){}; 32 | V3D(const double v):x(v),y(v),z(v){}; 33 | V3D(const double X, const double Y, const double Z):x(X),y(Y),z(Z){}; 34 | V3D(const double* d):x(d[0]),y(d[1]),z(d[2]){}; 35 | V3D(const float* d):x(d[0]),y(d[1]),z(d[2]){}; 36 | #ifdef BOOST_MSVC 37 | V3D(const boost::numeric::ublas::bounded_vector& d):x(d(0)),y(d(1)),z(d(2)){}; 38 | V3D(const boost::numeric::ublas::bounded_vector& d):x(d(0)),y(d(1)),z(d(2)){}; 39 | #endif 40 | 41 | inline V3D operator+(const V3D b) const{ return V3D(x+b.x,y+b.y,z+b.z); }; 42 | inline V3D operator*(const V3D b) const{ return V3D(x*b.x,y*b.y,z*b.z); }; 43 | inline V3D operator/(const V3D b) const{ return V3D(x/b.x,y/b.y,z/b.z); }; 44 | inline V3D operator-(const V3D b) const{ return V3D(x-b.x,y-b.y,z-b.z); }; 45 | 46 | inline V3D operator+(const double b) const{ return V3D(x+b,y+b,z+b); }; 47 | inline V3D operator*(const double b) const{ return V3D(x*b,y*b,z*b); }; 48 | inline V3D operator/(const double b) const{ return V3D(x/b,y/b,z/b); }; 49 | inline V3D operator-(const double b) const{ return V3D(x-b,y-b,z-b); }; 50 | 51 | inline bool operator==(const V3D& b) const{ return (b.x==x && b.y==y && b.z==z);} 52 | inline bool operator!=(const V3D& b) const{ return (b.x!=x || b.y!=y || b.z!=z);} 53 | inline bool operator>(const V3D& b) const{ return (b.x>x || b.y>y || b.z>z);} 54 | inline bool operator<(const V3D& b) const{ return (b.x::infinity(),numeric_limits::infinity(),numeric_limits::infinity()); 88 | } 89 | 90 | // projects this vector onto b and return the magnitude of the resulting vector 91 | inline double projectMag(const V3D& b) 92 | { 93 | return (this->dot(b))/b.magSq(); 94 | } 95 | 96 | // return projection of this vector onto b 97 | inline V3D projectOnto(const V3D& b) 98 | { 99 | return b.norm() * (this->dot(b.norm())); 100 | } 101 | 102 | inline double operator[](const int i) const { 103 | switch (i) { 104 | case 0: return x; 105 | case 1: return y; 106 | default: return z; 107 | } 108 | } 109 | 110 | inline double& operator[](const int i){ 111 | switch(i){ 112 | case 0: return x; 113 | case 1: return y; 114 | default: return z; 115 | } 116 | } 117 | 118 | inline const double* getArray() const { return &x; }; 119 | inline void get(double* v) const {v[0] = x; v[1] = y; v[2] = z;}; 120 | inline void get(float* v) const {v[0] = (float)x; v[1] = (float)y; v[2] = (float)z;}; 121 | inline void get(double& X, double& Y, double& Z) const {X = x; Y = y; Z = z;}; 122 | inline void set(double* v) {x = v[0]; y = v[1]; z = v[2];}; 123 | 124 | inline V3D cross(const V3D v) const{ 125 | V3D resVector; 126 | resVector.x = y*v.z - z*v.y; 127 | resVector.y = z*v.x - x*v.z; 128 | resVector.z = x*v.y - y*v.x; 129 | return resVector; 130 | } 131 | 132 | inline double dot(const V3D a) const{ 133 | return x*a.x + y*a.y + z*a.z; 134 | } 135 | 136 | inline V3D norm() const{ 137 | V3D res; 138 | double l = mag(); 139 | if (l == 0.0) return NULL_VECTOR_D; 140 | return operator/(l); 141 | } 142 | 143 | inline double sum() const{ return x+y+z;}; 144 | 145 | inline double dist(const V3D b) const{ return operator-(b).mag(); } 146 | inline double dist_sq(const V3D b) const{ return operator-(b).sq().sum(); } 147 | 148 | inline double distXY(const V3D b) const{ return ::sqrt((x-b.x)*(x-b.x)+(y-b.y)*(y-b.y)); } 149 | 150 | #ifdef _WIN32 151 | inline string ToString(const char* delim=" ") const 152 | { 153 | char buf[200]; 154 | sprintf_s(buf,200,"%.3lf%s%.3lf%s%.3lf",x,delim,y,delim,z); 155 | return string(buf); 156 | } 157 | 158 | inline string ToString_FullPrecision() const 159 | { 160 | char buf[200]; 161 | sprintf_s(buf,200,"%lf %lf %lf",x,y,z); 162 | return string(buf); 163 | } 164 | #endif 165 | }; 166 | 167 | inline V3D operator*(double a, V3D b) 168 | { 169 | return V3D(a*b.x,a*b.y,a*b.z); 170 | } 171 | 172 | inline V3D operator/(double a, V3D b) 173 | { 174 | return V3D(a/b.x,a/b.y,a/b.z); 175 | } 176 | 177 | inline V3D operator-(double a, V3D b) 178 | { 179 | return V3D(a-b.x,a-b.y,a-b.z); 180 | } 181 | 182 | inline V3D operator+(double a, V3D b) 183 | { 184 | return b+a; 185 | } 186 | 187 | #pragma pack(pop) 188 | 189 | -------------------------------------------------------------------------------- /src/Math/V3F.h: -------------------------------------------------------------------------------- 1 | // Vector, 3-Floats, super-lightweight-robotics library 2 | // 2003-2018 sergei lupashin 3 | // License: BSD-3-clause 4 | 5 | #pragma once 6 | 7 | #ifndef _USE_MATH_DEFINES 8 | #define _USE_MATH_DEFINES 9 | #endif 10 | #include 11 | #include "V3D.h" 12 | 13 | #define NULL_VECTOR_F V3F(0.0,0.0,0.0) 14 | 15 | inline void normalize_3(float* v){ 16 | float mag = sqrtf(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); 17 | v[0] /= mag; 18 | v[1] /= mag; 19 | v[2] /= mag; 20 | } 21 | 22 | #pragma pack(push,1) 23 | 24 | class V3F{ 25 | public: 26 | float x,y,z; 27 | V3F():x(0),y(0),z(0){}; 28 | V3F(const float v):x(v),y(v),z(v){}; 29 | V3F(const float X, const float Y, const float Z):x(X),y(Y),z(Z){}; 30 | V3F(const float* d):x(d[0]),y(d[1]),z(d[2]){}; 31 | explicit V3F(const double* d):x((float)d[0]),y((float)d[1]),z((float)d[2]){}; 32 | explicit V3F(const V3D& d):x((float)d.x),y((float)d.y),z((float)d.z){}; 33 | 34 | inline V3F operator+(const V3F b) const{ return V3F(x+b.x,y+b.y,z+b.z); }; 35 | inline V3F operator*(const V3F b) const{ return V3F(x*b.x,y*b.y,z*b.z); }; 36 | inline V3F operator/(const V3F b) const{ return V3F(x/b.x,y/b.y,z/b.z); }; 37 | inline V3F operator-(const V3F b) const{ return V3F(x-b.x,y-b.y,z-b.z); }; 38 | 39 | inline V3F operator+(const float b) const{ return V3F(x+b,y+b,z+b); }; 40 | inline V3F operator*(const float b) const{ return V3F(x*b,y*b,z*b); }; 41 | inline V3F operator/(const float b) const{ return V3F(x/b,y/b,z/b); }; 42 | inline V3F operator-(const float b) const{ return V3F(x-b,y-b,z-b); }; 43 | 44 | inline bool operator==(const V3F& b) const{ return (b.x==x && b.y==y && b.z==z);} 45 | inline bool operator!=(const V3F& b) const{ return (b.x!=x || b.y!=y || b.z!=z);} 46 | 47 | inline V3F operator/=(const V3F b){ *this = *this / b; return *this;} 48 | inline V3F operator*=(const V3F b){ *this = *this * b; return *this;} 49 | inline V3F operator+=(const V3F b){ *this = *this + b; return *this;} 50 | inline V3F operator-=(const V3F b){ *this = *this - b; return *this;} 51 | 52 | inline V3F sq() const{ return operator*(*this);} 53 | 54 | inline V3F operator - () const { return V3F(-x, -y, -z); } 55 | 56 | inline void constrain(float lowXYZ, float highXYZ) 57 | { 58 | constrain(lowXYZ, highXYZ, lowXYZ, highXYZ, lowXYZ, highXYZ); 59 | } 60 | inline void constrain(float lowX, float highX, float lowY, float highY, float lowZ, float highZ) 61 | { 62 | x = CONSTRAIN(x, lowX, highX); 63 | y = CONSTRAIN(y, lowY, highY); 64 | z = CONSTRAIN(z, lowZ, highZ); 65 | } 66 | 67 | inline float mag() const{return sqrtf(x*x+y*y+z*z); } 68 | inline float magSq() const{return (x*x+y*y+z*z); } 69 | inline float magXY() const{return sqrtf(x*x+y*y); } 70 | 71 | inline bool isZero() const{return (x==0.f && y==0.f && z==0.f);}; 72 | 73 | inline void zero(){x=y=z=0.0;}; 74 | 75 | inline float operator[](const int i) const { 76 | switch (i) { 77 | case 0: return x; 78 | case 1: return y; 79 | default: return z; 80 | } 81 | } 82 | 83 | inline float& operator[](const int i){ 84 | switch(i){ 85 | case 0: return x; 86 | case 1: return y; 87 | default: return z; 88 | } 89 | } 90 | 91 | inline const float* getArray() const { return &x; }; 92 | inline void get(float* v) const {v[0] = x; v[1] = y; v[2] = z;}; 93 | inline void get(float& X, float& Y, float& Z) const {X = x; Y = y; Z = z;}; 94 | inline void set(float* v) {x = v[0]; y = v[1]; z = v[2];}; 95 | 96 | inline V3F cross(const V3F v) const{ 97 | V3F resVector; 98 | resVector.x = y*v.z - z*v.y; 99 | resVector.y = z*v.x - x*v.z; 100 | resVector.z = x*v.y - y*v.x; 101 | return resVector; 102 | } 103 | 104 | inline float dot(const V3F a) const{ 105 | return x*a.x + y*a.y + z*a.z; 106 | } 107 | 108 | inline V3F norm() const{ 109 | V3F res; 110 | float l = mag(); 111 | if (l == 0.0f) return NULL_VECTOR_F; 112 | return operator/(l); 113 | } 114 | 115 | inline float sum() const{ return x+y+z;}; 116 | 117 | inline float dist(const V3F b) const{ return operator-(b).mag(); } 118 | inline float dist_sq(const V3F b) const{ return operator-(b).sq().sum(); } 119 | 120 | inline float distXY(const V3F b) const{ return sqrtf((x-b.x)*(x-b.x)+(y-b.y)*(y-b.y)); } 121 | 122 | inline operator V3D() const 123 | { 124 | return V3D(x,y,z); 125 | } 126 | 127 | }; 128 | 129 | inline V3F operator*(float a, V3F b) 130 | { 131 | return V3F(a*b.x, a*b.y, a*b.z); 132 | } 133 | 134 | inline V3F operator/(float a, V3F b) 135 | { 136 | return V3F(a / b.x, a / b.y, a / b.z); 137 | } 138 | 139 | inline V3F operator-(float a, V3F b) 140 | { 141 | return V3F(a - b.x, a - b.y, a - b.z); 142 | } 143 | 144 | inline V3F operator+(float a, V3F b) 145 | { 146 | return b + a; 147 | } 148 | 149 | #pragma pack(pop) 150 | 151 | -------------------------------------------------------------------------------- /src/Math/V4D.h: -------------------------------------------------------------------------------- 1 | // Vector, 4-Doubles, super-lightweight-robotics library 2 | // 2003-2018 sergei lupashin 3 | // License: BSD-3-clause 4 | 5 | #pragma once 6 | 7 | struct V4D 8 | { 9 | V4D() { v[0] = v[1] = v[2] = v[3] = 0; } 10 | 11 | V4D(double a, double b, double c, double d) 12 | { 13 | v[0] = a; 14 | v[1] = b; 15 | v[2] = c; 16 | v[3] = d; 17 | } 18 | 19 | V4D(const V4D& b) 20 | { 21 | v[0] = b.v[0]; 22 | v[1] = b.v[1]; 23 | v[2] = b.v[2]; 24 | v[3] = b.v[3]; 25 | } 26 | 27 | V4D(const double b[4]) 28 | { 29 | v[0] = b[0]; 30 | v[1] = b[1]; 31 | v[2] = b[2]; 32 | v[3] = b[3]; 33 | } 34 | 35 | V4D(const float b[4]) 36 | { 37 | v[0] = b[0]; 38 | v[1] = b[1]; 39 | v[2] = b[2]; 40 | v[3] = b[3]; 41 | } 42 | 43 | V4D operator*(double d) const 44 | { 45 | return V4D(v[0] * d, v[1] * d, v[2] * d, v[3] * d); 46 | } 47 | 48 | V4D operator/(double d) const 49 | { 50 | return V4D(v[0] / d, v[1] / d, v[2] / d, v[3] / d); 51 | } 52 | 53 | V4D operator+(V4D d) const 54 | { 55 | return V4D(v[0] + d[0], v[1] + d[1], v[2] + d[2], v[3] + d[3]); 56 | } 57 | 58 | V4D operator-(V4D d) const 59 | { 60 | return V4D(v[0] - d[0], v[1] - d[1], v[2] - d[2], v[3] - d[3]); 61 | } 62 | 63 | double operator[](int i) const { return v[i]; } 64 | double operator()(int i) const { return v[i]; } 65 | double & operator[](int i) { return v[i]; } 66 | double & operator()(int i) { return v[i]; } 67 | 68 | double v[4]; 69 | }; 70 | 71 | inline V4D element_prod(const V4D& a, const V4D& b) 72 | { 73 | return V4D(a.v[0] * b.v[0], a.v[1] * b.v[1], a.v[2] * b.v[2], a.v[3] * b.v[3]); 74 | } 75 | 76 | inline double sum(const V4D& a) 77 | { 78 | return a.v[0] + a.v[1] + a.v[2] + a.v[3]; 79 | } 80 | 81 | inline double norm_2(const V4D& a) 82 | { 83 | return a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]; 84 | 85 | } -------------------------------------------------------------------------------- /src/MavlinkNode/MavlinkNode.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "MavlinkNode.h" 3 | #ifndef _WIN32 4 | #include 5 | #endif 6 | 7 | MavlinkNode::MavlinkNode(string myIP) 8 | : _socket(myIP, MAVLINK_RX_PORT) 9 | { 10 | _first = true; 11 | _doubleCnt=0; 12 | 13 | _running = true; 14 | 15 | #ifdef _WIN32 16 | _thread = CreateThread(NULL,NULL,RxThread,this,NULL,NULL); 17 | #else 18 | pthread_create(&_thread, NULL, RxThread, this); 19 | #endif 20 | 21 | _packet.data = new unsigned char[MAX_UDP_PACKET_SIZE]; 22 | } 23 | 24 | MavlinkNode::~MavlinkNode() 25 | { 26 | _running = false; 27 | #ifdef _WIN32 28 | if(WaitForSingleObject(_thread,100)!=WAIT_OBJECT_0) 29 | { 30 | TerminateThread(_thread,10); 31 | } 32 | #else 33 | _socket.shutdown(); 34 | #ifdef __APPLE__ 35 | pthread_cancel(_thread); 36 | #endif 37 | pthread_join(_thread, NULL); 38 | #endif 39 | delete [] _packet.data; 40 | } 41 | 42 | #ifdef _WIN32 43 | DWORD WINAPI MavlinkNode::RxThread(LPVOID param) 44 | #else 45 | void* MavlinkNode::RxThread(void* param) 46 | #endif 47 | { 48 | int numRead; 49 | string srcAddr; 50 | unsigned short srcPort; 51 | 52 | MavlinkNode* p = (MavlinkNode*)param; 53 | while (p->_running) 54 | { 55 | try 56 | { 57 | numRead = p->_socket.recvFrom(p->_packet.data, MAX_UDP_PACKET_SIZE, srcAddr, srcPort); 58 | } 59 | catch (...) 60 | { 61 | continue; 62 | } 63 | 64 | if(numRead<0) 65 | { 66 | // error 67 | } 68 | else 69 | { 70 | p->_packet.len = numRead; 71 | p->UDPPacketCallback(p->_packet); 72 | } 73 | } 74 | 75 | return 0; 76 | } 77 | 78 | void MavlinkNode::Send(const vector& packet) 79 | { 80 | _socket.sendTo(&packet[0], (int)packet.size(), "127.0.0.1", MAVLINK_TX_PORT); 81 | } 82 | 83 | void MavlinkNode::UDPPacketCallback(UDPPacket& m) 84 | { 85 | // TODO 86 | 87 | mavlink_message_t msg; 88 | mavlink_status_t status; 89 | 90 | //printf("Bytes Received: %d\n", (int)m.len); 91 | for (unsigned int i = 0; i < m.len; ++i) 92 | { 93 | if (mavlink_parse_char(MAVLINK_COMM_0, m.data[i], &msg, &status)) 94 | { 95 | // Packet received 96 | //printf("Received packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); 97 | } 98 | } 99 | 100 | /*if(!callback.empty()){ 101 | callback(ret,callbackArg); 102 | }*/ 103 | } 104 | -------------------------------------------------------------------------------- /src/MavlinkNode/MavlinkNode.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "PracticalSocket.h" 4 | #include "Utility/FastDelegate.h" 5 | #include "UDPPacket.h" 6 | 7 | #ifdef __APPLE__ 8 | #pragma clang diagnostic push 9 | #pragma clang diagnostic ignored "-Waddress-of-packed-member" 10 | #endif 11 | #include "mavlink/common/mavlink.h" 12 | #ifdef __APPLE__ 13 | #pragma clang diagnostic pop 14 | #endif 15 | 16 | #include 17 | using namespace fastdelegate; 18 | using std::vector; 19 | 20 | #define MAVLINK_TX_PORT 14555 21 | #define MAVLINK_RX_PORT 14550 22 | 23 | typedef FastDelegate2 MavlinkNodeCallback; 24 | 25 | class MavlinkNode 26 | { 27 | public: 28 | MavlinkNode(string myIP="127.0.0.1"); 29 | ~MavlinkNode(); 30 | 31 | #ifdef _WIN32 32 | static DWORD WINAPI RxThread(LPVOID param); 33 | #else 34 | static void* RxThread(void* param); 35 | #endif 36 | 37 | void SetCallback(MavlinkNodeCallback callback, void* arg) 38 | { 39 | this->callback = callback; 40 | this->callbackArg = arg; 41 | } 42 | 43 | void ClearCallback() 44 | { 45 | this->callback.clear(); 46 | } 47 | 48 | void Send(const vector& packet); 49 | 50 | private: 51 | void UDPPacketCallback(UDPPacket& m); 52 | 53 | MavlinkNodeCallback callback; 54 | void* callbackArg; 55 | 56 | unsigned short _lastSeqNum; 57 | bool _first; 58 | unsigned int _doubleCnt; 59 | 60 | UDPSocket _socket; 61 | #ifdef _WIN32 62 | HANDLE _thread; 63 | #else 64 | pthread_t _thread; 65 | #endif 66 | UDPPacket _packet; 67 | bool _running; 68 | }; 69 | -------------------------------------------------------------------------------- /src/MavlinkNode/MavlinkTranslation.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "Math/Quaternion.h" 3 | #include 4 | using namespace std; 5 | 6 | #ifdef __APPLE__ 7 | #pragma clang diagnostic push 8 | #pragma clang diagnostic ignored "-Waddress-of-packed-member" 9 | #endif 10 | #include "mavlink/common/mavlink.h" 11 | #ifdef __APPLE__ 12 | #pragma clang diagnostic pop 13 | #endif 14 | 15 | vector MakeMavlinkPacket_LocalPose(float simTime, V3F pos, V3F vel) 16 | { 17 | vector ret; 18 | ret.resize(MAVLINK_MAX_PACKET_LEN); 19 | mavlink_message_t msg; 20 | 21 | mavlink_msg_local_position_ned_pack(1, 200, &msg, (int)(simTime*1e6f), 22 | pos[0], pos[1], pos[2], 23 | vel[0], vel[1], vel[2]); 24 | 25 | int len = mavlink_msg_to_send_buffer(&ret[0], &msg); 26 | ret.resize(len); 27 | return ret; 28 | } 29 | 30 | vector MakeMavlinkPacket_Heartbeat() 31 | { 32 | vector ret; 33 | ret.resize(MAVLINK_MAX_PACKET_LEN); 34 | mavlink_message_t msg; 35 | 36 | mavlink_msg_heartbeat_pack(1, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE); 37 | 38 | int len = mavlink_msg_to_send_buffer(&ret[0], &msg); 39 | ret.resize(len); 40 | return ret; 41 | } 42 | 43 | vector MakeMavlinkPacket_Status() 44 | { 45 | vector ret; 46 | ret.resize(MAVLINK_MAX_PACKET_LEN); 47 | mavlink_message_t msg; 48 | 49 | /* Send Status */ 50 | mavlink_msg_sys_status_pack(1, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0); 51 | 52 | int len = mavlink_msg_to_send_buffer(&ret[0], &msg); 53 | ret.resize(len); 54 | return ret; 55 | } 56 | 57 | vector MakeMavlinkPacket_Attitude(float simTime, SLR::Quaternion attitude, V3F omega) 58 | { 59 | vector ret; 60 | ret.resize(MAVLINK_MAX_PACKET_LEN); 61 | mavlink_message_t msg; 62 | 63 | mavlink_msg_attitude_pack(1, MAV_COMP_ID_AUTOPILOT1, &msg, (int)(simTime*1e6f), attitude.Roll(), attitude.Pitch(), attitude.Yaw(), omega.x, omega.y, omega.z); 64 | 65 | int len = mavlink_msg_to_send_buffer(&ret[0], &msg); 66 | ret.resize(len); 67 | return ret; 68 | } 69 | 70 | -------------------------------------------------------------------------------- /src/MavlinkNode/MavlinkTranslation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | vector MakeMavlinkPacket_LocalPose(float simTime, V3F pos, V3F vel); 4 | vector MakeMavlinkPacket_Heartbeat(); 5 | vector MakeMavlinkPacket_Status(); 6 | vector MakeMavlinkPacket_Attitude(float simTime, Quaternion attitude, V3F omega); -------------------------------------------------------------------------------- /src/MavlinkNode/UDPPacket.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_PACKET_H_FEB_27_2008_SVL5 2 | #define UDP_PACKET_H_FEB_27_2008_SVL5 3 | 4 | #define MAX_UDP_PACKET_SIZE 65467 // UDP protocol max message size 5 | 6 | struct UDPPacket { 7 | unsigned short port; 8 | int source_addr; 9 | int dest_addr; 10 | 11 | unsigned int len; 12 | unsigned char* data; 13 | }; 14 | 15 | #endif //UDP_PACKET_H_FEB_27_2008_SVL5 16 | 17 | -------------------------------------------------------------------------------- /src/QuadControl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | #include "Utility/StringUtils.h" 5 | #include "Trajectory.h" 6 | #include "BaseController.h" 7 | 8 | class QuadControl : public BaseController 9 | { 10 | public: 11 | QuadControl(string config) : BaseController(config) { Init(); }; 12 | 13 | virtual void Init(); 14 | 15 | // returns a desired acceleration in global frame 16 | V3F LateralPositionControl(V3F posCmd, V3F velCmd, V3F pos, V3F vel, V3F accelCmd); 17 | 18 | virtual VehicleCommand RunControl(float dt, float sim_time); 19 | 20 | VehicleCommand GenerateMotorCommands(float collThrustCmd, V3F momentCmd); 21 | 22 | // returns desired yaw rate 23 | float YawControl(float yawCmd, float yaw); 24 | 25 | // returns desired moments 26 | V3F BodyRateControl(V3F pqrCmd, V3F pqr); 27 | 28 | // returns a desired roll and pitch rate 29 | V3F RollPitchControl(V3F accelCmd, Quaternion attitude, float collThrustCmd); 30 | 31 | float AltitudeControl(float posZCmd, float velZCmd, float posZ, float velZ, Quaternion attitude, float accelZCmd, float dt); 32 | 33 | // -------------- PARAMETERS -------------- 34 | 35 | // controller gains 36 | float kpPosXY, kpPosZ; 37 | float kpVelXY, kpVelZ; 38 | float kpBank, kpYaw; 39 | float KiPosZ; 40 | V3F kpPQR; 41 | 42 | // limits & saturations 43 | float maxAscentRate, maxDescentRate; 44 | float maxSpeedXY; 45 | float maxAccelXY; 46 | float maxTiltAngle; 47 | float minMotorThrust, maxMotorThrust; 48 | 49 | // integral control 50 | float integratedAltitudeError; 51 | }; 52 | -------------------------------------------------------------------------------- /src/Simulation/BaseDynamics.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "BaseDynamics.h" 3 | #include "Math/Random.h" 4 | #include "Utility/SimpleConfig.h" 5 | #include "Utility/StringUtils.h" 6 | 7 | #ifdef _MSC_VER // visual studio 8 | #pragma warning(disable: 4267 4244 4996) 9 | #endif 10 | 11 | using namespace SLR; 12 | 13 | BaseDynamics::BaseDynamics(string name) 14 | { 15 | _name = name; 16 | _initialized = false; 17 | Initialize(); 18 | } 19 | 20 | int BaseDynamics::Initialize() 21 | { 22 | ParamsHandle config = SimpleConfig::GetInstance(); 23 | 24 | _initialized = false; 25 | 26 | _vehicleType = -1; // see BaseDynamics.h for list of numbers 27 | 28 | // load in BaseDynamics-specific double-valued settings from the config in your inheritor 29 | xMin = config->Get("Sim.xMin", -10.f); 30 | yMin = config->Get("Sim.yMin", -10.f);; 31 | xMax = config->Get("Sim.xMax", 10.f); 32 | yMax = config->Get("Sim.yMax", 10.f); 33 | bottom = config->Get("Sim.bottom", 0.f); 34 | top = config->Get("Sim.top", 10.f); 35 | 36 | return 1; 37 | } 38 | 39 | void BaseDynamics::ResetState(V3F newPos, V3F newVel, Quaternion newAtt, V3F newOmega) 40 | { 41 | omega = newOmega; 42 | pos = newPos; 43 | vel = newVel; 44 | quat = newAtt; 45 | 46 | } 47 | 48 | void BaseDynamics::SyncToVicon(GlobalPose gp) 49 | { 50 | vel = V3F(); 51 | omega = V3F(); 52 | pos = gp.pos; 53 | quat = gp.q; 54 | 55 | printf("\nSIMULATOR_RESET ncommand received; set sim pose = vicon, and sim vel = 0.\n"); 56 | } 57 | 58 | GlobalPose BaseDynamics::GenerateGP(void) 59 | { 60 | GlobalPose p; 61 | p.pos = pos; 62 | p.q = quat; 63 | return p; 64 | } 65 | 66 | bool BaseDynamics::GetData(const string& name, float& ret) const 67 | { 68 | if (name.find_first_of(".") == string::npos) return false; 69 | string leftPart = LeftOf(name, '.'); 70 | string rightPart = RightOf(name, '.'); 71 | if (ToUpper(leftPart) == ToUpper(_name)) 72 | { 73 | #define GETTER_HELPER(A,B) if (SLR::ToUpper(rightPart) == SLR::ToUpper(A)){ ret=(B); return true; } 74 | GETTER_HELPER("POS.X", pos.x); 75 | GETTER_HELPER("POS.Y", pos.y); 76 | GETTER_HELPER("POS.Z", pos.z); 77 | GETTER_HELPER("VEL.X", vel.x); 78 | GETTER_HELPER("VEL.Y", vel.y); 79 | GETTER_HELPER("VEL.Z", vel.z); 80 | GETTER_HELPER("YAW", quat.ToEulerYPR()[0]); 81 | GETTER_HELPER("PITCH", quat.ToEulerYPR()[1]); 82 | GETTER_HELPER("ROLL", quat.ToEulerYPR()[2]); 83 | GETTER_HELPER("OMEGA.X", omega.x); 84 | GETTER_HELPER("OMEGA.Y", omega.y); 85 | GETTER_HELPER("OMEGA.Z", omega.z); 86 | #undef GETTER_HELPER 87 | } 88 | return false; 89 | } 90 | 91 | vector BaseDynamics::GetFields() const 92 | { 93 | vector ret; 94 | ret.push_back(_name + ".Pos.X"); 95 | ret.push_back(_name + ".Pos.Y"); 96 | ret.push_back(_name + ".Pos.Z"); 97 | ret.push_back(_name + ".Vel.X"); 98 | ret.push_back(_name + ".Vel.Y"); 99 | ret.push_back(_name + ".Vel.Z"); 100 | ret.push_back(_name + ".Yaw"); 101 | ret.push_back(_name + ".Pitch"); 102 | ret.push_back(_name + ".Roll"); 103 | ret.push_back(_name + ".Omega.X"); 104 | ret.push_back(_name + ".Omega.Y"); 105 | ret.push_back(_name + ".Omega.Z"); 106 | return ret; 107 | } 108 | -------------------------------------------------------------------------------- /src/Simulation/BaseDynamics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Common.h" 4 | #include "Math/Quaternion.h" 5 | #include "VehicleDatatypes.h" 6 | #include "DataSource.h" 7 | 8 | #define VEHICLE_TYPE_QUAD 0 9 | 10 | #ifdef _MSC_VER 11 | #pragma warning(push) 12 | #pragma warning(disable: 4100) // unreferenced formal parameter 13 | #endif 14 | 15 | using SLR::Quaternion; 16 | class Trajectory; 17 | 18 | class BaseDynamics : public DataSource 19 | { 20 | public: 21 | BaseDynamics(string name=""); 22 | virtual ~BaseDynamics() {}; // destructor 23 | 24 | virtual int Initialize(); 25 | 26 | virtual void Run(float dt, float simulationTime, int &idum, // updates the simulation 27 | V3F externalForceInGlobalFrame = V3F(), // required to take net forces into account 28 | V3F externalMomentInBodyFrame = V3F()) // required to take net moments into account 29 | {} 30 | 31 | virtual void SetCommands(const VehicleCommand& cmd) {}; // update commands in the simulator coming from a command2 packet 32 | 33 | // inheritors have no reason to alter the following functions and therefore no sense demanding that they do 34 | GlobalPose GenerateGP () ; // returns the current simulation state in Vicon format - const? 35 | void SyncToVicon(GlobalPose gp); // sets simulation to zero velocities and vicon-based poses. 36 | 37 | V3F Position() const { return pos; }; 38 | V3F Velocity() const { return vel; }; 39 | V3F Omega() const { return omega; }; 40 | Quaternion Attitude() const { return quat; } 41 | 42 | void SetPosition(const V3F& p) { pos = p; } 43 | void SetVelocity(const V3F& v) { vel = v; } 44 | void SetOmega(const V3F& o) { omega = o; } 45 | void SetAttitude(const Quaternion& q){quat = q;} 46 | 47 | virtual bool GetData(const string& name, float& ret) const; 48 | virtual vector GetFields() const; 49 | 50 | int GetVehicleType(void) {return _vehicleType;}; 51 | 52 | virtual double GetRotDistInt() { return 0;}; 53 | virtual double GetXyzDistInt() {return 0;}; 54 | virtual double GetRotDistBW() {return 0;}; 55 | virtual double GetXyzDistBW() {return 0;}; 56 | virtual double GetGyroNoiseInt() {return 0;}; 57 | 58 | bool Initialized() const {return _initialized;} 59 | 60 | void ResetState(V3F pos=V3F(), V3F vel=V3F(), Quaternion att=Quaternion(), V3F omega=V3F()); 61 | 62 | shared_ptr _followed_traj; 63 | 64 | protected: 65 | string _name; 66 | 67 | // pos, vel, acc are in the global frame while omega is in the local frame (body rates) 68 | V3F pos, vel, acc, omega, old_omega; 69 | Quaternion quat; 70 | 71 | int _vehicleType; 72 | 73 | // vehicle geometry and mass properties 74 | float M; // veh mass, kg 75 | float Ixx,Iyy,Izz; 76 | float xMin,yMin,bottom,xMax,yMax,top; 77 | bool _initialized; 78 | 79 | float _lastTrajPointTime; 80 | float _trajLogStepTime; 81 | }; 82 | 83 | #ifdef _MSC_VER 84 | #pragma warning(pop) 85 | #endif 86 | -------------------------------------------------------------------------------- /src/Simulation/QuadDynamics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "BaseDynamics.h" 4 | #include "Math/V4D.h" 5 | #include "BaseController.h" 6 | #include 7 | #include "Math/LowPassFilter.h" 8 | #include "Drawing/ColorUtils.h" 9 | #include "Utility/FastDelegate.h" 10 | using namespace fastdelegate; 11 | 12 | class QuadDynamics; 13 | typedef shared_ptr QuadcopterHandle; 14 | 15 | class QuadDynamics : public BaseDynamics 16 | { 17 | public: 18 | static QuadcopterHandle Create(string name, int cnt=0) 19 | { 20 | QuadcopterHandle ret(new QuadDynamics(name)); 21 | float hue = (float)cnt*30.f; 22 | ret->color = HSVtoRGB(hue + 15.f, 1, 1); 23 | return ret; 24 | } 25 | 26 | QuadDynamics(string name=""); 27 | virtual ~QuadDynamics() {}; // destructor 28 | virtual int Initialize(); 29 | 30 | virtual void Run(float dt, float simulationTime, int &idum, // updates the simulation 31 | V3F externalForceInGlobalFrame = V3F(), // required to take net forces into account 32 | V3F externalMomentInBodyFrame = V3F()); // required to take net moments into account 33 | 34 | virtual void SetCommands(const VehicleCommand& cmd); // update commands in the simulator coming from a command2 packet 35 | 36 | virtual void Dynamics(float dt, float simTime, V3F external_force, V3F external_moment, int& idum); 37 | 38 | double GetRotDistInt() {return rotDisturbanceInt;}; 39 | double GetXyzDistInt() {return xyzDisturbanceInt;}; 40 | double GetRotDistBW() {return rotDisturbanceBW;}; 41 | double GetXyzDistBW() {return xyzDisturbanceBW;}; 42 | double GetGyroNoiseInt() {return gyroNoiseInt;}; 43 | 44 | virtual bool GetData(const string& name, float& ret) const; 45 | virtual vector GetFields() const; 46 | 47 | void Reset(); 48 | 49 | VehicleCommand GetCommands() const { return curCmd; } 50 | 51 | void ResetState(V3F pos=V3F(), V3F vel=V3F(), Quaternion att=Quaternion(), V3F omega=V3F()); 52 | void SetPosVelAttOmega(V3F pos=V3F(), V3F vel=V3F(), Quaternion att=Quaternion(), V3F omega=V3F()); 53 | 54 | void TurnOffNonidealities(); 55 | 56 | void RunRoomConstraints(const V3F& oldPos); 57 | 58 | VehicleCommand curCmd; 59 | 60 | FastDelegate4, V3F> updateIdealStateCallback; 61 | 62 | 63 | FastDelegate1 followedTrajectoryCallback; 64 | 65 | float GetArmLength() const { return L; } 66 | 67 | ControllerHandle controller; 68 | 69 | friend class Visualizer_GLUT; 70 | 71 | protected: 72 | matrix::Vector motorCmdsN; 73 | matrix::Vector motorCmdsOld; 74 | 75 | // useful matrices/vectors that are recomputed from the state at each timestep 76 | double YPR[3]; 77 | 78 | ////////////////////////////////////////////////////////////////// 79 | // vehicle geometry and mass properties 80 | float cx; 81 | float cy; 82 | float L; // distance from body z axis to the prop 83 | 84 | // properties of the prop/motor system as modeled 85 | double tauaUp, tauaDown; // time constant 86 | float kappa; // Nm drag per N lift 87 | float minMotorThrust, maxMotorThrust; 88 | 89 | double xyzDisturbanceInt, xyzDisturbanceBW, rotDisturbanceInt, rotDisturbanceBW, gyroNoiseInt; 90 | V3D xyzDisturbance, rotDisturbance; 91 | 92 | float randomMotorForceMag; 93 | 94 | double controllerUpdateInterval, timeSinceLastControllerUpdate; 95 | 96 | V3F _rawGyro; 97 | float _lastPosFollowErr; 98 | 99 | V3F color; 100 | string _flightMode; 101 | }; 102 | -------------------------------------------------------------------------------- /src/Simulation/Simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "Simulator.h" 3 | #include "Utility/SimpleConfig.h" 4 | #include "Simulation/BaseDynamics.h" 5 | using namespace SLR; 6 | 7 | Simulator::Simulator() 8 | { 9 | Reset(); 10 | } 11 | 12 | void Simulator::Reset() 13 | { 14 | ParamsHandle config = SimpleConfig::GetInstance(); 15 | 16 | // TODO: parameters 17 | 18 | for (unsigned int i = 0; i < _vehicles.size(); i++) 19 | { 20 | _vehicles[i]->Initialize(); 21 | } 22 | } 23 | 24 | 25 | void Simulator::Run(float dt) 26 | { 27 | // todo: step in maximally acceptable time steps 28 | } 29 | 30 | void Simulator::AddVehicle(shared_ptr vehicle) 31 | { 32 | _vehicles.push_back(vehicle); 33 | } 34 | -------------------------------------------------------------------------------- /src/Simulation/Simulator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | using namespace std; 6 | 7 | class BaseDynamics; 8 | 9 | class Simulator 10 | { 11 | public: 12 | Simulator(); 13 | 14 | void Reset(); 15 | 16 | void Run(float dt); 17 | 18 | void AddVehicle(shared_ptr vehicle); 19 | 20 | vector > _vehicles; 21 | float _simTime; 22 | }; -------------------------------------------------------------------------------- /src/Simulation/magnetometer.cpp: -------------------------------------------------------------------------------- 1 | #include "magnetometer.h" 2 | 3 | void magnetometer::magnetometer_sensor(float declination, SLR::Quaternion attitude, V3F &mag_measurement){ 4 | mag = V3F(0.215212f,0.0f,-0.42741f); // Values taken from WMM 2015, NED Frame, order 10^5 nT // maybe a recheck 5 | SLR::Quaternion declination_quat; 6 | declination_quat = SLR::Quaternion::FromEulerYPR(-declination,0.0f,0.0f); 7 | mag_measurement = declination_quat.Rotate_ItoB(mag); 8 | mag_measurement = attitude.Rotate_ItoB(mag_measurement); 9 | 10 | std::random_device rd; 11 | std::mt19937 gen(rd()); 12 | 13 | std::normal_distribution<> fx{0,fx_stddev}; 14 | std::normal_distribution<> fy{0,fy_stddev}; 15 | std::normal_distribution<> fz{0,fz_stddev}; 16 | 17 | float fx_sample = fx(gen); 18 | float fy_sample = fy(gen); 19 | float fz_sample = fz(gen); 20 | 21 | mag_measurement.x += fx_sample; 22 | mag_measurement.y += fy_sample; 23 | mag_measurement.z += fz_sample; 24 | 25 | } 26 | -------------------------------------------------------------------------------- /src/Simulation/magnetometer.h: -------------------------------------------------------------------------------- 1 | #ifndef MAGNETOMETER_H 2 | #define MAGNETOMETER_H 3 | 4 | #include 5 | #include "Common.h" 6 | #include "Math/Random.h" 7 | #include "Math/Quaternion.h" 8 | #include "matrix/math.hpp" 9 | #include 10 | 11 | class magnetometer 12 | { 13 | public: 14 | V3F mag; 15 | float fx_stddev = 0.0001f; 16 | float fy_stddev = 0.0001f; 17 | float fz_stddev = 0.0001f; 18 | void magnetometer_sensor(float declination, SLR::Quaternion attitude, V3F &mag_measurement); 19 | }; 20 | 21 | #endif // MAGNETOMETER_H 22 | -------------------------------------------------------------------------------- /src/Simulation/opticalflow.cpp: -------------------------------------------------------------------------------- 1 | #include "opticalflow.h" 2 | 3 | void opticalflow::opticalflow_sensor(float dt, V3F position, V3F velocity, SLR::Quaternion attitude, V3F omega, float &x_measurement, float &y_measurement){ 4 | x_measurement = (dt*N_pixel/aperture) * ((attitude.Rotate_ItoB(velocity).x / attitude.Rotate_ItoB(position).z) + omega_factor * omega.y); 5 | y_measurement = (dt*N_pixel/aperture) * ((attitude.Rotate_ItoB(velocity).y / attitude.Rotate_ItoB(position).z) - omega_factor * omega.x); 6 | 7 | // The signs used for omega above need to be checked again - not sure which frame is used by crazyflie 8 | 9 | std::random_device rd; 10 | std::mt19937 gen(rd()); 11 | 12 | std::normal_distribution<> fx{0,fx_stddev}; 13 | std::normal_distribution<> fy{0,fy_stddev}; 14 | 15 | float fx_sample = fx(gen); 16 | float fy_sample = fy(gen); 17 | 18 | x_measurement += fx_sample; 19 | y_measurement += fy_sample; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /src/Simulation/opticalflow.h: -------------------------------------------------------------------------------- 1 | #ifndef OPTICALFLOW_H 2 | #define OPTICALFLOW_H 3 | 4 | #include "Common.h" 5 | #include "Math/Random.h" 6 | #include "Math/Quaternion.h" 7 | #include "matrix/math.hpp" 8 | #include 9 | 10 | class opticalflow 11 | { 12 | public: 13 | float omega_factor = 1.25f; // Taken directly from Michael Hamer's code on estimation on crazyflie 14 | float N_pixel = 30.0f; 15 | float aperture = 4.2f * M_PI / 180.f; 16 | float fx_stddev = 0.0001f; 17 | float fy_stddev = 0.0001f; 18 | void opticalflow_sensor(float dt, V3F position, V3F velocity, SLR::Quaternion attitude, V3F omega, float &x_measurement, float &y_measurement); 19 | 20 | }; 21 | 22 | #endif // OPTICALFLOW_H 23 | -------------------------------------------------------------------------------- /src/Simulation/rangefinder.cpp: -------------------------------------------------------------------------------- 1 | #include "rangefinder.h" 2 | 3 | // Assumption: The distance sensor is located at the centre of the crazyflie, implying that the yaw motion does not affect the measurements 4 | 5 | void rangefinder::range_sensor(V3F position, SLR::Quaternion attitude, float &measurement){ 6 | 7 | V3D ypr = attitude.ToEulerYPR(); 8 | measurement = position.z/(cos(ypr.y)*cos(ypr.z)); 9 | 10 | // Now adding the normal noise 11 | std::random_device rd; 12 | std::mt19937 gen(rd()); 13 | std::normal_distribution<> fd{0, fd_stddev}; 14 | 15 | float fd_sample = fd(gen); 16 | 17 | measurement += fd_sample; 18 | } 19 | -------------------------------------------------------------------------------- /src/Simulation/rangefinder.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGEFINDER_H 2 | #define RANGEFINDER_H 3 | 4 | #include "Common.h" 5 | #include "Math/Random.h" 6 | #include "Math/Quaternion.h" 7 | #include "matrix/math.hpp" 8 | #include 9 | 10 | class rangefinder 11 | { 12 | public: 13 | float fd_stddev = 0.0001f; 14 | void range_sensor(V3F position, SLR::Quaternion attitude, float &measurement); 15 | }; 16 | 17 | #endif // RANGEFINDER_H 18 | -------------------------------------------------------------------------------- /src/Trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "Trajectory.h" 3 | #include "Utility/SimpleConfig.h" 4 | #include "Utility/StringUtils.h" 5 | 6 | using namespace SLR; 7 | 8 | Trajectory::Trajectory() : traj(MAX_TRAJECTORY_POINTS) 9 | { 10 | _log_file = NULL; 11 | } 12 | 13 | Trajectory::Trajectory(const string& filename) : traj(MAX_TRAJECTORY_POINTS) 14 | { 15 | ReadFile(filename); 16 | } 17 | 18 | Trajectory::~Trajectory() 19 | { 20 | // Close any open logging files 21 | if (_log_file) 22 | { 23 | fclose(_log_file); 24 | } 25 | } 26 | 27 | bool Trajectory::ReadFile(const string& filename) 28 | { 29 | traj.reset(); 30 | 31 | FILE* f = fopen(filename.c_str(), "r"); 32 | if (!f) 33 | { 34 | return false; 35 | } 36 | 37 | char buf[512]; 38 | buf[511] = 0; // null char 39 | 40 | // read line by line... 41 | while (fgets(buf, 510, f)) 42 | { 43 | string s(buf); 44 | 45 | ParseLine(filename, s); 46 | } 47 | 48 | fclose(f); 49 | 50 | // Handle empty trajectory files 51 | // check the length of the trajectory vector 52 | // if there are no points in the trajectory file, then use the initial position as the only trajectory point 53 | if (traj.n_meas() == 0) 54 | { 55 | ParamsHandle config = SimpleConfig::GetInstance(); 56 | TrajectoryPoint traj_pt; 57 | // TODO: no quad naming here. 58 | traj_pt.position = config->Get("Quad.InitialPos", V3F()); 59 | traj_pt.velocity = config->Get("Quad.InitialVel", V3F()); 60 | traj_pt.omega = config->Get("Quad.InitialOmega", V3F()); 61 | V3F ypr = config->Get("Quad.InitialYPR", V3F()); 62 | traj_pt.attitude = Quaternion::FromEulerYPR(ypr[0], ypr[1], ypr[2]); 63 | 64 | traj.push(traj_pt); 65 | } 66 | 67 | return true; 68 | } 69 | 70 | void Trajectory::ParseLine(const string& filename, const string& s) 71 | { 72 | std::size_t firstNonWS = s.find_first_not_of("\n\t "); 73 | 74 | // Ignore comments 75 | if (firstNonWS == std::string::npos || s[firstNonWS] == '#' || firstNonWS == '/') 76 | { 77 | return; 78 | } 79 | 80 | TrajectoryPoint traj_pt; 81 | 82 | V3F ypr; // Helper variable to read in yaw, pitch and roll 83 | sscanf(s.c_str(), "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", &traj_pt.time, &traj_pt.position.x, &traj_pt.position.y, &traj_pt.position.z, &traj_pt.velocity.x, &traj_pt.velocity.y, &traj_pt.velocity.z, &ypr[0], &ypr[1], &ypr[2], &traj_pt.omega.x, &traj_pt.omega.y, &traj_pt.omega.z); 84 | 85 | // Convert yaw, pitch, and roll to an attitude quaternion 86 | traj_pt.attitude = Quaternion::FromEulerYPR(ypr[0], ypr[1], ypr[2]); 87 | 88 | // Add the trajectory point to the vector of all trajectory points 89 | traj.push(traj_pt); 90 | } 91 | 92 | void Trajectory::Clear() 93 | { 94 | _curTrajPoint = 0; 95 | traj.reset(); 96 | 97 | // close and reopen the log file 98 | if (_log_file) 99 | { 100 | fclose(_log_file); 101 | _log_file = nullptr; 102 | } 103 | 104 | if (!_log_filename.empty()) 105 | { 106 | _log_file = fopen(_log_filename.c_str(), "w"); 107 | } 108 | } 109 | 110 | void Trajectory::SetLogFile(const string& filename) 111 | { 112 | _log_filename = filename; 113 | 114 | // Close any file that might have been open and open the new file 115 | if (_log_file) 116 | { 117 | fclose(_log_file); 118 | 119 | if (_log_filename != "") 120 | { 121 | _log_file = fopen(_log_filename.c_str(), "w"); 122 | } 123 | } 124 | } 125 | 126 | void Trajectory::AddTrajectoryPoint(TrajectoryPoint traj_pt) 127 | { 128 | traj.push(traj_pt); 129 | 130 | // If there is a log file, write the point to file 131 | if (_log_file) 132 | { 133 | WriteTrajectoryPointToFile(_log_file, traj_pt); 134 | } 135 | } 136 | 137 | TrajectoryPoint Trajectory::NextTrajectoryPoint(float time) 138 | { 139 | if (traj.empty()) return TrajectoryPoint(); 140 | 141 | // Loop through the trajectory vector and get the next trajectory point 142 | for (int i = traj.n_meas()-1; i >= 0; i--) 143 | { 144 | if(traj.at(i).time < time) 145 | { 146 | _curTrajPoint = i; 147 | return traj.at(i); 148 | } 149 | } 150 | 151 | _curTrajPoint = traj.n_meas() - 1; 152 | // I should not get here 153 | return traj.newest(); 154 | } 155 | 156 | void Trajectory::WriteTrajectoryPointToFile(FILE* f, TrajectoryPoint traj_pt) 157 | { 158 | if (!f) 159 | { 160 | return; 161 | } 162 | 163 | // Write the trajectory point to file 164 | V3D ypr = traj_pt.attitude.ToEulerYPR(); 165 | fprintf (f, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n", (double)traj_pt.time, (double)traj_pt.position.x, (double)traj_pt.position.y, (double)traj_pt.position.z, (double)traj_pt.velocity.x, (double)traj_pt.velocity.y, (double)traj_pt.velocity.z, ypr[0], ypr[1], ypr[2], (double)traj_pt.omega.x, (double)traj_pt.omega.y, (double)traj_pt.omega.z); 166 | 167 | // Flush to file 168 | fflush(f); 169 | } 170 | -------------------------------------------------------------------------------- /src/Trajectory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Math/Quaternion.h" 4 | #include "Utility/FixedQueue.h" 5 | #include "VehicleDatatypes.h" 6 | #include 7 | 8 | using namespace SLR; 9 | 10 | #define MAX_TRAJECTORY_POINTS 10000 11 | 12 | class Trajectory { 13 | public: 14 | Trajectory(); 15 | Trajectory(const string& filename); 16 | ~Trajectory(); 17 | bool ReadFile(const string& filename); 18 | void ParseLine(const string& filename, const string& s); 19 | void Clear(); 20 | void SetLogFile(const string& filename); 21 | void AddTrajectoryPoint(TrajectoryPoint traj_pt); 22 | TrajectoryPoint NextTrajectoryPoint(float time); 23 | void WriteTrajectoryPointToFile(FILE* f, TrajectoryPoint traj_pt); 24 | 25 | FixedQueue traj; // vector containing the trajectory points 26 | 27 | int GetCurTrajectoryPoint() const { return _curTrajPoint; } 28 | private: 29 | string _log_filename; 30 | FILE* _log_file; 31 | int _curTrajPoint; 32 | }; 33 | -------------------------------------------------------------------------------- /src/Utility/Camera.cpp: -------------------------------------------------------------------------------- 1 | #include "Camera.h" 2 | #include "../Math/MathUtils.h" 3 | 4 | #ifndef __APPLE__ 5 | #include 6 | #include 7 | #include 8 | #include 9 | #else 10 | #include 11 | #endif 12 | 13 | #define PI 3.1415926535897932384626433832795 14 | #define PIdiv180 (PI/180.0) 15 | 16 | using SLR::ScopedMutexLock; 17 | 18 | Camera::Camera(V3D pos, V3D lookat) 19 | { 20 | //Init with standard OGL values: 21 | _lookat = lookat; 22 | _pos = pos; 23 | _up = V3D (0.0, 0.0, 1.0); 24 | 25 | Reset(); 26 | 27 | _lookatFiltered.Reset(_lookat); 28 | _posFiltered.Reset(_pos); 29 | _upFiltered.Reset(_up); 30 | } 31 | 32 | void Camera::Reset() 33 | { 34 | double tau=.1; 35 | _lookatFiltered.SetTau(tau); 36 | _posFiltered.SetTau(tau); 37 | _upFiltered.SetTau(tau); 38 | } 39 | 40 | void Camera::PanGlobal (V3D delta) 41 | { 42 | ScopedMutexLock sml(_mutex); 43 | _lookat += delta; 44 | _pos += delta; 45 | lastUserInteractionTimer.Reset(); 46 | } 47 | 48 | // positive == closer to subject 49 | void Camera::DollyIn(double delta) 50 | { 51 | ScopedMutexLock sml(_mutex); 52 | double r = _pos.dist(_lookat); 53 | r -= delta; 54 | r = CONSTRAIN(r,.1,50); 55 | _pos = _lookat + (_pos-_lookat).norm()*r; 56 | lastUserInteractionTimer.Reset(); 57 | } 58 | 59 | void Camera::SetYaw(double angleRads) 60 | { 61 | ScopedMutexLock sml(_mutex); 62 | double r = _pos.distXY(_lookat); 63 | _pos.x = _lookat.x + cos(angleRads)*r; 64 | _pos.y = _lookat.y + sin(angleRads)*r; 65 | } 66 | 67 | void Camera::YawAboutCenter(double angleRads) 68 | { 69 | ScopedMutexLock sml(_mutex); 70 | double curAngle = atan2(_pos.y-_lookat.y,_pos.x-_lookat.x); 71 | double r = _pos.distXY(_lookat); 72 | curAngle += angleRads; 73 | 74 | _pos.x = _lookat.x + cos(curAngle)*r; 75 | _pos.y = _lookat.y + sin(curAngle)*r; 76 | lastUserInteractionTimer.Reset(); 77 | } 78 | 79 | void Camera::TiltAboutCenter(double angleRads) 80 | { 81 | ScopedMutexLock sml(_mutex); 82 | double r = (_pos-_lookat).mag(); 83 | double curYaw = atan2(_pos.y-_lookat.y,_pos.x-_lookat.x); 84 | double curPitch = atan2(_pos.z-_lookat.z,_pos.distXY(_lookat)); 85 | 86 | curPitch += angleRads; 87 | curPitch = CONSTRAIN(curPitch,-M_PI/2.0+.001,M_PI/2.0-.001); 88 | 89 | _pos.x = _lookat.x + r*cos(curYaw)*cos(curPitch); 90 | _pos.y = _lookat.y + r*sin(curYaw)*cos(curPitch); 91 | _pos.z = _lookat.z + r*sin(curPitch); 92 | lastUserInteractionTimer.Reset(); 93 | } 94 | 95 | void Camera::PanLeft(double delta) 96 | { 97 | ScopedMutexLock sml(_mutex); 98 | V3D left = -((_lookat-_pos).cross(_up).norm()); 99 | _lookat += left*delta; 100 | _pos += left*delta; 101 | lastUserInteractionTimer.Reset(); 102 | } 103 | 104 | void Camera::PanUp(double delta) 105 | { 106 | ScopedMutexLock sml(_mutex); 107 | V3D right = ((_lookat-_pos).cross(_up).norm()); 108 | V3D realUp = right.cross(_lookat-_pos).norm(); 109 | _lookat += realUp*delta; 110 | _pos += realUp*delta; 111 | lastUserInteractionTimer.Reset(); 112 | } 113 | 114 | void Camera::TranslateViaLookAt(V3D la, bool showTargetBall) 115 | { 116 | ScopedMutexLock sml(_mutex); 117 | _pos = _pos - _lookat + la; 118 | _lookat = la; 119 | if(showTargetBall) 120 | { 121 | lastUserInteractionTimer.Reset(); 122 | } 123 | } 124 | 125 | void Camera::SetLookAt(V3D la) 126 | { 127 | ScopedMutexLock sml(_mutex); 128 | _lookat = la; 129 | } 130 | 131 | void Camera::DrawLookAtMarker(double size) 132 | { 133 | _mutex.lock(); 134 | V3D la = _lookat; 135 | _mutex.unlock(); 136 | 137 | glPushMatrix(); 138 | glTranslated(la.x,la.y,la.z); 139 | 140 | GLUquadricObj* q = gluNewQuadric(); 141 | gluSphere(q,size,16,6); 142 | gluDeleteQuadric(q); 143 | 144 | glPopMatrix(); 145 | } 146 | 147 | void Camera::SetView( double aspect, bool filtered) 148 | { 149 | _mutex.lock(); 150 | V3D lookat = filtered?_lookatFiltered.Read():_lookat; 151 | V3D pos = filtered?_posFiltered.Read():_pos; 152 | V3D up = filtered?_upFiltered.Read():_up; 153 | _mutex.unlock(); 154 | 155 | glMatrixMode(GL_PROJECTION); 156 | glLoadIdentity(); 157 | gluPerspective(50.0, aspect, .05, 200.0); 158 | 159 | glMatrixMode(GL_MODELVIEW); 160 | glPushMatrix(); 161 | glLoadIdentity(); 162 | 163 | //as we know the up vector, we can easily use gluLookAt: 164 | gluLookAt(pos.x,pos.y,pos.z,lookat.x,lookat.y,lookat.z,up.x,up.y,up.z); 165 | } 166 | 167 | void Camera::Update(double dt) 168 | { 169 | _lookatFiltered.Update(_lookat,dt); 170 | _posFiltered.Update(_pos,dt); 171 | _upFiltered.Update(_up,dt); 172 | } 173 | -------------------------------------------------------------------------------- /src/Utility/Camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../Common.h" 4 | 5 | /* 6 | http://studios.cla.umn.edu/tutorials/cameraterms.pdf 7 | */ 8 | #ifdef _WIN32 9 | #define WIN32_LEAN_AND_MEAN 10 | #include 11 | #pragma comment (lib, "opengl32.lib") 12 | #pragma comment (lib, "glu32.lib") 13 | #endif 14 | 15 | #include "../Utility/Mutex.h" 16 | #include "../Utility/Timer.h" 17 | #include "../Math/LowPassFilter.h" 18 | 19 | class Camera { 20 | public: 21 | SLR::Mutex _mutex; 22 | V3D _lookat, _pos, _up; 23 | LowPassFilter _lookatFiltered, _posFiltered, _upFiltered; 24 | 25 | Timer lastUserInteractionTimer; 26 | 27 | public: 28 | Camera(V3D pos, V3D lookat); 29 | void PanGlobal (V3D delta); 30 | 31 | // positive = "dolly in" = closer to subject 32 | // negative = "dolly out" = further away from subject 33 | void DollyIn(double delta); 34 | 35 | void SetYaw(double angleRad); 36 | void YawAboutCenter(double angleRad); 37 | void TiltAboutCenter(double angleRad); 38 | void SetView(double aspect, bool filtered=true); 39 | 40 | void PanLeft(double delta); 41 | void PanUp(double delta); 42 | 43 | void DrawLookAtMarker(double size); 44 | void TranslateViaLookAt(V3D newLookAt, bool showTargetBall=true); 45 | void SetLookAt(V3D newLookAt); 46 | void SetUp(V3D up) { _up = up; } 47 | 48 | void Update(double dt); 49 | V3D FilteredLookAt() const{return _lookatFiltered.Read();} 50 | V3D FilteredUp() const{return _upFiltered.Read();} 51 | V3D FilteredPos() const{return _posFiltered.Read();} 52 | 53 | void Reset(); 54 | 55 | V3D Pos() const { return _pos; } 56 | V3D Front() const { return (_lookat-_pos).norm(); } 57 | }; 58 | -------------------------------------------------------------------------------- /src/Utility/FixedQueue.h: -------------------------------------------------------------------------------- 1 | // Fixed-span circular FIFO buffer 2 | // 2003-2018 sergei lupashin 3 | // License: BSD-3-clause 4 | #pragma once 5 | 6 | #ifdef _MSC_VER // visual studio 7 | #pragma warning(push) 8 | #pragma warning(disable : 4244) 9 | #endif 10 | 11 | #include 12 | #include 13 | 14 | template 15 | class FixedQueue{ 16 | public: 17 | FixedQueue(unsigned int span, T failret=T()) 18 | :_span(span),_failret(failret) 19 | { 20 | assert(span>=1); 21 | _data = new T[_span+1]; 22 | assert(_data!=NULL); 23 | reset(); 24 | } 25 | 26 | FixedQueue(const FixedQueue& b) 27 | { 28 | _span = b._span; 29 | _data = new T[_span+1]; 30 | _failret = b._failret; 31 | reset(); 32 | if(b.empty()) return; 33 | /*for(unsigned int i=b.n_meas();i>0;i--) 34 | { 35 | push(b.at(i-1)); 36 | }*/ 37 | for(unsigned int i=0;i=0;i--) 60 | { 61 | push(b.at(i)); 62 | } 63 | } 64 | 65 | void push(const T& val) 66 | { 67 | assert(_end <= _span); 68 | _data[_end] = val; 69 | _end = (_end+1)%(_span+1); 70 | if(_begin==_end) 71 | _begin = (_begin+1)%(_span+1); 72 | } 73 | 74 | inline unsigned int n_meas() const 75 | { 76 | if(_begin==_end) return 0; 77 | if(_end<_begin) return (_span+1)-(_begin-_end); 78 | else return _end-_begin; 79 | } 80 | 81 | inline bool empty() const{ return _begin==_end; } 82 | inline bool full() const { return n_meas()==_span; } 83 | inline void reset(){ _begin = _end = 0; } 84 | 85 | // newest pushed data (bottom) 86 | T newest() const 87 | { 88 | if(_begin==_end) return _failret; 89 | return _data[(_end + _span)%(_span+1)]; 90 | } 91 | 92 | // oldest pushed data (top) 93 | T oldest() const 94 | { 95 | if(_begin==_end) return _failret; 96 | return _data[_begin]; 97 | } 98 | 99 | inline T pop_newest() 100 | { 101 | if(_begin==_end) return _failret; 102 | _end = (_end-1+_span+1)%(_span+1); 103 | return _data[_end]; 104 | } 105 | 106 | inline T pop_oldest() 107 | { 108 | if(_begin==_end) return _failret; 109 | unsigned int b = _begin; 110 | _begin = (_begin+1)%(_span+1); 111 | return _data[b]; 112 | } 113 | 114 | inline T pop_oldest(unsigned int cnt) 115 | { 116 | // TODO: this could be a lot more efficient 117 | T ret; 118 | for(unsigned int i=0;in_meas()) 143 | return _failret; 144 | i = (i+_begin)%(_span+1); 145 | assert(i <= _span); 146 | return _data[i]; 147 | } 148 | 149 | const T& operator[](unsigned int i) const 150 | { 151 | if((i+1)>n_meas()) 152 | return _failret; 153 | i = (i+_begin)%(_span+1); 154 | assert(i <= _span); 155 | return _data[i]; 156 | } 157 | 158 | const T& at(unsigned int i) const 159 | { 160 | if((i+1)>n_meas()) 161 | return _failret; 162 | i = (i+_begin)%(_span+1); 163 | assert(i <= _span); 164 | return _data[i]; 165 | } 166 | 167 | T& at(unsigned int i) 168 | { 169 | if((i+1)>n_meas()) 170 | return _failret; 171 | i = (i+_begin)%(_span+1); 172 | assert(i <= _span); 173 | return _data[i]; 174 | } 175 | 176 | protected: 177 | unsigned int _begin, _end; 178 | unsigned int _span; 179 | T *_data; 180 | T _failret; 181 | }; 182 | 183 | #ifdef _MSC_VER // visual studio 184 | #pragma warning(pop) 185 | #endif 186 | -------------------------------------------------------------------------------- /src/Utility/Mutex.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../Common.h" 4 | #ifdef _WIN32 5 | #include 6 | #else 7 | #include 8 | #include 9 | #endif 10 | 11 | namespace SLR{ 12 | 13 | class ScopedMutexLock; 14 | 15 | class Mutex{ 16 | friend class ScopedMutexLock; 17 | 18 | protected: 19 | #ifdef _WIN32 20 | // in win32 use critical sections, cause they're fast and simple 21 | CRITICAL_SECTION _cs; 22 | #else 23 | // in linux/pthread, use pthread_mutex.. 24 | pthread_mutex_t _mutex; 25 | #endif 26 | 27 | // copy constructor and assignment are disallowed 28 | Mutex(const Mutex&){}; 29 | Mutex& operator=(const Mutex&){return *this;} 30 | 31 | public: 32 | Mutex() 33 | { 34 | #ifdef _WIN32 35 | InitializeCriticalSection(&_cs); 36 | #else 37 | pthread_mutex_init(&_mutex, NULL); 38 | #endif 39 | } 40 | 41 | virtual ~Mutex() 42 | { 43 | #ifdef _WIN32 44 | DeleteCriticalSection(&_cs); 45 | #else 46 | pthread_mutex_destroy(&_mutex); 47 | #endif 48 | } 49 | 50 | inline bool TryLock() 51 | { 52 | #ifdef _WIN32 53 | return (TryEnterCriticalSection(&_cs)==TRUE); 54 | #else 55 | return (pthread_mutex_trylock(&_mutex) != EBUSY); 56 | #endif 57 | } 58 | 59 | inline void Lock(){lock();} 60 | 61 | inline void lock() 62 | { 63 | #ifdef _WIN32 64 | EnterCriticalSection(&_cs); 65 | #else 66 | pthread_mutex_lock(&_mutex); 67 | #endif 68 | } 69 | 70 | inline void Unlock(){unlock();} 71 | 72 | inline void unlock() 73 | { 74 | #ifdef _WIN32 75 | LeaveCriticalSection(&_cs); 76 | #else 77 | pthread_mutex_unlock(&_mutex); 78 | #endif 79 | } 80 | }; 81 | 82 | class ScopedMutexLock; 83 | 84 | template 85 | class Mutexed 86 | { 87 | friend ScopedMutexLock; 88 | public: 89 | Mutexed(const T init):val(init){}; 90 | Mutexed(){}; 91 | Mutexed(const Mutexed & c) 92 | { 93 | val = c.AtomicCopy(); 94 | } 95 | Mutexed& operator=(const Mutexed& c) 96 | { 97 | AtomicWrite(c.AtomicCopy()); 98 | return *this; 99 | } 100 | operator T() 101 | { 102 | return val; 103 | } 104 | 105 | const T* operator->() const 106 | { 107 | return &val; 108 | } 109 | 110 | T* operator->() 111 | { 112 | return &val; 113 | } 114 | 115 | T& operator*() const 116 | { 117 | return val; 118 | } 119 | 120 | T& operator*() 121 | { 122 | return val; 123 | } 124 | 125 | T val; 126 | 127 | void lock() const{_m.lock();} 128 | void unlock() const {_m.unlock();} 129 | 130 | T AtomicCopy() const 131 | { 132 | lock(); 133 | T ret(val); 134 | unlock(); 135 | return ret; 136 | } 137 | 138 | void AtomicWrite(const T& v) 139 | { 140 | lock(); 141 | val = v; 142 | unlock(); 143 | } 144 | 145 | protected: 146 | mutable Mutex _m; 147 | }; 148 | 149 | class ScopedMutexLock 150 | { 151 | protected: 152 | Mutex* _m; 153 | 154 | // default, copy constructor and assignment are disallowed 155 | ScopedMutexLock(){}; 156 | ScopedMutexLock(const ScopedMutexLock&){}; 157 | ScopedMutexLock& operator=(const ScopedMutexLock&){return *this;} 158 | 159 | public: 160 | ScopedMutexLock(Mutex& mutex) 161 | { 162 | _m = &mutex; 163 | _m->lock(); 164 | } 165 | 166 | template 167 | ScopedMutexLock(const Mutexed& mutexed) 168 | { 169 | _m = &mutexed._m; 170 | _m->lock(); 171 | } 172 | 173 | ~ScopedMutexLock() 174 | { 175 | _m->unlock(); 176 | } 177 | }; 178 | 179 | template 180 | class MutexedSPtr : public shared_ptr 181 | { 182 | protected: 183 | Mutex _m; 184 | public: 185 | MutexedSPtr(){}; 186 | MutexedSPtr(T* a):shared_ptr(a){}; 187 | MutexedSPtr(const shared_ptr& a){shared_ptr::operator=(a);}; 188 | MutexedSPtr operator=(const shared_ptr& a){return shared_ptr::operator=(a);}; 189 | void lock(){_m.lock();} 190 | void unlock(){_m.unlock();} 191 | }; 192 | 193 | } //namespace FLR 194 | -------------------------------------------------------------------------------- /src/Utility/SimpleConfig.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "SimpleConfig.h" 3 | #include "Utility/StringUtils.h" 4 | 5 | #include 6 | #include 7 | using namespace std; 8 | 9 | #define MAX_INCLUDE_DEPTH 5 10 | 11 | namespace SLR 12 | { 13 | 14 | shared_ptr SimpleConfig::s_config; 15 | 16 | SimpleConfig::SimpleConfig() 17 | { 18 | Reset(""); 19 | } 20 | 21 | ParamsHandle SimpleConfig::GetInstance() 22 | { 23 | if (!s_config) 24 | { 25 | s_config.reset(new SimpleConfig()); 26 | } 27 | return s_config; 28 | } 29 | 30 | void SimpleConfig::Reset(string rootParam) 31 | { 32 | // todo: go to the right directory 33 | // load all the files in the directory? 34 | _params.clear(); 35 | if (rootParam != "") 36 | { 37 | ReadFile(rootParam); 38 | } 39 | } 40 | 41 | void SimpleConfig::ReadFile(const string& filename, int depth) 42 | { 43 | if (depth > MAX_INCLUDE_DEPTH) 44 | { 45 | SLR_WARNING0("Config includes excede maximum include depth (is something including itself?)"); 46 | return; 47 | } 48 | 49 | FILE* f = fopen(filename.c_str(), "r"); 50 | if (!f) 51 | { 52 | SLR_ERROR1("Can't open file %s", filename.c_str()); 53 | return; 54 | } 55 | 56 | char buf[512]; buf[511] = 0; 57 | int lineNum = 0; 58 | string curNamespace = ""; 59 | 60 | // read line by line... 61 | while (fgets(buf, 510, f)) 62 | { 63 | lineNum++; 64 | string s(buf); 65 | 66 | ParseLine(filename, s, lineNum, curNamespace, depth); 67 | } 68 | 69 | fclose(f); 70 | } 71 | 72 | void SimpleConfig::ParseLine(const string& filename, const string& line, int lineNum, string& curNamespace, int depth) 73 | { 74 | // primitive trailing removal 75 | string s = SLR::LeftOf(line, '#'); 76 | 77 | std::size_t firstNonWS = s.find_first_not_of("\n\t "); 78 | 79 | // is it a comment? 80 | if (firstNonWS == std::string::npos || s[firstNonWS] == '#' || firstNonWS == '/') 81 | { 82 | return; 83 | } 84 | 85 | // include? 86 | if (SLR::ToUpper(s).find("INCLUDE ") == 0) 87 | { 88 | string filenameToInclude = s.substr(7); 89 | filenameToInclude = Trim(filenameToInclude); 90 | // need to put the file in the same directory as this one 91 | auto tmp = filename.find_last_of("/\\"); 92 | string path=""; 93 | if (tmp != string::npos) 94 | { 95 | path = filename.substr(0, tmp+1); 96 | } 97 | ReadFile(path+filenameToInclude, depth + 1); 98 | return; 99 | } 100 | 101 | // is it a namespace? 102 | std::size_t leftBracket = s.find_first_of("["); 103 | std::size_t rightBracket = s.find_last_of("]"); 104 | if (leftBracket != std::string::npos && rightBracket != std::string::npos) 105 | { 106 | curNamespace = ToUpper(s.substr(leftBracket + 1, rightBracket - leftBracket - 1)); 107 | // is it an inherited namespace? 108 | if (Contains(curNamespace, ':')) 109 | { 110 | string baseNamespace = Trim(RightOf(curNamespace, ':')); 111 | curNamespace = Trim(LeftOf(curNamespace, ':')); 112 | CopyNamespaceParams(baseNamespace, curNamespace); 113 | } 114 | return; 115 | } 116 | 117 | // is there an equals sign? 118 | std::size_t equals1 = s.find_first_of("="); 119 | std::size_t equals2 = s.find_last_of("="); 120 | if (equals1 != equals2 || equals1 == std::string::npos) 121 | { 122 | SLR_WARNING2("Line %d in config file %s is malformed", lineNum, filename.c_str()); 123 | return; 124 | } 125 | 126 | // must be a parameter. split off the left part and the right part and remove whitespace 127 | // TODO: handle "" and '' strings? 128 | 129 | string leftPart = ToUpper(Trim(s.substr(firstNonWS, equals1 - firstNonWS))); 130 | string rightPart = Trim(s.substr(equals1 + 1)); 131 | 132 | if (leftPart == "" || rightPart == "") 133 | { 134 | SLR_WARNING2("Line %d in config file %s is malformed", lineNum, filename.c_str()); 135 | return; 136 | } 137 | 138 | if (curNamespace != "") 139 | { 140 | _params[curNamespace + "." + leftPart] = rightPart; 141 | } 142 | else 143 | { 144 | _params[leftPart] = rightPart; 145 | } 146 | } 147 | 148 | void SimpleConfig::CopyNamespaceParams(const string& fromNamespace, const string& toNamespace) 149 | { 150 | string searchString = ToUpper(fromNamespace + "."); 151 | // very lazy implementation 152 | map pCopy = _params; 153 | for (map::iterator i = pCopy.begin(); i != pCopy.end(); i++) 154 | { 155 | if (i->first.compare(0, searchString.length(), searchString) == 0) 156 | { 157 | string tmp = i->first.substr(searchString.size()); 158 | tmp = toNamespace + "." + tmp; 159 | _params[tmp] = i->second; 160 | } 161 | 162 | } 163 | 164 | } 165 | 166 | void SimpleConfig::PrintAll() 167 | { 168 | for(auto i=_params.begin(); i!=_params.end(); i++) 169 | { 170 | printf("%s=%s\n",i->first.c_str(),i->second.c_str()); 171 | } 172 | } 173 | 174 | bool SimpleConfig::Exists(const string& param) 175 | { 176 | return _params.find(ToUpper(param)) != _params.end(); 177 | } 178 | 179 | bool SimpleConfig::GetFloat(const string& param, float& ret) 180 | { 181 | auto i = _params.find(ToUpper(param)); 182 | if(i==_params.end()) return false; 183 | try 184 | { 185 | ret = std::stof(i->second); 186 | return true; 187 | } 188 | catch(...) 189 | { 190 | return false; 191 | } 192 | } 193 | 194 | bool SimpleConfig::GetString(const string& param, string& ret) 195 | { 196 | auto i = _params.find(ToUpper(param)); 197 | if(i==_params.end()) return false; 198 | ret = i->second; 199 | return true; 200 | } 201 | 202 | bool SimpleConfig::GetV3F(const string& param, V3F& ret) 203 | { 204 | auto i = _params.find(ToUpper(param)); 205 | if(i==_params.end()) return false; 206 | string s = i->second; 207 | std::size_t comma1 = s.find_first_of(","); 208 | std::size_t comma2 = s.find_last_of(","); 209 | if(comma1==comma2 || comma1==string::npos || comma2==string::npos) return false; 210 | string a = s.substr(0,comma1); 211 | string b = s.substr(comma1+1,comma2-comma1-1); 212 | string c = s.substr(comma2+1); 213 | try 214 | { 215 | ret = V3F(std::stof(a),std::stof(b),std::stof(c)); 216 | return true; 217 | } 218 | catch(...) 219 | { 220 | return false; 221 | } 222 | } 223 | 224 | bool SimpleConfig::GetFloatVector(const string& param, vector& ret) 225 | { 226 | auto i = _params.find(ToUpper(param)); 227 | if (i == _params.end()) return false; 228 | string s = i->second; 229 | vector spl = SLR::Split(s, ','); 230 | ret.clear(); 231 | for (unsigned i = 0; i < s.size(); i++) 232 | { 233 | try 234 | { 235 | float tmp = std::stof(spl[i]); 236 | ret.push_back(tmp); 237 | } 238 | catch (...) 239 | { 240 | return false; 241 | } 242 | } 243 | return true; 244 | } 245 | 246 | 247 | float SimpleConfig::Get(const string& param, float defaultRet) 248 | { 249 | this->GetFloat(param,defaultRet); 250 | return defaultRet; 251 | } 252 | 253 | string SimpleConfig::Get(const string& param, string defaultRet) 254 | { 255 | this->GetString(param,defaultRet); 256 | return defaultRet; 257 | } 258 | 259 | V3F SimpleConfig::Get(const string& param, V3F defaultRet) 260 | { 261 | this->GetV3F(param,defaultRet); 262 | return defaultRet; 263 | } 264 | 265 | 266 | 267 | 268 | } // namespace SLR 269 | -------------------------------------------------------------------------------- /src/Utility/SimpleConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | using std::vector; 6 | using std::map; 7 | 8 | namespace SLR{ 9 | 10 | class SimpleConfig; 11 | typedef shared_ptr ParamsHandle; 12 | 13 | 14 | class SimpleConfig 15 | { 16 | SimpleConfig(); 17 | void ReadFile(const string& filename, int depth=0); 18 | 19 | public: 20 | static ParamsHandle GetInstance(); 21 | void Reset(string rootParam); 22 | 23 | bool Exists(const string& param); 24 | bool GetFloat(const string& param, float& ret); 25 | bool GetString(const string& param, string& ret); 26 | bool GetV3F(const string& param, V3F& ret); 27 | bool GetFloatVector(const string& param, vector& ret); 28 | 29 | // convenience always-returning functions, with defaults 30 | float Get(const string& param, float defaultRet); 31 | string Get(const string& param, string defaultRet); 32 | V3F Get(const string& param, V3F defaultRet); 33 | 34 | void PrintAll(); 35 | 36 | protected: 37 | static shared_ptr s_config; 38 | map _params; 39 | void ParseLine(const string& filename, const string& ln, int lineNum, string& curNamespace, int depth); 40 | void CopyNamespaceParams(const string& fromNamespace, const string& toNamespace); 41 | }; 42 | 43 | 44 | 45 | } // namespace SLR 46 | -------------------------------------------------------------------------------- /src/Utility/StringUtils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace SLR 6 | { 7 | // helper function -- trimming 8 | inline std::string Trim(const std::string& str, 9 | const std::string& whitespace = " \t\n") 10 | { 11 | const auto strBegin = str.find_first_not_of(whitespace); 12 | if (strBegin == std::string::npos) 13 | return ""; // no content 14 | 15 | const auto strEnd = str.find_last_not_of(whitespace); 16 | const auto strRange = strEnd - strBegin + 1; 17 | 18 | return str.substr(strBegin, strRange); 19 | } 20 | 21 | inline std::string ToUpper(const std::string& in) 22 | { 23 | std::string ret=in; 24 | for(std::size_t i=0;i='a' && ret[i]<='z') ret[i] -= ('a'-'A'); 27 | } 28 | return ret; 29 | } 30 | 31 | inline std::string ToLower(const std::string& in) 32 | { 33 | std::string ret = in; 34 | for (std::size_t i = 0; i= 'A' && ret[i] <= 'Z') ret[i] += ('a' - 'A'); 37 | } 38 | return ret; 39 | } 40 | 41 | 42 | inline std::string CapitalizeFirstLetter(const std::string& in) 43 | { 44 | std::string ret=in; 45 | if(in.size()>0) 46 | { 47 | if(ret[0]>='a' && ret[0]<='z') ret[0] -= ('a'-'A'); 48 | } 49 | return ret; 50 | } 51 | 52 | inline bool Contains(const std::string& s, char c) 53 | { 54 | const auto i = s.find_first_of(c); 55 | return i != std::string::npos; 56 | } 57 | 58 | inline std::string LeftOf(const std::string& s, char c) 59 | { 60 | const auto i = s.find_first_of(c); 61 | if (i == std::string::npos) return s; 62 | return s.substr(0, i); 63 | } 64 | 65 | inline std::string RightOf(const std::string& s, char c) 66 | { 67 | const auto i = s.find_first_of(c); 68 | if (i == std::string::npos) return ""; 69 | return s.substr(i + 1); 70 | } 71 | 72 | inline std::vector Split(const char* str, char c = ' ') 73 | { 74 | std::vector result; 75 | 76 | do 77 | { 78 | const char *begin = str; 79 | 80 | while (*str != c && *str) 81 | { 82 | str++; 83 | } 84 | 85 | result.push_back(std::string(begin, str)); 86 | } while (0 != *str++); 87 | 88 | return result; 89 | } 90 | 91 | inline std::vector Split(std::string s, char c = ' ') 92 | { 93 | return Split(s.c_str(), c); 94 | } 95 | 96 | } // namespace SLR 97 | -------------------------------------------------------------------------------- /src/Utility/Timer.cpp: -------------------------------------------------------------------------------- 1 | #include "../Common.h" 2 | #include "Timer.h" 3 | #include 4 | 5 | #ifdef _WIN32 6 | #include 7 | #pragma comment(lib,"winmm.lib") 8 | #endif 9 | 10 | CHighResTimingScope __highResTimingScope; 11 | int64_t __highResCounterFreq=0; 12 | 13 | CHighResTimingScope::CHighResTimingScope(){ 14 | #ifdef _WIN32 15 | timeBeginPeriod(1); // inits hi-res sleep 16 | QueryPerformanceFrequency((LARGE_INTEGER*)&__highResCounterFreq); 17 | #else 18 | __highResCounterFreq = 1e6; 19 | #endif 20 | if(__highResCounterFreq==0){ 21 | printf("ERROR: no performance counter found\n"); 22 | __highResCounterFreq = 1; 23 | } 24 | }; 25 | 26 | CHighResTimingScope::~CHighResTimingScope() 27 | { 28 | #ifdef _WIN32 29 | timeEndPeriod(1); // de-inits hi-res sleep 30 | #endif 31 | }; 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/Utility/Timer.h: -------------------------------------------------------------------------------- 1 | // Precision timing functions 2 | // Win32 / Linux compatable 3 | // 2002-2018 sergei lupashin 4 | // License: BSD-3-clause 5 | 6 | #pragma once 7 | 8 | #include "Common.h" 9 | #ifdef _WIN32 10 | #ifdef WS2 11 | #include 12 | #endif 13 | #include 14 | #else 15 | #include 16 | #endif 17 | 18 | #ifdef max 19 | #undef max 20 | #endif 21 | 22 | #include 23 | using namespace std; 24 | 25 | typedef uint32_t TICKS; 26 | #define SECS_TO_TICKS(seconds) ((TICKS)((seconds)*10000.0)) 27 | #define TICKS_TO_SECS(ticks) ((double)((double)(ticks)/10000.0)) 28 | // to/from microseconds 29 | #define TICKS_TO_US(ticks) ((__int64)((__int64)(ticks)*100)) 30 | #define US_TO_TICKS(us) ((TICKS)((us)/100)) 31 | #define US_TO_SECS(us) ((double)((double)(us)/1000000.0)) 32 | 33 | // frequency of the hi-res counter 34 | extern int64_t __highResCounterFreq; 35 | 36 | // We use a global instance of this variable to handle the housekeeping 37 | // of initializing and de-initializing Windows high-res timing and sleep 38 | class CHighResTimingScope{ 39 | public: 40 | CHighResTimingScope(); 41 | ~CHighResTimingScope(); 42 | }; 43 | extern CHighResTimingScope __highResTimingScope; 44 | 45 | #ifndef _WIN32 46 | inline int64_t GetTimeMicroseconds() 47 | { 48 | struct timezone tz; 49 | struct timeval t; 50 | gettimeofday(&t, &tz); 51 | return ((int64_t)t.tv_sec*1e6 + t.tv_usec); 52 | } 53 | #endif 54 | 55 | class Timer 56 | { 57 | public: 58 | // the timer times stuff from its creation or most recent reset 59 | 60 | Timer(bool autostart=true) 61 | { 62 | _isBaseClass=true; 63 | if(autostart) 64 | { 65 | Reset(); 66 | } 67 | else 68 | { 69 | _begin_t=0; 70 | } 71 | } 72 | 73 | virtual void Reset() 74 | { 75 | #ifdef _WIN32 76 | QueryPerformanceCounter((LARGE_INTEGER*)&_begin_t); 77 | #else 78 | _begin_t = GetTimeMicroseconds(); 79 | #endif 80 | } 81 | 82 | virtual double Seconds() const 83 | { 84 | return ElapsedSeconds(); 85 | } 86 | 87 | virtual operator double() const{ 88 | return ElapsedSeconds(); 89 | } 90 | 91 | virtual double ElapsedSeconds() const 92 | { 93 | if(!Valid()) return numeric_limits::max(); 94 | 95 | int64_t current; 96 | #ifdef _WIN32 97 | QueryPerformanceCounter((LARGE_INTEGER*)¤t); 98 | #else 99 | current = GetTimeMicroseconds(); 100 | #endif 101 | return ((double)(current-_begin_t)) / (double)__highResCounterFreq; 102 | } 103 | 104 | virtual void AddSeconds(const double& s) 105 | { 106 | if(!Valid()) return; 107 | _begin_t = _begin_t + (int64_t)(s*__highResCounterFreq); 108 | } 109 | 110 | virtual TICKS Ticks() const 111 | { 112 | return ElapsedTicks(); 113 | } 114 | 115 | virtual TICKS ElapsedTicks() const 116 | { 117 | if(!Valid()) return numeric_limits::max(); 118 | 119 | int64_t current; 120 | #ifdef _WIN32 121 | QueryPerformanceCounter((LARGE_INTEGER*)¤t); 122 | #else 123 | current = GetTimeMicroseconds(); 124 | #endif 125 | if(current<_begin_t) return 0; 126 | return (TICKS) ( ((double)(current-_begin_t)) * 10000.0 / (double)__highResCounterFreq); 127 | } 128 | 129 | virtual uint64_t ElapsedMicroseconds() const 130 | { 131 | if(!Valid()) return numeric_limits::max(); 132 | 133 | int64_t current; 134 | #ifdef _WIN32 135 | QueryPerformanceCounter((LARGE_INTEGER*)¤t); 136 | #else 137 | current = GetTimeMicroseconds(); 138 | #endif 139 | if(current<_begin_t) return 0; 140 | return (int64_t) ( ((double)(current-_begin_t)) * 1000000.0 / (double)__highResCounterFreq); 141 | } 142 | 143 | // returns a timer that appears as if started a *long* time ago 144 | static const Timer InvalidTimer() 145 | { 146 | Timer ret; 147 | ret._begin_t = 0; 148 | return ret; 149 | } 150 | 151 | virtual bool Valid() const 152 | { 153 | return _begin_t != 0; 154 | } 155 | 156 | bool IsBaseClass() const {return _isBaseClass;} 157 | 158 | protected: 159 | int64_t _begin_t; 160 | bool _isBaseClass; 161 | }; 162 | 163 | class RunEveryNSeconds 164 | { 165 | public: 166 | RunEveryNSeconds(const double N) 167 | { 168 | _n=N; 169 | } 170 | ~RunEveryNSeconds() 171 | { 172 | // seems to be buggy!! 173 | double dt = timer.Seconds(); 174 | if(_n-dt > 0) 175 | { 176 | Sleep((int)((_n-dt)*1000.f)); 177 | } 178 | } 179 | protected: 180 | double _n; 181 | Timer timer; 182 | }; 183 | -------------------------------------------------------------------------------- /src/VehicleDatatypes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Math/V3F.h" 4 | #include "Math/Quaternion.h" 5 | 6 | using SLR::Quaternion; 7 | 8 | struct GlobalPose 9 | { 10 | V3F pos; 11 | Quaternion q; 12 | }; 13 | 14 | struct VehicleCommand 15 | { 16 | VehicleCommand() 17 | { 18 | mode = 0; 19 | desiredThrustsN[0] = desiredThrustsN[1] = desiredThrustsN[2] = desiredThrustsN[3] = 0; 20 | } 21 | float desiredThrustsN[4]; // N, motor A, motor B, motor C, motor D 22 | uint8_t mode; 23 | }; 24 | 25 | // Struct for holding all of the data related to a single trajectory point 26 | struct TrajectoryPoint { 27 | float time; 28 | V3F position; 29 | V3F velocity; 30 | V3F omega; 31 | V3F accel; 32 | Quaternion attitude; 33 | 34 | // Initialise all fields to zero when declared 35 | TrajectoryPoint() : 36 | time(0.f), 37 | position(0.f, 0.f, 0.f), 38 | velocity(0.f, 0.f, 0.f), 39 | omega(0.f, 0.f, 0.f), 40 | attitude(0.f, 0.f, 0.f, 0.f) 41 | { 42 | } 43 | }; -------------------------------------------------------------------------------- /x64.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Controls-CPP/23e66256e16a79d3ea327674ad97ff7cdbd49c91/x64.png --------------------------------------------------------------------------------