upload code

CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5)
+project(CPPEstSim)
+
+set(CMAKE_CXX_STANDARD 11)
+#set(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -std=c++0x")
+
+include_directories(src)
+include_directories(lib)
+
+FILE(GLOB SOURCES
+        src/*.cpp
+        src/Drawing/*.cpp
+        src/Math/*.cpp
+        src/Simulation/*.cpp
+        src/Utility/*.cpp
+        src/MavlinkNode/*.cpp
+        src/MavlinkNode/*.h)
+
+FILE(GLOB HEADERS
+        src/*.h
+        src/Drawing/*.h
+        src/Math/*.h
+        src/Simulation/*.h
+        src/Utility/*.h
+        lib/matrix/*.hpp
+        lib/mavlink/*.h
+        lib/mavlink/common/*.h)
+
+find_package(Qt5Core REQUIRED)
+find_package(Qt5Network REQUIRED)
+find_package(Qt5Widgets REQUIRED)
+
+# /System/Library/Frameworks/GLUT.framework
+find_package(GLUT REQUIRED)
+include_directories(${GLUT_INCLUDE_DIR})
+# /System/Library/Frameworks/OpenGL.framework
+find_package(OpenGL REQUIRED)
+include_directories(${OPENGL_INCLUDE_DIR})
+
+#find_package(GL REQUIRED)
+#find_package(pthread REQUIRED)
+
+add_executable(CPPEstSim
+        ${SOURCES}
+        ${HEADERS}
+        )
+
+target_link_libraries(CPPEstSim
+        Qt5::Core
+        Qt5::Network
+        Qt5::Widgets
+        ${GLUT_LIBRARIES}
+        ${OPENGL_LIBRARIES}
+        pthread
+        )

build-CPPSim-Desktop_Qt_5_7_0_GCC_64bit-Debug/.qmake.stash

@@ -0,0 +1,15 @@
+QMAKE_DEFAULT_INCDIRS = \
+    /usr/include/c++/5 \
+    /usr/include/x86_64-linux-gnu/c++/5 \
+    /usr/include/c++/5/backward \
+    /usr/lib/gcc/x86_64-linux-gnu/5/include \
+    /usr/local/include \
+    /usr/lib/gcc/x86_64-linux-gnu/5/include-fixed \
+    /usr/include/x86_64-linux-gnu \
+    /usr/include
+QMAKE_DEFAULT_LIBDIRS = \
+    /usr/lib/gcc/x86_64-linux-gnu/5 \
+    /usr/lib/x86_64-linux-gnu \
+    /usr/lib \
+    /lib/x86_64-linux-gnu \
+    /lib

config/01_Intro.txt

@@ -0,0 +1,38 @@
+# Introduction: open-loop hover
+
+INCLUDE QuadPhysicalParams.txt
+
+# simulation setup
+Sim.RunMode = Repeat
+Sim.EndTime = 1
+Sim.Vehicle1 = Quad
+
+# Controller selection
+Quad.ControlType = QuadControl
+Quad.ControlConfig = QuadControlParams
+
+# reference trajectory (just the starting position)
+QuadControlParams.Trajectory=0,0,-1
+
+# initial conditions
+Quad.InitialPos=0,0,-1
+Quad.InitialVel=0,0,0
+Quad.InitialYPR=0,0,0
+Quad.InitialOmega=0,0,0
+
+# graphing commands
+Commands.1=AddGraph1.Quad.Pos.Z
+Commands.2=AddGraph1.WindowThreshold(Quad.PosFollowErr,.5,.8)
+
+INCLUDE QuadControlParams.txt
+INCLUDE Simulation.txt
+
+# make sure the controller is off
+[QuadControlParams]
+kpPosXY = 0
+kpPosZ = 0
+kpVelXY = 0
+kpVelZ = 0
+kpBank = 0
+kpYaw = 0
+kpPQR = 0,0,0

config/02_AttitudeControl.txt

@@ -0,0 +1,39 @@
+# Attitude control
+
+INCLUDE QuadPhysicalParams.txt
+
+# simulation setup
+Sim.RunMode = Repeat
+Sim.EndTime = 1
+Sim.Vehicle1 = Quad
+
+# Controller selection
+Quad.ControlType = QuadControl
+Quad.ControlConfig = QuadControlParams
+
+# reference trajectory (just the starting position)
+QuadControlParams.Trajectory=0,0,-1
+
+# initial conditions
+Quad.InitialPos=0,0,-1
+Quad.InitialVel=0,0,0
+Quad.InitialYPR=0,0,0
+Quad.InitialOmega=30,0,0
+
+# graphing commands
+Commands.1=AddGraph1.Quad.Roll
+Commands.2=AddGraph1.AbsThreshold(Quad.Roll,0.03,0.1)
+Commands.3=AddGraph2.Quad.Omega.X
+Commands.4=AddGraph2.AbsThreshold(Quad.Omega.X,3,0.1)
+Commands.5=AddGraph1.WindowThreshold(Quad.Roll,0.025,0.75)
+Commands.6=AddGraph2.WindowThreshold(Quad.Omega.X,2.5,0.75)
+
+INCLUDE QuadControlParams.txt
+INCLUDE Simulation.txt
+
+# make sure position control is turned off here
+[QuadControlParams]
+kpPosXY = 0
+kpPosZ = 0
+kpVelXY = 0
+kpVelZ = 0

config/03_PositionControl.txt

@@ -0,0 +1,44 @@
+# Hover at the initial point using full 3D control
+
+INCLUDE QuadPhysicalParams.txt
+
+# simulation setup
+Sim.RunMode = Repeat
+Sim.EndTime = 2
+Sim.Vehicle1 = Quad1
+Sim.Vehicle2 = Quad2
+
+# Controller selection
+Quad.ControlType = QuadControl
+Quad.ControlConfig = QuadControlParams
+
+# reference trajectory (just the starting position)
+QuadControlParams.Trajectory=0,0,-1
+
+# initial conditions
+Quad.InitialPos=0.5,0,-1
+Quad.InitialVel=0,0,0
+Quad.InitialYPR=0,0,0
+Quad.InitialOmega=0,0,0
+
+# graphing commands
+Commands.1=AddGraph1.Quad1.Pos.X
+Commands.2=AddGraph1.AbsThreshold(Quad1.Pos.X,0.05,0.5)
+Commands.3=AddGraph1.Quad2.Pos.X
+Commands.4=AddGraph2.Quad1.Yaw
+Commands.5=AddGraph2.Quad2.Yaw
+Commands.6=AddGraph1.WindowThreshold(Quad1.Pos.X,0.1,1.25)
+Commands.7=AddGraph1.WindowThreshold(Quad2.Pos.X,0.1,1.25)
+Commands.8=AddGraph2.WindowThreshold(Quad2.Yaw,0.1,1.0)
+
+INCLUDE QuadControlParams.txt
+INCLUDE Simulation.txt
+
+[Quad1:Quad]
+InitialPos=0.5,1,-1
+TrajectoryOffset=0,1,0
+
+[Quad2:Quad]
+InitialPos=0.5,-1,-1
+InitialYPR=.7,0,0
+TrajectoryOffset=0,-1,0

config/04_Nonidealities.txt

@@ -0,0 +1,52 @@
+# Hover at the initial point using full 3D control
+
+INCLUDE QuadPhysicalParams.txt
+
+# simulation setup
+Sim.RunMode = Repeat
+Sim.EndTime = 3
+Sim.Vehicle1 = Quad1
+Sim.Vehicle2 = Quad2
+Sim.Vehicle3 = Quad3
+
+# Controller selection
+Quad.ControlType = QuadControl
+Quad.ControlConfig = QuadControlParams
+
+# reference trajectory (just the starting position)
+QuadControlParams.Trajectory=0,0,-1
+#QuadControlParams.Trajectory=traj/CircleNoFF.txt
+
+# initial conditions
+Quad.InitialPos=1,0,-.5
+Quad.InitialVel=0,0,0
+Quad.InitialYPR=0,0,0
+Quad.InitialOmega=0,0,0
+
+# graphing commands
+Commands.1=AddGraph1.Quad1.PosFollowErr
+Commands.2=AddGraph1.Quad2.PosFollowErr
+Commands.3=AddGraph1.Quad3.PosFollowErr
+Commands.4=Toggle.RefTrajectory
+Commands.5=Toggle.ActualTrajectory
+Commands.6=AddGraph1.WindowThreshold(Quad1.PosFollowErr,0.1,1.5)
+Commands.7=AddGraph1.WindowThreshold(Quad2.PosFollowErr,0.1,1.5)
+Commands.8=AddGraph1.WindowThreshold(Quad3.PosFollowErr,0.1,1.5)
+
+INCLUDE QuadControlParams.txt
+INCLUDE Simulation.txt
+
+# Vehicle-specific config
+[Quad1:Quad]
+InitialPos=-2,1,-1
+TrajectoryOffset = 0,1,0
+Mass = .8
+
+[Quad2:Quad]
+InitialPos=-2,0,-1
+TrajectoryOffset=0,0,0
+
+[Quad3:Quad]
+InitialPos=-2,-1,-1
+TrajectoryOffset=0,-1,0
+cy=-.05

config/05_TrajectoryFollow.txt

@@ -0,0 +1,39 @@
+# Hover at the initial point using full 3D control
+
+INCLUDE QuadPhysicalParams.txt
+
+# simulation setup
+Sim.RunMode = Repeat
+Sim.EndTime = 10
+Sim.Vehicle1 = Quad1
+Sim.Vehicle2 = Quad2
+
+# Controller selection
+Quad.ControlType = QuadControl
+Quad.ControlConfig = QuadControlParams
+
+# reference trajectory (just the starting position)
+QuadControlParams.Trajectory=traj/FigureEight.txt
+
+# graphing commands
+Commands.1=AddGraph1.Quad1.PosFollowErr
+Commands.2=AddGraph1.Quad2.PosFollowErr
+Commands.3=Toggle.RefTrajectory
+Commands.4=Toggle.ActualTrajectory
+Commands.5=AddGraph1.WindowThreshold(Quad2.PosFollowErr,.25,3)
+
+INCLUDE QuadControlParams.txt
+INCLUDE Simulation.txt
+
+# Vehicle-specific config
+[Quad1:Quad]
+InitialPos=0,1,-1
+TrajectoryOffset = 0,1.5,0
+
+[Quad2:Quad]
+InitialPos=0,-1,-1
+TrajectoryOffset=0,-1.5,0
+ControlConfig = QuadControlParamsFF
+
+[QuadControlParamsFF:QuadControlParams]
+Trajectory=traj/FigureEightFF.txt

config/06_SensorNoise.txt

@@ -0,0 +1,46 @@
+### STUDENT SECTION
+
+MeasuredStdDev_GPSPosXY = 0.72
+MeasuredStdDev_AccelXY = 0.49
+
+### END STUDENT SECTION
+
+INCLUDE Simulation.txt
+INCLUDE QuadPhysicalParams.txt
+INCLUDE SimulatedSensors.txt
+INCLUDE QuadControlParams.txt
+
+# simulation setup
+Sim.RunMode = Repeat
+Sim.EndTime = 10
+Sim.Vehicle1 = Quad
+
+# Controller selection
+Quad.ControlType = QuadControl
+Quad.ControlConfig = QuadControlParams
+
+# reference trajectory
+QuadControlParams.Trajectory=0,0,-1
+
+# SigmaThreshold syntax:
+# SigmaThreshold(signal, refValue, sigma, minOKPercent, maxOKPercent, okWindow)
+#   looks at the percentage of points from <signal> being inside the range of [refValue-sigma, refValue+sigma]
+#   if this percentage is inside [minOKPercent,maxOKPercent] for at least okWindow seconds,
+#   the threshold visualization turns green
+
+# graphing commands
+Commands += SetTitle(1,"GPS")
+Commands += Plot(1,Quad.GPS.X,"x position",0,1,1)
+Commands += AddGraph1.SigmaThreshold(Quad.GPS.X, Quad.Pos.X, MeasuredStdDev_GPSPosXY,64,73,2)
+Commands += AddGraph1.LogToFile
+
+Commands += SetTitle(2,"Accelerometer")
+Commands += Plot(2,Quad.IMU.AX,"x accel",.5,.7,1)
+Commands += AddGraph2.SigmaThreshold(Quad.IMU.AX, 0, MeasuredStdDev_AccelXY,64,73,2)
+Commands += AddGraph2.LogToFile
+
+Quad.Sensors = SimIMU, SimGPS, SimMag
+
+# Vehicle-specific config
+[Quad]
+InitialPos=0,0,-1

config/07_AttitudeEstimation.txt

@@ -0,0 +1,39 @@
+INCLUDE Simulation.txt
+INCLUDE QuadPhysicalParams.txt
+INCLUDE SimulatedSensors.txt
+INCLUDE QuadControlParams.txt
+INCLUDE QuadEstimatorEKF.txt
+
+# Basic Simulation Setup
+Sim.RunMode = Repeat
+Sim.EndTime = 4
+Sim.Vehicle1 = Quad
+
+# Controller selection
+Quad.ControlType = QuadControl
+Quad.ControlConfig = QuadControlParams
+
+# Sensors
+Quad.Sensors = SimIMU
+# use a perfect IMU
+SimIMU.AccelStd = 0,0,0
+SimIMU.GyroStd = 0,0,0
+
+# Initial state & reference trajectory
+Quad.InitialPos=0,0,-1
+QuadControlParams.Trajectory=traj/AttitudeTest.txt
+
+# Graphing commands
+Commands += SetTitle(1,"Estimated Attitude Error")
+Commands += Plot(1,Quad.Est.E.Yaw,"Yaw")
+Commands += Plot(1,Quad.Est.E.Pitch,"Pitch")
+Commands += Plot(1,Quad.Est.E.Roll,"Roll")
+Commands += AddGraph1.WindowThreshold(Quad.Est.E.MaxEuler, .1, 3)
+
+Commands += SetTitle(2,"Estimated vs Actual Attitude")
+Commands += Plot(2,Quad.Roll,"true roll")
+Commands += Plot(2,Quad.Est.Roll,"est roll")
+Commands += Plot(2,Quad.Pitch,"true pitch")
+Commands += Plot(2,Quad.Est.Pitch,"est pitch")
+Commands += Plot(2,Quad.Yaw,"true yaw")
+Commands += Plot(2,Quad.Est.Yaw,"est yaw")