├── config ├── log │ └── .gitignore ├── traj │ ├── Origin.txt │ ├── Turn.txt │ ├── MakeCircleTrajectory.py │ ├── MakeSpiralTrajectory.py │ ├── MakeHelixTrajectory.py │ ├── AttitudeTest.txt │ ├── Square.txt │ ├── MakeHelixUpDownTrajectory.py │ ├── MakePeriodicTrajectory.py │ └── SpiralNoFF.txt ├── X_Scenarios.txt ├── X_TestMavlink.txt ├── Scenarios.txt ├── SimulatedSensors.txt ├── QuadEstimatorEKF.txt ├── 01_Intro.txt ├── X_DebugAttitudeInterp.txt ├── Simulation.txt ├── QuadControlParams.txt ├── 10_MagUpdate.txt ├── 02_AttitudeControl.txt ├── 05_TrajectoryFollow.txt ├── QuadPhysicalParams.txt ├── 03_PositionControl.txt ├── 07_AttitudeEstimation.txt ├── 11_GPSUpdate.txt ├── 08_PredictState.txt ├── 04_Nonidealities.txt ├── 06_SensorNoise.txt ├── X_TestManyQuads.txt ├── X_MonteCarloTest.txt └── 09_PredictCovariance.txt ├── CODEOWNERS ├── lib ├── Eigen │ ├── Eigen │ ├── src │ │ ├── Core │ │ │ ├── util │ │ │ │ ├── NonMPL2.h │ │ │ │ └── ReenableStupidWarnings.h │ │ │ ├── functors │ │ │ │ └── TernaryFunctors.h │ │ │ ├── DiagonalProduct.h │ │ │ ├── arch │ │ │ │ ├── AVX │ │ │ │ │ └── TypeCasting.h │ │ │ │ ├── Default │ │ │ │ │ └── Settings.h │ │ │ │ ├── SSE │ │ │ │ │ └── TypeCasting.h │ │ │ │ ├── CUDA │ │ │ │ │ └── MathFunctions.h │ │ │ │ └── NEON │ │ │ │ │ └── MathFunctions.h │ │ │ ├── SelfCwiseBinaryOp.h │ │ │ ├── Swap.h │ │ │ ├── Assign.h │ │ │ └── MathFunctionsImpl.h │ │ ├── misc │ │ │ ├── lapacke_mangling.h │ │ │ ├── RealSvd2x2.h │ │ │ └── Kernel.h │ │ ├── SparseCore │ │ │ ├── SparseFuzzy.h │ │ │ ├── SparseRedux.h │ │ │ └── MappedSparseMatrix.h │ │ ├── SparseLU │ │ │ ├── SparseLU_Utils.h │ │ │ └── SparseLU_relax_snode.h │ │ └── StlSupport │ │ │ └── details.h │ ├── .hg_archival.txt │ ├── Dense │ ├── CMakeLists.txt │ ├── StdList │ ├── StdDeque │ ├── StdVector │ ├── Householder │ ├── Sparse │ ├── Jacobi │ ├── QtAlignedMalloc │ ├── MetisSupport │ ├── PardisoSupport │ ├── Cholesky │ ├── SPQRSupport │ ├── SparseQR │ ├── QR │ ├── UmfPackSupport │ ├── LU │ ├── SparseCholesky │ ├── SVD │ ├── PaStiXSupport │ ├── SparseLU │ ├── Eigenvalues │ ├── CholmodSupport │ ├── IterativeLinearSolvers │ ├── Geometry │ ├── SuperLUSupport │ ├── SparseCore │ └── OrderingMethods ├── freeglut │ ├── Readme.txt │ ├── bin │ │ ├── freeglut.dll │ │ └── x64 │ │ │ └── freeglut.dll │ ├── lib │ │ ├── freeglut.lib │ │ └── x64 │ │ │ └── freeglut.lib │ ├── include │ │ └── GL │ │ │ ├── glut.h │ │ │ └── freeglut.h │ └── Copying.txt ├── mavlink │ ├── mavlink_sha256.h │ ├── common │ │ ├── version.h │ │ └── mavlink.h │ ├── mavlink_get_info.h │ └── checksum.h └── matrix │ ├── math.hpp │ ├── filter.hpp │ ├── helper_functions.hpp │ ├── integration.hpp │ ├── Vector2.hpp │ ├── Scalar.hpp │ ├── LICENSE │ ├── Vector.hpp │ └── Vector3.hpp ├── images ├── mag-drift.png ├── bad-x-sigma.PNG ├── bad-vx-sigma.PNG ├── bad-vx-sigma-low.PNG ├── predict-good-cov.png ├── attitude-screenshot.png ├── mag-good-solution.png └── predict-slow-drift.png ├── project ├── freeglut.dll ├── Simulator.vcxproj.user ├── FCND-CPPSim.xcodeproj │ └── project.xcworkspace │ │ └── contents.xcworkspacedata ├── CPPSim.pro └── Simulator.sln ├── src ├── Math │ ├── Constants.h │ ├── MathUtils.h │ ├── Random.h │ ├── LowPassFilter.h │ ├── Angles.h │ ├── Random.cpp │ └── V4D.h ├── BaseQuadEstimator.cpp ├── Drawing │ ├── BaseAnalyzer.h │ ├── GraphManager.h │ ├── GLUTMenu.h │ ├── ColorUtils.h │ ├── Graph.h │ ├── DrawingFuncs.h │ ├── ColorUtils.cpp │ ├── AbsThreshold.h │ └── GLUTMenu.cpp ├── MavlinkNode │ ├── MavlinkTranslation.h │ ├── UDPPacket.h │ ├── MavlinkNode.h │ ├── MavlinkTranslation.cpp │ └── MavlinkNode.cpp ├── ControllerFactory.h ├── Simulation │ ├── Simulator.h │ ├── rangefinder.h │ ├── magnetometer.h │ ├── rangefinder.cpp │ ├── Simulator.cpp │ ├── opticalflow.h │ ├── opticalflow.cpp │ ├── magnetometer.cpp │ ├── SimulatedQuadSensor.h │ ├── SimulatedMag.h │ └── BaseDynamics.h ├── DataSource.h ├── Utility │ ├── Timer.cpp │ ├── Camera.h │ └── SimpleConfig.h ├── Trajectory.h ├── VehicleDatatypes.h ├── BaseQuadEstimator.h ├── QuadControl.h ├── BaseController.h ├── QuadEstimatorEKF.h └── BaseController.cpp ├── .gitignore ├── CMakeLists.txt └── .github └── workflows └── manual.yml /config/log/.gitignore: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /config/traj/Origin.txt: -------------------------------------------------------------------------------- 1 | 0,0,0,-1 -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @udacity/active-public-content -------------------------------------------------------------------------------- /lib/Eigen/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | #include "Sparse" 3 | -------------------------------------------------------------------------------- /images/mag-drift.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/mag-drift.png -------------------------------------------------------------------------------- /project/freeglut.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/project/freeglut.dll -------------------------------------------------------------------------------- /images/bad-x-sigma.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/bad-x-sigma.PNG -------------------------------------------------------------------------------- /images/bad-vx-sigma.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/bad-vx-sigma.PNG -------------------------------------------------------------------------------- /lib/freeglut/Readme.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/lib/freeglut/Readme.txt -------------------------------------------------------------------------------- /images/bad-vx-sigma-low.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/bad-vx-sigma-low.PNG -------------------------------------------------------------------------------- /images/predict-good-cov.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/predict-good-cov.png -------------------------------------------------------------------------------- /config/X_Scenarios.txt: -------------------------------------------------------------------------------- 1 | X_DebugAttitudeInterp 2 | X_TestMavlink 3 | X_TestManyQuads 4 | X_MonteCarloTest 5 | X_ParamTests -------------------------------------------------------------------------------- /images/attitude-screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/attitude-screenshot.png -------------------------------------------------------------------------------- /images/mag-good-solution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/mag-good-solution.png -------------------------------------------------------------------------------- /images/predict-slow-drift.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/images/predict-slow-drift.png -------------------------------------------------------------------------------- /lib/freeglut/bin/freeglut.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/lib/freeglut/bin/freeglut.dll -------------------------------------------------------------------------------- /lib/freeglut/lib/freeglut.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/lib/freeglut/lib/freeglut.lib -------------------------------------------------------------------------------- /lib/mavlink/mavlink_sha256.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/lib/mavlink/mavlink_sha256.h -------------------------------------------------------------------------------- /lib/Eigen/src/Core/util/NonMPL2.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_MPL2_ONLY 2 | #error Including non-MPL2 code in EIGEN_MPL2_ONLY mode 3 | #endif 4 | -------------------------------------------------------------------------------- /lib/freeglut/bin/x64/freeglut.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/lib/freeglut/bin/x64/freeglut.dll -------------------------------------------------------------------------------- /lib/freeglut/lib/x64/freeglut.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-Estimation-CPP/HEAD/lib/freeglut/lib/x64/freeglut.lib -------------------------------------------------------------------------------- /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 -------------------------------------------------------------------------------- /lib/Eigen/.hg_archival.txt: -------------------------------------------------------------------------------- 1 | repo: 8a21fd850624c931e448cbcfb38168cb2717c790 2 | node: 5a0156e40feb7c4136680b493c6e433d91a6f355 3 | branch: 3.3 4 | tag: 3.3.4 5 | -------------------------------------------------------------------------------- /lib/Eigen/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /project/Simulator.vcxproj.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /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] -------------------------------------------------------------------------------- /project/FCND-CPPSim.xcodeproj/project.xcworkspace/contents.xcworkspacedata: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /config/traj/Turn.txt: -------------------------------------------------------------------------------- 1 | # time, x, y, z, vx, vy, vz, yaw 2 | 0.0, 0, 0, -1, 0, 0, 0, 0 3 | 0.5, 0, 0, -1, 0, 0, 0, 1.57 4 | 1.0, 0, 0, -1, 0, 0, 0, 3.14 5 | 1.5, 0, 0, -1, 0, 0, 0, -1.57 6 | 2.0, 0, 0, -1, 0, 0, 0, 0 7 | -------------------------------------------------------------------------------- /config/Scenarios.txt: -------------------------------------------------------------------------------- 1 | 01_Intro 2 | 02_AttitudeControl 3 | 03_PositionControl 4 | 04_Nonidealities 5 | 05_TrajectoryFollow 6 | 06_SensorNoise 7 | 07_AttitudeEstimation 8 | 08_PredictState 9 | 09_PredictCovariance 10 | 10_MagUpdate 11 | 11_GPSUpdate -------------------------------------------------------------------------------- /src/BaseQuadEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "Common.h" 2 | #include "BaseQuadEstimator.h" 3 | 4 | BaseQuadEstimator::BaseQuadEstimator(string config) 5 | { 6 | _config = config; 7 | Init(); 8 | } 9 | 10 | BaseQuadEstimator::~BaseQuadEstimator() 11 | { 12 | 13 | } 14 | -------------------------------------------------------------------------------- /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/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/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/ControllerFactory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "QuadControl.h" 4 | 5 | inline ControllerHandle CreateController(string name, string controllerType, string config) 6 | { 7 | ControllerHandle ret; 8 | 9 | if (controllerType == "QuadControl") 10 | { 11 | ret.reset(new QuadControl(name, config)); 12 | } 13 | 14 | return ret; 15 | } -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /config/SimulatedSensors.txt: -------------------------------------------------------------------------------- 1 | # note that sensors can not be faster than the controller rate (0.002) 2 | 3 | [SimIMU] 4 | AccelStd = .5, .5, 1.5 5 | GyroStd = .5, .5, .5 6 | dt = .002 7 | 8 | [SimBaro] 9 | Std = .1 10 | dt = .01 11 | 12 | [SimMag] 13 | Std = .1 14 | dt = .01 15 | 16 | [SimGPS] 17 | PosStd = .7, .7, 2 18 | #PosRandomWalkStd = .1, .1, .1 19 | VelStd = .1, .1, .3 20 | dt = .1 -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | 16 | virtual vector GetFields() const 17 | { 18 | return vector(); 19 | } 20 | 21 | virtual void FinalizeDataFrame() {} 22 | }; -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /.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 | # manual cmake build ignores 27 | build/ 28 | -------------------------------------------------------------------------------- /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/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/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/QuadEstimatorEKF.txt: -------------------------------------------------------------------------------- 1 | [QuadEstimatorEKF] 2 | InitState = 0, 0, -1, 0, 0, 0, 0 3 | InitStdDevs = .1, .1, .3, .1, .1, .3, .05 4 | 5 | # Process noise model 6 | # note that the process covariance matrix is diag(pow(QStd,2))*dtIMU 7 | 8 | QPosXYStd = .05 9 | QPosZStd = .05 10 | QVelXYStd = .05 11 | QVelZStd = .1 12 | QYawStd = .05 13 | 14 | # GPS measurement std deviations 15 | GPSPosXYStd = 1 16 | GPSPosZStd = 3 17 | GPSVelXYStd = .1 18 | GPSVelZStd = .3 19 | 20 | # Magnetometer 21 | MagYawStd = .1 22 | 23 | dtIMU = 0.002 24 | attitudeTau = 100 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 | -------------------------------------------------------------------------------- /lib/Eigen/src/misc/lapacke_mangling.h: -------------------------------------------------------------------------------- 1 | #ifndef LAPACK_HEADER_INCLUDED 2 | #define LAPACK_HEADER_INCLUDED 3 | 4 | #ifndef LAPACK_GLOBAL 5 | #if defined(LAPACK_GLOBAL_PATTERN_LC) || defined(ADD_) 6 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 7 | #elif defined(LAPACK_GLOBAL_PATTERN_UC) || defined(UPPER) 8 | #define LAPACK_GLOBAL(lcname,UCNAME) UCNAME 9 | #elif defined(LAPACK_GLOBAL_PATTERN_MC) || defined(NOCHANGE) 10 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname 11 | #else 12 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 13 | #endif 14 | #endif 15 | 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /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 | inline float gasdev_f(int &idum) { return (float)gasdev(idum); } 10 | 11 | inline float ran1_inRange(float min, float max, int& idum) 12 | { 13 | return (float)(ran1(idum)*(max-min)+min); 14 | } 15 | 16 | inline double ran1_inRange(double min, double max, int& idum) 17 | { 18 | return (double)(ran1(idum)*(max-min)+min); 19 | } -------------------------------------------------------------------------------- /config/traj/AttitudeTest.txt: -------------------------------------------------------------------------------- 1 | # time, x, y, z, vx, vy, vz, yaw 2 | 0.0, 0, 0, -1, 0, 0, 0, 0 3 | 4 | 0.1, 1, 0, -1, 0, 0, 0, 0 5 | 6 | 0.2, 0, 0, -1, 0, 0, 0, 0 7 | 0.9, 0, 0, -1, 0, 0, 0, 0 8 | 9 | 1.0, 0, -1, -1, 0, 0, 0, 0 10 | 11 | 1.1, 0, 0, -1, 0, 0, 0, 0 12 | 1.8, 0, 0, -1, 0, 0, 0, 0 13 | 14 | 1.9, -0.7, -0.7, -1, 0, 0, 0, 0 15 | 16 | 2.0, 0, 0, -1, 0, 0, 0, 0 17 | 2.7, 0, 0, -1, 0, 0, 0, 0 18 | 19 | 3.0, 0, 0, -1, 0, 0, 0, 1.57 20 | 3.2, 0, 0, -1, 0, 0, 0, 3.14 21 | 3.6, 0, 0, -1, 0, 0, 0, -1.57 22 | 3.9, 0, 0, -1, 0, 0, 0, 0 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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | void Simulator::Run(float dt) 25 | { 26 | // todo: step in maximally acceptable time steps 27 | } 28 | 29 | void Simulator::AddVehicle(shared_ptr vehicle) 30 | { 31 | _vehicles.push_back(vehicle); 32 | } 33 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/functors/TernaryFunctors.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Eugene Brevdo 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TERNARY_FUNCTORS_H 11 | #define EIGEN_TERNARY_FUNCTORS_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | //---------- associative ternary functors ---------- 18 | 19 | 20 | 21 | } // end namespace internal 22 | 23 | } // end namespace Eigen 24 | 25 | #endif // EIGEN_TERNARY_FUNCTORS_H 26 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /src/Math/LowPassFilter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | template 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 | }; -------------------------------------------------------------------------------- /lib/Eigen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(RegexUtils) 2 | test_escape_string_as_regex() 3 | 4 | file(GLOB Eigen_directory_files "*") 5 | 6 | escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 7 | 8 | foreach(f ${Eigen_directory_files}) 9 | if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src") 10 | list(APPEND Eigen_directory_files_to_install ${f}) 11 | endif() 12 | endforeach(f ${Eigen_directory_files}) 13 | 14 | install(FILES 15 | ${Eigen_directory_files_to_install} 16 | DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel 17 | ) 18 | 19 | install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h") 20 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /config/traj/Square.txt: -------------------------------------------------------------------------------- 1 | # time, x, y, z, vx, vy, vz, yaw 2 | 2.0, 0, 0, -1, 0, 0, 0, 0 3 | 3.0, 3, 0, -1, 0, 0, 0, 0 4 | 5 | 4.0, 3, 0, -1, 0, 0, 0, 1.57 6 | 5.0, 3, 3, -1, 0, 0, 0, 1.57 7 | 6.0, 3, 3, -1, 0, 0, 0, 3.14 8 | 7.0, -3, 3, -1, 0, 0, 0, 3.14 9 | 8.0, -3, 3, -1, 0, 0, 0, -1.57 10 | 9.0, -3, -3, -1, 0, 0, 0, -1.57 11 | 10.0, -3, -3, -1, 0, 0, 0, 0 12 | 11.0, 3, -3, -1, 0, 0, 0, 0 13 | 12.0, 3, -3, -1, 0, 0, 0, 1.57 14 | 15 | 13.0, 3, 3, -1, 0, 0, 0, 1.57 16 | 14.0, 3, 3, -1, 0, 0, 0, 3.14 17 | 15.0, -3, 3, -1, 0, 0, 0, 3.14 18 | 16.0, -3, 3, -1, 0, 0, 0, -1.57 19 | 17.0, -3, -3, -1, 0, 0, 0, -1.57 20 | 18.0, -3, -3, -1, 0, 0, 0, 0 21 | 19.0, 3, -3, -1, 0, 0, 0, 0 22 | 20.0, 3, -3, -1, 0, 0, 0, 1.57 23 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | } -------------------------------------------------------------------------------- /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 | HEADERS += ../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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/Eigen/StdList: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Hauke Heibel 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_STDLIST_MODULE_H 11 | #define EIGEN_STDLIST_MODULE_H 12 | 13 | #include "Core" 14 | #include 15 | 16 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 17 | 18 | #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) 19 | 20 | #else 21 | 22 | #include "src/StlSupport/StdList.h" 23 | 24 | #endif 25 | 26 | #endif // EIGEN_STDLIST_MODULE_H 27 | -------------------------------------------------------------------------------- /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/01_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 -------------------------------------------------------------------------------- /lib/Eigen/StdDeque: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDDEQUE_MODULE_H 12 | #define EIGEN_STDDEQUE_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdDeque.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDDEQUE_MODULE_H 28 | -------------------------------------------------------------------------------- /lib/Eigen/StdVector: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDVECTOR_MODULE_H 12 | #define EIGEN_STDVECTOR_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdVector.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDVECTOR_MODULE_H 28 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/util/ReenableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_WARNINGS_DISABLED 2 | #undef EIGEN_WARNINGS_DISABLED 3 | 4 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 5 | #ifdef _MSC_VER 6 | #pragma warning( pop ) 7 | #elif defined __INTEL_COMPILER 8 | #pragma warning pop 9 | #elif defined __clang__ 10 | #pragma clang diagnostic pop 11 | #elif defined __GNUC__ && __GNUC__>=6 12 | #pragma GCC diagnostic pop 13 | #endif 14 | 15 | #if defined __NVCC__ 16 | // Don't reenable the diagnostic messages, as it turns out these messages need 17 | // to be disabled at the point of the template instantiation (i.e the user code) 18 | // otherwise they'll be triggered by nvcc. 19 | // #pragma diag_default code_is_unreachable 20 | // #pragma diag_default initialization_not_reachable 21 | // #pragma diag_default 2651 22 | // #pragma diag_default 2653 23 | #endif 24 | 25 | #endif 26 | 27 | #endif // EIGEN_WARNINGS_DISABLED 28 | -------------------------------------------------------------------------------- /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 GraphCommand(string path); 20 | void InitPaint(); 21 | void Paint(); 22 | 23 | void RegisterDataSource(shared_ptr src); 24 | template 25 | void RegisterDataSources(vector > srcs) 26 | { 27 | for (auto i = srcs.begin(); i != srcs.end(); i++) 28 | { 29 | RegisterDataSource(*i); 30 | } 31 | } 32 | 33 | shared_ptr graph1, graph2; 34 | std::vector > _sources; 35 | 36 | std::vector GetGraphableStrings(); 37 | 38 | protected: 39 | int _glutWindowNum; 40 | bool _ownWindow; 41 | }; 42 | -------------------------------------------------------------------------------- /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 1000 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 | vector 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 | -------------------------------------------------------------------------------- /config/X_DebugAttitudeInterp.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 = 5 8 | Sim.Vehicle1 = Quad1 9 | 10 | # Controller selection 11 | Quad.ControlType = QuadControl 12 | Quad.ControlConfig = QuadControlParams 13 | 14 | # reference trajectory 15 | QuadControlParams.Trajectory=traj/Turn.txt 16 | 17 | # graphing commands 18 | Commands += AddGraph2.Quad1.Est.S.Yaw 19 | Commands += AddGraph2.Quad1.Est.Yaw 20 | Commands += AddGraph2.Quad1.Yaw 21 | Commands += AddGraph2.Quad1.Ref.Yaw 22 | 23 | INCLUDE QuadControlParams.txt 24 | INCLUDE Simulation.txt 25 | 26 | # USE ACTUAL ESTIMATOR! 27 | INCLUDE SimulatedSensors.txt 28 | [QuadControlParams] 29 | UseIdealEstimator=1 30 | INCLUDE QuadEstimatorEKF.txt 31 | 32 | # relax control so we can move around the vehicle 33 | [QuadControlParams] 34 | kpPosXY = .5 35 | 36 | # Vehicle-specific config 37 | [Quad1:Quad] 38 | InitialPos=0,3,-1 -------------------------------------------------------------------------------- /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 | xBounds = -30, 30 20 | yBounds = -30, 30 21 | zBounds = -40, 0 22 | 23 | # Simulated noise 24 | gyroNoiseInt = 0.00001 25 | rotDisturbanceInt = 0.00001 26 | xyzDisturbanceInt = 0.00001 27 | rotDisturbanceBW = 2 28 | xyzDisturbanceBW = 2 29 | 30 | [Quad] 31 | randomMotorForceMag = .25 32 | trajectoryLogStepTime = .05 -------------------------------------------------------------------------------- /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/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 | }; -------------------------------------------------------------------------------- /lib/Eigen/Householder: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_HOUSEHOLDER_MODULE_H 9 | #define EIGEN_HOUSEHOLDER_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Householder_Module Householder module 16 | * This module provides Householder transformations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | */ 22 | 23 | #include "src/Householder/Householder.h" 24 | #include "src/Householder/HouseholderSequence.h" 25 | #include "src/Householder/BlockHouseholder.h" 26 | 27 | #include "src/Core/util/ReenableStupidWarnings.h" 28 | 29 | #endif // EIGEN_HOUSEHOLDER_MODULE_H 30 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 31 | -------------------------------------------------------------------------------- /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.5 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 = 2 23 | kpPosZ = 2 24 | KiPosZ = 50 25 | 26 | # Velocity control gains 27 | kpVelXY = 8 28 | kpVelZ = 8 29 | 30 | # Angle control gains 31 | kpBank = 12 32 | kpYaw = 3 33 | 34 | # Angle rate gains 35 | kpPQR = 70, 70, 15 36 | 37 | # limits 38 | maxAscentRate = 5 39 | maxDescentRate = 2 40 | maxSpeedXY = 5 41 | maxHorizAccel = 12 42 | maxTiltAngle = .7 43 | -------------------------------------------------------------------------------- /config/10_MagUpdate.txt: -------------------------------------------------------------------------------- 1 | INCLUDE Simulation.txt 2 | INCLUDE QuadPhysicalParams.txt 3 | INCLUDE SimulatedSensors.txt 4 | INCLUDE QuadControlParams.txt 5 | INCLUDE QuadEstimatorEKF.txt 6 | 7 | # BASIC 8 | Sim.RunMode = Repeat 9 | Sim.EndTime = 20 10 | Sim.Vehicle1 = Quad 11 | Quad.InitialPos=0,0,-1 12 | 13 | # REFERENCE 14 | QuadControlParams.Trajectory=traj/Square.txt 15 | 16 | # CONTROLLER 17 | Quad.ControlType = QuadControl 18 | Quad.ControlConfig = QuadControlParams 19 | 20 | # SENSORS 21 | Quad.Sensors = SimIMU, SimMag 22 | 23 | # GRAPHING 24 | Commands += Toggle.ActualTrajectory 25 | 26 | Commands += SetTitle(1,"Yaw") 27 | Commands += Plot(1,Quad.MagYaw,"Mag") 28 | Commands += Plot(1,Quad.Est.Yaw,"Estimated") 29 | Commands += Plot(1,Quad.Yaw,"True") 30 | 31 | Commands += SetTitle(2,"Yaw Error") 32 | Commands += Plot(2,Quad.Est.E.Yaw,"yaw error", 1, 1, 0) 33 | Commands += AddGraph2.WindowThreshold(Quad.Est.E.Yaw, .12 ,10) 34 | Commands += AddGraph2.SigmaThreshold(Quad.Est.E.Yaw, 0, Quad.Est.S.Yaw,55,85,2) 35 | Commands += AddGraph2.SetYAxis(-0.15,0.15) -------------------------------------------------------------------------------- /src/BaseQuadEstimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "DataSource.h" 4 | #include "Math/V3F.h" 5 | #include "Math/Quaternion.h" 6 | 7 | using SLR::Quaternion; 8 | 9 | class BaseQuadEstimator : public DataSource 10 | { 11 | public: 12 | BaseQuadEstimator(string config); 13 | virtual ~BaseQuadEstimator(); 14 | 15 | virtual void Init() {}; 16 | 17 | virtual void Predict(float dt, V3F accel, V3F gyro) {}; 18 | 19 | virtual void UpdateFromIMU(V3F accel, V3F gyro) {}; 20 | virtual void UpdateFromGPS(V3F pos, V3F vel) {}; 21 | virtual void UpdateFromBaro(float z) {}; 22 | virtual void UpdateFromMag(float magYaw) {}; 23 | virtual void UpdateFromOpticalFlow(float dx, float dy) {}; 24 | virtual void UpdateFromRangeSensor(float rng) {}; 25 | 26 | virtual void UpdateTrueError(V3F truePos, V3F trueVel, Quaternion trueAtt) {}; 27 | 28 | virtual V3F EstimatedPosition() = 0; 29 | virtual V3F EstimatedVelocity()=0; 30 | virtual Quaternion EstimatedAttitude()=0; 31 | virtual V3F EstimatedOmega()=0; 32 | 33 | string _config; 34 | }; 35 | -------------------------------------------------------------------------------- /lib/Eigen/Sparse: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSE_MODULE_H 9 | #define EIGEN_SPARSE_MODULE_H 10 | 11 | /** \defgroup Sparse_Module Sparse meta-module 12 | * 13 | * Meta-module including all related modules: 14 | * - \ref SparseCore_Module 15 | * - \ref OrderingMethods_Module 16 | * - \ref SparseCholesky_Module 17 | * - \ref SparseLU_Module 18 | * - \ref SparseQR_Module 19 | * - \ref IterativeLinearSolvers_Module 20 | * 21 | \code 22 | #include 23 | \endcode 24 | */ 25 | 26 | #include "SparseCore" 27 | #include "OrderingMethods" 28 | #ifndef EIGEN_MPL2_ONLY 29 | #include "SparseCholesky" 30 | #endif 31 | #include "SparseLU" 32 | #include "SparseQR" 33 | #include "IterativeLinearSolvers" 34 | 35 | #endif // EIGEN_SPARSE_MODULE_H 36 | 37 | -------------------------------------------------------------------------------- /lib/Eigen/Jacobi: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_JACOBI_MODULE_H 9 | #define EIGEN_JACOBI_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Jacobi_Module Jacobi module 16 | * This module provides Jacobi and Givens rotations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | * 22 | * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation: 23 | * - MatrixBase::applyOnTheLeft() 24 | * - MatrixBase::applyOnTheRight(). 25 | */ 26 | 27 | #include "src/Jacobi/Jacobi.h" 28 | 29 | #include "src/Core/util/ReenableStupidWarnings.h" 30 | 31 | #endif // EIGEN_JACOBI_MODULE_H 32 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 33 | 34 | -------------------------------------------------------------------------------- /config/02_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 -------------------------------------------------------------------------------- /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/Simulation/SimulatedQuadSensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | class BaseQuadEstimator; 4 | 5 | class SimulatedQuadSensor : public DataSource 6 | { 7 | public: 8 | SimulatedQuadSensor(string config, string name) : _config(config), _name(name) { Init(); } 9 | 10 | virtual void Init() 11 | { 12 | _freshMeas = false; 13 | _timeAccum = 0; 14 | }; 15 | 16 | // if it's time, generates a new sensor measurement, saves it internally (for graphing), and calls appropriate estimator update function 17 | virtual void Update(QuadDynamics& quadDynamics, shared_ptr estimator, float dt, int& idum) {}; 18 | 19 | // Access functions for graphing variables 20 | // note that GetData will only return true if a fresh measurement was generated last Update() 21 | virtual bool GetData(const string& name, float& ret) const { return false; }; 22 | virtual vector GetFields() const { return vector(); }; 23 | virtual void FinalizeDataFrame() { _freshMeas = false; } 24 | 25 | string _config, _name; 26 | bool _freshMeas; 27 | float _timeAccum; 28 | }; 29 | 30 | -------------------------------------------------------------------------------- /config/05_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 -------------------------------------------------------------------------------- /lib/Eigen/src/Core/DiagonalProduct.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // Copyright (C) 2007-2009 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_DIAGONALPRODUCT_H 12 | #define EIGEN_DIAGONALPRODUCT_H 13 | 14 | namespace Eigen { 15 | 16 | /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal. 17 | */ 18 | template 19 | template 20 | inline const Product 21 | MatrixBase::operator*(const DiagonalBase &a_diagonal) const 22 | { 23 | return Product(derived(),a_diagonal.derived()); 24 | } 25 | 26 | } // end namespace Eigen 27 | 28 | #endif // EIGEN_DIAGONALPRODUCT_H 29 | -------------------------------------------------------------------------------- /lib/Eigen/QtAlignedMalloc: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QTMALLOC_MODULE_H 9 | #define EIGEN_QTMALLOC_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #if (!EIGEN_MALLOC_ALREADY_ALIGNED) 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | void *qMalloc(std::size_t size) 18 | { 19 | return Eigen::internal::aligned_malloc(size); 20 | } 21 | 22 | void qFree(void *ptr) 23 | { 24 | Eigen::internal::aligned_free(ptr); 25 | } 26 | 27 | void *qRealloc(void *ptr, std::size_t size) 28 | { 29 | void* newPtr = Eigen::internal::aligned_malloc(size); 30 | memcpy(newPtr, ptr, size); 31 | Eigen::internal::aligned_free(ptr); 32 | return newPtr; 33 | } 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | 39 | #endif // EIGEN_QTMALLOC_MODULE_H 40 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 41 | -------------------------------------------------------------------------------- /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 -------------------------------------------------------------------------------- /lib/Eigen/MetisSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_METISSUPPORT_MODULE_H 9 | #define EIGEN_METISSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | 20 | /** \ingroup Support_modules 21 | * \defgroup MetisSupport_Module MetisSupport module 22 | * 23 | * \code 24 | * #include 25 | * \endcode 26 | * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). 27 | * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink 28 | */ 29 | 30 | 31 | #include "src/MetisSupport/MetisSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_METISSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /config/03_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 -------------------------------------------------------------------------------- /lib/Eigen/src/SparseCore/SparseFuzzy.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_FUZZY_H 11 | #define EIGEN_SPARSE_FUZZY_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | template 17 | bool SparseMatrixBase::isApprox(const SparseMatrixBase& other, const RealScalar &prec) const 18 | { 19 | const typename internal::nested_eval::type actualA(derived()); 20 | typename internal::conditional::type, 22 | const PlainObject>::type actualB(other.derived()); 23 | 24 | return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm()); 25 | } 26 | 27 | } // end namespace Eigen 28 | 29 | #endif // EIGEN_SPARSE_FUZZY_H 30 | -------------------------------------------------------------------------------- /lib/Eigen/PardisoSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PARDISOSUPPORT_MODULE_H 9 | #define EIGEN_PARDISOSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup PardisoSupport_Module PardisoSupport module 19 | * 20 | * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies. 27 | * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration. 28 | * 29 | */ 30 | 31 | #include "src/PardisoSupport/PardisoSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_PARDISOSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /config/07_AttitudeEstimation.txt: -------------------------------------------------------------------------------- 1 | INCLUDE Simulation.txt 2 | INCLUDE QuadPhysicalParams.txt 3 | INCLUDE SimulatedSensors.txt 4 | INCLUDE QuadControlParams.txt 5 | INCLUDE QuadEstimatorEKF.txt 6 | 7 | # Basic Simulation Setup 8 | Sim.RunMode = Repeat 9 | Sim.EndTime = 4 10 | Sim.Vehicle1 = Quad 11 | 12 | # Controller selection 13 | Quad.ControlType = QuadControl 14 | Quad.ControlConfig = QuadControlParams 15 | 16 | # Sensors 17 | Quad.Sensors = SimIMU 18 | # use a perfect IMU 19 | SimIMU.AccelStd = 0,0,0 20 | SimIMU.GyroStd = 0,0,0 21 | 22 | # Initial state & reference trajectory 23 | Quad.InitialPos=0,0,-1 24 | QuadControlParams.Trajectory=traj/AttitudeTest.txt 25 | 26 | # Graphing commands 27 | Commands += SetTitle(1,"Estimated Attitude Error") 28 | Commands += Plot(1,Quad.Est.E.Yaw,"Yaw") 29 | Commands += Plot(1,Quad.Est.E.Pitch,"Pitch") 30 | Commands += Plot(1,Quad.Est.E.Roll,"Roll") 31 | Commands += AddGraph1.WindowThreshold(Quad.Est.E.MaxEuler, .1, 3) 32 | 33 | Commands += SetTitle(2,"Estimated vs Actual Attitude") 34 | Commands += Plot(2,Quad.Roll,"true roll") 35 | Commands += Plot(2,Quad.Est.Roll,"est roll") 36 | Commands += Plot(2,Quad.Pitch,"true pitch") 37 | Commands += Plot(2,Quad.Est.Pitch,"est pitch") 38 | Commands += Plot(2,Quad.Yaw,"true yaw") 39 | Commands += Plot(2,Quad.Est.Yaw,"est yaw") -------------------------------------------------------------------------------- /lib/Eigen/Cholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLESKY_MODULE_H 9 | #define EIGEN_CHOLESKY_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Cholesky_Module Cholesky module 16 | * 17 | * 18 | * 19 | * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. 20 | * Those decompositions are also accessible via the following methods: 21 | * - MatrixBase::llt() 22 | * - MatrixBase::ldlt() 23 | * - SelfAdjointView::llt() 24 | * - SelfAdjointView::ldlt() 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | */ 30 | 31 | #include "src/Cholesky/LLT.h" 32 | #include "src/Cholesky/LDLT.h" 33 | #ifdef EIGEN_USE_LAPACKE 34 | #include "src/misc/lapacke.h" 35 | #include "src/Cholesky/LLT_LAPACKE.h" 36 | #endif 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_CHOLESKY_MODULE_H 41 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 42 | -------------------------------------------------------------------------------- /lib/Eigen/SPQRSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPQRSUPPORT_MODULE_H 9 | #define EIGEN_SPQRSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SuiteSparseQR.hpp" 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup SPQRSupport_Module SuiteSparseQR module 19 | * 20 | * This module provides an interface to the SPQR library, which is part of the suitesparse package. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...). 27 | * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules 28 | * 29 | */ 30 | 31 | #include "src/CholmodSupport/CholmodSupport.h" 32 | #include "src/SPQRSupport/SuiteSparseQRSupport.h" 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/matrix/Vector2.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Vector2.hpp 3 | * 4 | * 2D vector class. 5 | * 6 | * @author James Goppert 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "math.hpp" 12 | 13 | namespace matrix 14 | { 15 | 16 | template 17 | class Vector; 18 | 19 | template 20 | class Vector2 : public Vector 21 | { 22 | public: 23 | 24 | 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 | -------------------------------------------------------------------------------- /config/11_GPSUpdate.txt: -------------------------------------------------------------------------------- 1 | INCLUDE Simulation.txt 2 | INCLUDE QuadPhysicalParams.txt 3 | INCLUDE SimulatedSensors.txt 4 | INCLUDE QuadControlParams.txt 5 | INCLUDE QuadEstimatorEKF.txt 6 | 7 | # BASIC 8 | Sim.RunMode = Repeat 9 | Sim.EndTime = 25 10 | Sim.Vehicle1 = Quad 11 | 12 | # QUAD SETUP 13 | Quad.InitialPos=0,0,-1 14 | Quad.Sensors = SimIMU, SimMag, SimGPS 15 | Quad.ControlType = QuadControl 16 | Quad.ControlConfig = QuadControlParams 17 | QuadControlParams.Trajectory=traj/Square.txt 18 | 19 | ###################################### 20 | # # 21 | # IDEAL SENSORS? IDEAL ESTIMATOR? # 22 | # # 23 | ###################################### 24 | 25 | Quad.UseIdealEstimator = 1 26 | SimIMU.AccelStd = 0,0,0 27 | SimIMU.GyroStd = 0,0,0 28 | 29 | ###################################### 30 | 31 | # GRAPHING 32 | Commands += Toggle.ActualTrajectory 33 | 34 | Commands += SetTitle(1,"Y Pos/Vel: True vs Predicted") 35 | Commands += Plot(1,Quad.Pos.Y,"true Y") 36 | Commands += Plot(1,Quad.Est.Y,"est Y") 37 | Commands += Plot(1,Quad.Vel.Y,"true vY") 38 | Commands += Plot(1,Quad.Est.VY,"est vY") 39 | 40 | Commands += SetTitle(2,"Pos error vs Z std") 41 | Commands += AddGraph2.Quad.Est.E.Pos 42 | Commands += AddGraph2.Quad.Est.S.Z 43 | 44 | Commands += AddGraph2.WindowThreshold(Quad.Est.E.Pos,1,20) 45 | 46 | -------------------------------------------------------------------------------- /lib/Eigen/SparseQR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSEQR_MODULE_H 9 | #define EIGEN_SPARSEQR_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup SparseQR_Module SparseQR module 16 | * \brief Provides QR decomposition for sparse matrices 17 | * 18 | * This module provides a simplicial version of the left-looking Sparse QR decomposition. 19 | * The columns of the input matrix should be reordered to limit the fill-in during the 20 | * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. 21 | * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 22 | * of built-in and external ordering methods. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | * 28 | * 29 | */ 30 | 31 | #include "OrderingMethods" 32 | #include "src/SparseCore/SparseColEtree.h" 33 | #include "src/SparseQR/SparseQR.h" 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /config/08_PredictState.txt: -------------------------------------------------------------------------------- 1 | INCLUDE Simulation.txt 2 | INCLUDE QuadPhysicalParams.txt 3 | INCLUDE SimulatedSensors.txt 4 | INCLUDE QuadControlParams.txt 5 | INCLUDE QuadEstimatorEKF.txt 6 | 7 | # BASIC 8 | Sim.RunMode = Repeat 9 | Sim.EndTime = 40 10 | Sim.Vehicle1 = Quad 11 | Quad.InitialPos=0,0,-1 12 | 13 | # REFERENCE 14 | QuadControlParams.Trajectory=traj/Square.txt 15 | 16 | # CONTROLLER 17 | Quad.ControlType = QuadControl 18 | Quad.ControlConfig = QuadControlParams 19 | 20 | # SENSORS 21 | Quad.Sensors = SimIMU 22 | # use ideal sensors 23 | SimIMU.AccelStd = 0,0,0 24 | SimIMU.GyroStd = 0,0,0 25 | 26 | # ESTIMATION 27 | # We set the attitude time constant very high to effectively disable attitude updates from accelerometers 28 | # with perfect rate gyros, the accel attitude updates only introduce additional errors 29 | QuadEstimatorEKF.attitudeTau = 100 30 | 31 | # GRAPHING 32 | Commands += Toggle.ActualTrajectory 33 | 34 | Commands += SetTitle(1,"Y Pos/Vel: True vs Predicted") 35 | Commands += Plot(1,Quad.Pos.Y,"true Y") 36 | Commands += Plot(1,Quad.Est.Y,"est Y") 37 | Commands += Plot(1,Quad.Vel.Y,"true vY") 38 | Commands += Plot(1,Quad.Est.VY,"est vY") 39 | 40 | Commands += SetTitle(2,"Z Pos/Vel: True vs Predicted") 41 | Commands += Plot(2,Quad.Pos.Z,"true z") 42 | Commands += Plot(2,Quad.Est.Z,"est z") 43 | Commands += Plot(2,Quad.Vel.Z,"true vz") 44 | Commands += Plot(2,Quad.Est.VZ,"est vz") -------------------------------------------------------------------------------- /lib/Eigen/src/Core/arch/AVX/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_AVX_H 11 | #define EIGEN_TYPE_CASTING_AVX_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // For now we use SSE to handle integers, so we can't use AVX instructions to cast 18 | // from int to float 19 | template <> 20 | struct type_casting_traits { 21 | enum { 22 | VectorizedCast = 0, 23 | SrcCoeffRatio = 1, 24 | TgtCoeffRatio = 1 25 | }; 26 | }; 27 | 28 | template <> 29 | struct type_casting_traits { 30 | enum { 31 | VectorizedCast = 0, 32 | SrcCoeffRatio = 1, 33 | TgtCoeffRatio = 1 34 | }; 35 | }; 36 | 37 | 38 | 39 | template<> EIGEN_STRONG_INLINE Packet8i pcast(const Packet8f& a) { 40 | return _mm256_cvtps_epi32(a); 41 | } 42 | 43 | template<> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8i& a) { 44 | return _mm256_cvtepi32_ps(a); 45 | } 46 | 47 | } // end namespace internal 48 | 49 | } // end namespace Eigen 50 | 51 | #endif // EIGEN_TYPE_CASTING_AVX_H 52 | -------------------------------------------------------------------------------- /config/04_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 -------------------------------------------------------------------------------- /lib/Eigen/QR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QR_MODULE_H 9 | #define EIGEN_QR_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | 19 | /** \defgroup QR_Module QR module 20 | * 21 | * 22 | * 23 | * This module provides various QR decompositions 24 | * This module also provides some MatrixBase methods, including: 25 | * - MatrixBase::householderQr() 26 | * - MatrixBase::colPivHouseholderQr() 27 | * - MatrixBase::fullPivHouseholderQr() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/QR/HouseholderQR.h" 35 | #include "src/QR/FullPivHouseholderQR.h" 36 | #include "src/QR/ColPivHouseholderQR.h" 37 | #include "src/QR/CompleteOrthogonalDecomposition.h" 38 | #ifdef EIGEN_USE_LAPACKE 39 | #include "src/misc/lapacke.h" 40 | #include "src/QR/HouseholderQR_LAPACKE.h" 41 | #include "src/QR/ColPivHouseholderQR_LAPACKE.h" 42 | #endif 43 | 44 | #include "src/Core/util/ReenableStupidWarnings.h" 45 | 46 | #endif // EIGEN_QR_MODULE_H 47 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(CPPEstSim) 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(CPPEstSim 44 | ${SOURCES} 45 | ${HEADERS} 46 | ) 47 | 48 | target_link_libraries(CPPEstSim 49 | Qt5::Core 50 | Qt5::Network 51 | Qt5::Widgets 52 | ${GLUT_LIBRARIES} 53 | ${OPENGL_LIBRARIES} 54 | pthread 55 | ) 56 | -------------------------------------------------------------------------------- /config/06_SensorNoise.txt: -------------------------------------------------------------------------------- 1 | ### STUDENT SECTION 2 | 3 | MeasuredStdDev_GPSPosXY = 2 4 | MeasuredStdDev_AccelXY = .1 5 | 6 | ### END STUDENT SECTION 7 | 8 | INCLUDE Simulation.txt 9 | INCLUDE QuadPhysicalParams.txt 10 | INCLUDE SimulatedSensors.txt 11 | INCLUDE QuadControlParams.txt 12 | 13 | # simulation setup 14 | Sim.RunMode = Repeat 15 | Sim.EndTime = 10 16 | Sim.Vehicle1 = Quad 17 | 18 | # Controller selection 19 | Quad.ControlType = QuadControl 20 | Quad.ControlConfig = QuadControlParams 21 | 22 | # reference trajectory 23 | QuadControlParams.Trajectory=0,0,-1 24 | 25 | # SigmaThreshold syntax: 26 | # SigmaThreshold(signal, refValue, sigma, minOKPercent, maxOKPercent, okWindow) 27 | # looks at the percentage of points from being inside the range of [refValue-sigma, refValue+sigma] 28 | # if this percentage is inside [minOKPercent,maxOKPercent] for at least okWindow seconds, 29 | # the threshold visualization turns green 30 | 31 | # graphing commands 32 | Commands += SetTitle(1,"GPS") 33 | Commands += Plot(1,Quad.GPS.X,"x position",0,1,1) 34 | Commands += AddGraph1.SigmaThreshold(Quad.GPS.X, Quad.Pos.X, MeasuredStdDev_GPSPosXY,64,73,2) 35 | Commands += AddGraph1.LogToFile 36 | 37 | Commands += SetTitle(2,"Accelerometer") 38 | Commands += Plot(2,Quad.IMU.AX,"x accel",.5,.7,1) 39 | Commands += AddGraph2.SigmaThreshold(Quad.IMU.AX, 0, MeasuredStdDev_AccelXY,64,73,2) 40 | Commands += AddGraph2.LogToFile 41 | 42 | Quad.Sensors = SimIMU, SimGPS, SimMag 43 | 44 | # Vehicle-specific config 45 | [Quad] 46 | InitialPos=0,0,-1 -------------------------------------------------------------------------------- /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|x86 = Release|x86 13 | Release|x64 = Release|x64 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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/Eigen/UmfPackSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_UMFPACKSUPPORT_MODULE_H 9 | #define EIGEN_UMFPACKSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup UmfPackSupport_Module UmfPackSupport module 21 | * 22 | * This module provides an interface to the UmfPack library which is part of the suitesparse package. 23 | * It provides the following factorization class: 24 | * - class UmfPackLU: a multifrontal sequential LU factorization. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | * 30 | * In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies. 31 | * The dependencies depend on how umfpack has been compiled. 32 | * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task. 33 | * 34 | */ 35 | 36 | #include "src/UmfPackSupport/UmfPackSupport.h" 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_UMFPACKSUPPORT_MODULE_H 41 | -------------------------------------------------------------------------------- /lib/Eigen/LU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_LU_MODULE_H 9 | #define EIGEN_LU_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup LU_Module LU module 16 | * This module includes %LU decomposition and related notions such as matrix inversion and determinant. 17 | * This module defines the following MatrixBase methods: 18 | * - MatrixBase::inverse() 19 | * - MatrixBase::determinant() 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | 26 | #include "src/misc/Kernel.h" 27 | #include "src/misc/Image.h" 28 | #include "src/LU/FullPivLU.h" 29 | #include "src/LU/PartialPivLU.h" 30 | #ifdef EIGEN_USE_LAPACKE 31 | #include "src/misc/lapacke.h" 32 | #include "src/LU/PartialPivLU_LAPACKE.h" 33 | #endif 34 | #include "src/LU/Determinant.h" 35 | #include "src/LU/InverseImpl.h" 36 | 37 | // Use the SSE optimized version whenever possible. At the moment the 38 | // SSE version doesn't compile when AVX is enabled 39 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 40 | #include "src/LU/arch/Inverse_SSE.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_LU_MODULE_H 46 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 47 | -------------------------------------------------------------------------------- /lib/Eigen/SparseCholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2013 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSECHOLESKY_MODULE_H 11 | #define EIGEN_SPARSECHOLESKY_MODULE_H 12 | 13 | #include "SparseCore" 14 | #include "OrderingMethods" 15 | 16 | #include "src/Core/util/DisableStupidWarnings.h" 17 | 18 | /** 19 | * \defgroup SparseCholesky_Module SparseCholesky module 20 | * 21 | * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices. 22 | * Those decompositions are accessible via the following classes: 23 | * - SimplicialLLt, 24 | * - SimplicialLDLt 25 | * 26 | * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module. 27 | * 28 | * \code 29 | * #include 30 | * \endcode 31 | */ 32 | 33 | #ifdef EIGEN_MPL2_ONLY 34 | #error The SparseCholesky module has nothing to offer in MPL2 only mode 35 | #endif 36 | 37 | #include "src/SparseCholesky/SimplicialCholesky.h" 38 | 39 | #ifndef EIGEN_MPL2_ONLY 40 | #include "src/SparseCholesky/SimplicialCholesky_impl.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_SPARSECHOLESKY_MODULE_H 46 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 -------------------------------------------------------------------------------- /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 name, string config) : BaseController(name,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/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 | #define MAX_POINTS 10000 13 | 14 | class Graph 15 | { 16 | public: 17 | Graph(const char* name); 18 | void Reset(); 19 | void Clear(); 20 | void Update(double time, std::vector >& sources); 21 | 22 | void Draw(); 23 | void AddItem(string path); 24 | void AddItem(string path, vector options); 25 | void AddSeries(string path, bool autoColor = true, V3F color = V3F(), vector options=vector()); 26 | void AddAbsThreshold(string path); 27 | void AddWindowThreshold(string path); 28 | void SetYAxis(string argsString); 29 | void AddSigmaThreshold(string path); 30 | bool IsSeriesPlotted(string path); 31 | void RemoveAllElements(); 32 | void SetTitle(string title) { _title = title; } 33 | 34 | void BeginLogToFile(); 35 | 36 | 37 | struct Series 38 | { 39 | Series(); 40 | V3F _color; 41 | string _yName, _legend; 42 | string _objName, _fieldName; 43 | FixedQueue x; 44 | FixedQueue y; 45 | bool noLegend, bold, negate; 46 | void Clear() 47 | { 48 | x.reset(); 49 | y.reset(); 50 | } 51 | }; 52 | 53 | vector > _analyzers; 54 | 55 | void DrawSeries(Series& s); 56 | 57 | vector _series; 58 | string _name; 59 | 60 | FILE* _logFile; 61 | 62 | float _graphYLow, _graphYHigh; 63 | string _title; 64 | }; -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/Eigen/SVD: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SVD_MODULE_H 9 | #define EIGEN_SVD_MODULE_H 10 | 11 | #include "QR" 12 | #include "Householder" 13 | #include "Jacobi" 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | /** \defgroup SVD_Module SVD module 18 | * 19 | * 20 | * 21 | * This module provides SVD decomposition for matrices (both real and complex). 22 | * Two decomposition algorithms are provided: 23 | * - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones. 24 | * - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems. 25 | * These decompositions are accessible via the respective classes and following MatrixBase methods: 26 | * - MatrixBase::jacobiSvd() 27 | * - MatrixBase::bdcSvd() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/misc/RealSvd2x2.h" 35 | #include "src/SVD/UpperBidiagonalization.h" 36 | #include "src/SVD/SVDBase.h" 37 | #include "src/SVD/JacobiSVD.h" 38 | #include "src/SVD/BDCSVD.h" 39 | #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) 40 | #include "src/misc/lapacke.h" 41 | #include "src/SVD/JacobiSVD_LAPACKE.h" 42 | #endif 43 | 44 | #include "src/Core/util/ReenableStupidWarnings.h" 45 | 46 | #endif // EIGEN_SVD_MODULE_H 47 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 48 | -------------------------------------------------------------------------------- /lib/Eigen/PaStiXSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PASTIXSUPPORT_MODULE_H 9 | #define EIGEN_PASTIXSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | #include 18 | } 19 | 20 | #ifdef complex 21 | #undef complex 22 | #endif 23 | 24 | /** \ingroup Support_modules 25 | * \defgroup PaStiXSupport_Module PaStiXSupport module 26 | * 27 | * This module provides an interface to the PaSTiX library. 28 | * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver. 29 | * It provides the two following main factorization classes: 30 | * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization. 31 | * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization. 32 | * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern). 33 | * 34 | * \code 35 | * #include 36 | * \endcode 37 | * 38 | * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies. 39 | * The dependencies depend on how PaSTiX has been compiled. 40 | * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task. 41 | * 42 | */ 43 | 44 | #include "src/PaStiXSupport/PaStiXSupport.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_PASTIXSUPPORT_MODULE_H 49 | -------------------------------------------------------------------------------- /lib/Eigen/SparseLU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // Copyright (C) 2012 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_SPARSELU_MODULE_H 12 | #define EIGEN_SPARSELU_MODULE_H 13 | 14 | #include "SparseCore" 15 | 16 | /** 17 | * \defgroup SparseLU_Module SparseLU module 18 | * This module defines a supernodal factorization of general sparse matrices. 19 | * The code is fully optimized for supernode-panel updates with specialized kernels. 20 | * Please, see the documentation of the SparseLU class for more details. 21 | */ 22 | 23 | // Ordering interface 24 | #include "OrderingMethods" 25 | 26 | #include "src/SparseLU/SparseLU_gemm_kernel.h" 27 | 28 | #include "src/SparseLU/SparseLU_Structs.h" 29 | #include "src/SparseLU/SparseLU_SupernodalMatrix.h" 30 | #include "src/SparseLU/SparseLUImpl.h" 31 | #include "src/SparseCore/SparseColEtree.h" 32 | #include "src/SparseLU/SparseLU_Memory.h" 33 | #include "src/SparseLU/SparseLU_heap_relax_snode.h" 34 | #include "src/SparseLU/SparseLU_relax_snode.h" 35 | #include "src/SparseLU/SparseLU_pivotL.h" 36 | #include "src/SparseLU/SparseLU_panel_dfs.h" 37 | #include "src/SparseLU/SparseLU_kernel_bmod.h" 38 | #include "src/SparseLU/SparseLU_panel_bmod.h" 39 | #include "src/SparseLU/SparseLU_column_dfs.h" 40 | #include "src/SparseLU/SparseLU_column_bmod.h" 41 | #include "src/SparseLU/SparseLU_copy_to_ucol.h" 42 | #include "src/SparseLU/SparseLU_pruneL.h" 43 | #include "src/SparseLU/SparseLU_Utils.h" 44 | #include "src/SparseLU/SparseLU.h" 45 | 46 | #endif // EIGEN_SPARSELU_MODULE_H 47 | -------------------------------------------------------------------------------- /lib/Eigen/src/SparseCore/SparseRedux.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSEREDUX_H 11 | #define EIGEN_SPARSEREDUX_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | typename internal::traits::Scalar 17 | SparseMatrixBase::sum() const 18 | { 19 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 20 | Scalar res(0); 21 | internal::evaluator thisEval(derived()); 22 | for (Index j=0; j::InnerIterator iter(thisEval,j); iter; ++iter) 24 | res += iter.value(); 25 | return res; 26 | } 27 | 28 | template 29 | typename internal::traits >::Scalar 30 | SparseMatrix<_Scalar,_Options,_Index>::sum() const 31 | { 32 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 33 | if(this->isCompressed()) 34 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 35 | else 36 | return Base::sum(); 37 | } 38 | 39 | template 40 | typename internal::traits >::Scalar 41 | SparseVector<_Scalar,_Options,_Index>::sum() const 42 | { 43 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 44 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 45 | } 46 | 47 | } // end namespace Eigen 48 | 49 | #endif // EIGEN_SPARSEREDUX_H 50 | -------------------------------------------------------------------------------- /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 name, string config); 19 | virtual ~BaseController() {}; 20 | 21 | virtual VehicleCommand RunControl(float dt, float sim_time) { return VehicleCommand(); }; 22 | 23 | virtual void Init(); 24 | virtual void Reset(); 25 | 26 | TrajectoryPoint GetNextTrajectoryPoint(float mission_time); 27 | 28 | // update the vehicle state estimates the controller will use to do control 29 | virtual void UpdateEstimates(V3F pos, V3F vel, Quaternion attitude, V3F omega); 30 | 31 | // Access functions for graphing variables 32 | virtual bool GetData(const string& name, float& ret) const; 33 | virtual vector GetFields() const; 34 | 35 | void SetTrajectoryOffset(V3F trajOffset) { _trajectoryOffset = trajOffset; } 36 | void SetTrajTimeOffset(float timeOffset) { _trajectoryTimeOffset = timeOffset; } 37 | 38 | // system parameters params 39 | float mass; // mass 40 | float L; // length of arm from centre of quadrocopter to motor 41 | float Ixx, Iyy, Izz; // mass moment of inertia / second moment of inertia 42 | float kappa; // torque (Nm) produced by motor per N of thrust produced 43 | 44 | // controller input (reference state) 45 | int mode; 46 | VehicleCommand cmd; 47 | 48 | // Estimator state 49 | Quaternion estAtt; 50 | V3F estVel; 51 | V3F estPos; 52 | V3F estOmega; 53 | 54 | Trajectory trajectory; 55 | TrajectoryPoint curTrajPoint; 56 | string _config; 57 | string _name; 58 | 59 | V3F _trajectoryOffset; 60 | float _trajectoryTimeOffset; 61 | }; 62 | 63 | -------------------------------------------------------------------------------- /config/X_MonteCarloTest.txt: -------------------------------------------------------------------------------- 1 | INCLUDE Simulation.txt 2 | INCLUDE QuadPhysicalParams.txt 3 | INCLUDE SimulatedSensors.txt 4 | INCLUDE QuadEstimatorEKF.txt 5 | INCLUDE QuadControlParams.txt 6 | 7 | # BASIC 8 | Sim.RunMode = Repeat 9 | Sim.EndTime = 25 10 | Sim.Vehicle1 = Quad1 11 | Sim.Vehicle2 = Quad2 12 | Sim.Vehicle3 = Quad3 13 | Sim.Vehicle4 = Quad4 14 | Sim.Vehicle5 = Quad5 15 | Sim.Vehicle6 = Quad6 16 | Sim.Vehicle7 = Quad7 17 | Sim.Vehicle8 = Quad8 18 | Sim.Vehicle9 = Quad9 19 | Sim.Vehicle10 = Quad10 20 | Sim.Vehicle11 = Quad11 21 | Sim.Vehicle12 = Quad12 22 | Sim.Vehicle13 = Quad13 23 | Sim.Vehicle14 = Quad14 24 | Sim.Vehicle15 = Quad15 25 | Sim.Vehicle16 = Quad16 26 | Sim.Vehicle17 = Quad17 27 | Sim.Vehicle18 = Quad18 28 | Sim.Vehicle19 = Quad19 29 | Sim.Vehicle20 = Quad20 30 | 31 | # QUAD SETUP 32 | Quad.InitialPos=0,0,-1 33 | Quad.Sensors = SimIMU, SimMag, SimGPS 34 | Quad.ControlType = QuadControl 35 | Quad.ControlConfig = QuadControlParams 36 | QuadControlParams.Trajectory=traj/Square.txt 37 | 38 | ###################################### 39 | # # 40 | # IDEAL SENSORS? IDEAL ESTIMATOR? # 41 | # # 42 | ###################################### 43 | 44 | Quad.UseIdealEstimator = 0 45 | #SimIMU.AccelStd = 0,0,0 46 | #SimIMU.GyroStd = 0,0,0 47 | 48 | ###################################### 49 | 50 | [Quad1 : Quad] 51 | [Quad2 : Quad] 52 | [Quad3 : Quad] 53 | [Quad4 : Quad] 54 | [Quad5 : Quad] 55 | [Quad6 : Quad] 56 | [Quad7 : Quad] 57 | [Quad8 : Quad] 58 | [Quad9 : Quad] 59 | [Quad10 : Quad] 60 | [Quad11 : Quad] 61 | [Quad12 : Quad] 62 | [Quad13 : Quad] 63 | [Quad14 : Quad] 64 | [Quad15 : Quad] 65 | [Quad16 : Quad] 66 | [Quad17 : Quad] 67 | [Quad18 : Quad] 68 | [Quad19 : Quad] 69 | [Quad20 : Quad] 70 | 71 | [] 72 | 73 | # GRAPHING 74 | Commands += Toggle.ActualTrajectory 75 | Commands += AddGraph2.WindowThreshold(Quad.Est.E.Pos,1,20) 76 | _DEBUG.NO_QUAD_GRAPHS=1 77 | 78 | -------------------------------------------------------------------------------- /.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 nd787 - Flying Car ND | 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":"nd787 - Flying Car ND"}], "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 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/arch/Default/Settings.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2010 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | 12 | /* All the parameters defined in this file can be specialized in the 13 | * architecture specific files, and/or by the user. 14 | * More to come... */ 15 | 16 | #ifndef EIGEN_DEFAULT_SETTINGS_H 17 | #define EIGEN_DEFAULT_SETTINGS_H 18 | 19 | /** Defines the maximal loop size to enable meta unrolling of loops. 20 | * Note that the value here is expressed in Eigen's own notion of "number of FLOPS", 21 | * it does not correspond to the number of iterations or the number of instructions 22 | */ 23 | #ifndef EIGEN_UNROLLING_LIMIT 24 | #define EIGEN_UNROLLING_LIMIT 100 25 | #endif 26 | 27 | /** Defines the threshold between a "small" and a "large" matrix. 28 | * This threshold is mainly used to select the proper product implementation. 29 | */ 30 | #ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 31 | #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8 32 | #endif 33 | 34 | /** Defines the maximal width of the blocks used in the triangular product and solver 35 | * for vectors (level 2 blas xTRMV and xTRSV). The default is 8. 36 | */ 37 | #ifndef EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 38 | #define EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 8 39 | #endif 40 | 41 | 42 | /** Defines the default number of registers available for that architecture. 43 | * Currently it must be 8 or 16. Other values will fail. 44 | */ 45 | #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 46 | #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 8 47 | #endif 48 | 49 | #endif // EIGEN_DEFAULT_SETTINGS_H 50 | -------------------------------------------------------------------------------- /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 | #define GLD_ALIGN_LEFT -1 24 | #define GLD_ALIGN_CENTER 0 25 | #define GLD_ALIGN_RIGHT 1 26 | void DrawStrokeText_Align(const char* str, float x, float y, float z, float lineWidth, float scaleX = 1, float scaleY = 1, int align=GLD_ALIGN_CENTER); 27 | 28 | using SLR::Quaternion; 29 | 30 | namespace SLR { 31 | 32 | // Utility class for helping with OpenGL drawing 33 | 34 | class OpenGLDrawer 35 | { 36 | public: 37 | OpenGLDrawer(); 38 | ~OpenGLDrawer(); 39 | 40 | void PushLighting(); 41 | void PopLighting(); 42 | void SetLighting(bool enable); 43 | 44 | void DrawQuadrotor2(V3F pos, Quaternion att, V3F color, V3F centerOffset, float centerScale, float armLength); 45 | 46 | void DrawArrow(double len, double r1, double r2, double arrowLen); 47 | void DrawArrow(V3D from, V3D to, V3D color); 48 | 49 | GLUquadricObj* Quadric() { return _glQuadric; }; 50 | 51 | V3D cameraPos; 52 | 53 | protected: 54 | GLUquadricObj* _glQuadric; 55 | }; 56 | 57 | } // namespace SLR 58 | -------------------------------------------------------------------------------- /lib/Eigen/src/misc/RealSvd2x2.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Benoit Jacob 5 | // Copyright (C) 2013-2016 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_REALSVD2X2_H 12 | #define EIGEN_REALSVD2X2_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | template 19 | void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, 20 | JacobiRotation *j_left, 21 | JacobiRotation *j_right) 22 | { 23 | using std::sqrt; 24 | using std::abs; 25 | Matrix m; 26 | m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)), 27 | numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q)); 28 | JacobiRotation rot1; 29 | RealScalar t = m.coeff(0,0) + m.coeff(1,1); 30 | RealScalar d = m.coeff(1,0) - m.coeff(0,1); 31 | 32 | if(abs(d) < (std::numeric_limits::min)()) 33 | { 34 | rot1.s() = RealScalar(0); 35 | rot1.c() = RealScalar(1); 36 | } 37 | else 38 | { 39 | // If d!=0, then t/d cannot overflow because the magnitude of the 40 | // entries forming d are not too small compared to the ones forming t. 41 | RealScalar u = t / d; 42 | RealScalar tmp = sqrt(RealScalar(1) + numext::abs2(u)); 43 | rot1.s() = RealScalar(1) / tmp; 44 | rot1.c() = u / tmp; 45 | } 46 | m.applyOnTheLeft(0,1,rot1); 47 | j_right->makeJacobi(m,0,1); 48 | *j_left = rot1 * j_right->transpose(); 49 | } 50 | 51 | } // end namespace internal 52 | 53 | } // end namespace Eigen 54 | 55 | #endif // EIGEN_REALSVD2X2_H 56 | -------------------------------------------------------------------------------- /lib/Eigen/Eigenvalues: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_EIGENVALUES_MODULE_H 9 | #define EIGEN_EIGENVALUES_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | #include "LU" 19 | #include "Geometry" 20 | 21 | /** \defgroup Eigenvalues_Module Eigenvalues module 22 | * 23 | * 24 | * 25 | * This module mainly provides various eigenvalue solvers. 26 | * This module also provides some MatrixBase methods, including: 27 | * - MatrixBase::eigenvalues(), 28 | * - MatrixBase::operatorNorm() 29 | * 30 | * \code 31 | * #include 32 | * \endcode 33 | */ 34 | 35 | #include "src/misc/RealSvd2x2.h" 36 | #include "src/Eigenvalues/Tridiagonalization.h" 37 | #include "src/Eigenvalues/RealSchur.h" 38 | #include "src/Eigenvalues/EigenSolver.h" 39 | #include "src/Eigenvalues/SelfAdjointEigenSolver.h" 40 | #include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" 41 | #include "src/Eigenvalues/HessenbergDecomposition.h" 42 | #include "src/Eigenvalues/ComplexSchur.h" 43 | #include "src/Eigenvalues/ComplexEigenSolver.h" 44 | #include "src/Eigenvalues/RealQZ.h" 45 | #include "src/Eigenvalues/GeneralizedEigenSolver.h" 46 | #include "src/Eigenvalues/MatrixBaseEigenvalues.h" 47 | #ifdef EIGEN_USE_LAPACKE 48 | #include "src/misc/lapacke.h" 49 | #include "src/Eigenvalues/RealSchur_LAPACKE.h" 50 | #include "src/Eigenvalues/ComplexSchur_LAPACKE.h" 51 | #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" 52 | #endif 53 | 54 | #include "src/Core/util/ReenableStupidWarnings.h" 55 | 56 | #endif // EIGEN_EIGENVALUES_MODULE_H 57 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 58 | -------------------------------------------------------------------------------- /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 dot(const V4D& b) const 64 | { 65 | return (v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3]); 66 | } 67 | 68 | double operator[](int i) const { return v[i]; } 69 | double operator()(int i) const { return v[i]; } 70 | double & operator[](int i) { return v[i]; } 71 | double & operator()(int i) { return v[i]; } 72 | 73 | double v[4]; 74 | }; 75 | 76 | inline V4D element_prod(const V4D& a, const V4D& b) 77 | { 78 | 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]); 79 | } 80 | 81 | inline double sum(const V4D& a) 82 | { 83 | return a.v[0] + a.v[1] + a.v[2] + a.v[3]; 84 | } 85 | 86 | inline double norm_2(const V4D& a) 87 | { 88 | return a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]; 89 | 90 | } -------------------------------------------------------------------------------- /lib/Eigen/src/Core/arch/SSE/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_SSE_H 11 | #define EIGEN_TYPE_CASTING_SSE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | template <> 18 | struct type_casting_traits { 19 | enum { 20 | VectorizedCast = 1, 21 | SrcCoeffRatio = 1, 22 | TgtCoeffRatio = 1 23 | }; 24 | }; 25 | 26 | template<> EIGEN_STRONG_INLINE Packet4i pcast(const Packet4f& a) { 27 | return _mm_cvttps_epi32(a); 28 | } 29 | 30 | 31 | template <> 32 | struct type_casting_traits { 33 | enum { 34 | VectorizedCast = 1, 35 | SrcCoeffRatio = 1, 36 | TgtCoeffRatio = 1 37 | }; 38 | }; 39 | 40 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet4i& a) { 41 | return _mm_cvtepi32_ps(a); 42 | } 43 | 44 | 45 | template <> 46 | struct type_casting_traits { 47 | enum { 48 | VectorizedCast = 1, 49 | SrcCoeffRatio = 2, 50 | TgtCoeffRatio = 1 51 | }; 52 | }; 53 | 54 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet2d& a, const Packet2d& b) { 55 | return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6)); 56 | } 57 | 58 | template <> 59 | struct type_casting_traits { 60 | enum { 61 | VectorizedCast = 1, 62 | SrcCoeffRatio = 1, 63 | TgtCoeffRatio = 2 64 | }; 65 | }; 66 | 67 | template<> EIGEN_STRONG_INLINE Packet2d pcast(const Packet4f& a) { 68 | // Simply discard the second half of the input 69 | return _mm_cvtps_pd(a); 70 | } 71 | 72 | 73 | } // end namespace internal 74 | 75 | } // end namespace Eigen 76 | 77 | #endif // EIGEN_TYPE_CASTING_SSE_H 78 | -------------------------------------------------------------------------------- /lib/Eigen/CholmodSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLMODSUPPORT_MODULE_H 9 | #define EIGEN_CHOLMODSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup CholmodSupport_Module CholmodSupport module 21 | * 22 | * This module provides an interface to the Cholmod library which is part of the suitesparse package. 23 | * It provides the two following main factorization classes: 24 | * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. 25 | * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). 26 | * 27 | * For the sake of completeness, this module also propose the two following classes: 28 | * - class CholmodSimplicialLLT 29 | * - class CholmodSimplicialLDLT 30 | * Note that these classes does not bring any particular advantage compared to the built-in 31 | * SimplicialLLT and SimplicialLDLT factorization classes. 32 | * 33 | * \code 34 | * #include 35 | * \endcode 36 | * 37 | * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies. 38 | * The dependencies depend on how cholmod has been compiled. 39 | * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task. 40 | * 41 | */ 42 | 43 | #include "src/CholmodSupport/CholmodSupport.h" 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_CHOLMODSUPPORT_MODULE_H 48 | 49 | -------------------------------------------------------------------------------- /src/Utility/SimpleConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "matrix/math.hpp" 6 | using std::vector; 7 | using std::map; 8 | 9 | #include "Eigen/Dense" 10 | using Eigen::MatrixXf; 11 | using Eigen::VectorXf; 12 | 13 | 14 | namespace SLR{ 15 | 16 | class SimpleConfig; 17 | typedef shared_ptr ParamsHandle; 18 | 19 | 20 | class SimpleConfig 21 | { 22 | SimpleConfig(); 23 | void ReadFile(const string& filename, int depth=0); 24 | 25 | public: 26 | static ParamsHandle GetInstance(); 27 | void Reset(string rootParam); 28 | 29 | bool Exists(const string& param); 30 | bool GetFloat(const string& param, float& ret); 31 | bool GetString(const string& param, string& ret); 32 | bool GetV3F(const string& param, V3F& ret); 33 | bool GetFloatVector(const string& param, vector& ret); 34 | 35 | template 36 | inline bool GetFloatVector(const string& param, matrix::Vector& ret) 37 | { 38 | vector tmp; 39 | if (!GetFloatVector(param, tmp)) return false; 40 | if (tmp.size() != N) return false; 41 | for (size_t i = 0; i < N; i++) 42 | { 43 | ret(i) = tmp[i]; 44 | } 45 | return true; 46 | } 47 | 48 | inline bool GetFloatVector(const string& param, VectorXf& ret) 49 | { 50 | vector tmp; 51 | if (!GetFloatVector(param, tmp)) return false; 52 | ret.resize(tmp.size()); 53 | for (size_t i = 0; i < tmp.size(); i++) 54 | { 55 | ret(i) = tmp[i]; 56 | } 57 | return true; 58 | } 59 | 60 | // convenience always-returning functions, with defaults 61 | float Get(const string& param, float defaultRet); 62 | string Get(const string& param, string defaultRet); 63 | V3F Get(const string& param, V3F defaultRet); 64 | 65 | void PrintAll(); 66 | 67 | protected: 68 | static shared_ptr s_config; 69 | map _params; 70 | void ParseLine(const string& filename, const string& ln, int lineNum, string& curNamespace, int depth); 71 | void CopyNamespaceParams(const string& fromNamespace, const string& toNamespace); 72 | }; 73 | 74 | 75 | 76 | } // namespace SLR 77 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/SelfCwiseBinaryOp.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SELFCWISEBINARYOP_H 11 | #define EIGEN_SELFCWISEBINARYOP_H 12 | 13 | namespace Eigen { 14 | 15 | // TODO generalize the scalar type of 'other' 16 | 17 | template 18 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator*=(const Scalar& other) 19 | { 20 | typedef typename Derived::PlainObject PlainObject; 21 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op()); 22 | return derived(); 23 | } 24 | 25 | template 26 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const Scalar& other) 27 | { 28 | typedef typename Derived::PlainObject PlainObject; 29 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op()); 30 | return derived(); 31 | } 32 | 33 | template 34 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const Scalar& other) 35 | { 36 | typedef typename Derived::PlainObject PlainObject; 37 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op()); 38 | return derived(); 39 | } 40 | 41 | template 42 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator/=(const Scalar& other) 43 | { 44 | typedef typename Derived::PlainObject PlainObject; 45 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op()); 46 | return derived(); 47 | } 48 | 49 | } // end namespace Eigen 50 | 51 | #endif // EIGEN_SELFCWISEBINARYOP_H 52 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/Eigen/IterativeLinearSolvers: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 9 | #define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** 17 | * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module 18 | * 19 | * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. 20 | * Those solvers are accessible via the following classes: 21 | * - ConjugateGradient for selfadjoint (hermitian) matrices, 22 | * - LeastSquaresConjugateGradient for rectangular least-square problems, 23 | * - BiCGSTAB for general square matrices. 24 | * 25 | * These iterative solvers are associated with some preconditioners: 26 | * - IdentityPreconditioner - not really useful 27 | * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. 28 | * - IncompleteLUT - incomplete LU factorization with dual thresholding 29 | * 30 | * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. 31 | * 32 | \code 33 | #include 34 | \endcode 35 | */ 36 | 37 | #include "src/IterativeLinearSolvers/SolveWithGuess.h" 38 | #include "src/IterativeLinearSolvers/IterativeSolverBase.h" 39 | #include "src/IterativeLinearSolvers/BasicPreconditioners.h" 40 | #include "src/IterativeLinearSolvers/ConjugateGradient.h" 41 | #include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" 42 | #include "src/IterativeLinearSolvers/BiCGSTAB.h" 43 | #include "src/IterativeLinearSolvers/IncompleteLUT.h" 44 | #include "src/IterativeLinearSolvers/IncompleteCholesky.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 49 | -------------------------------------------------------------------------------- /lib/Eigen/Geometry: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_GEOMETRY_MODULE_H 9 | #define EIGEN_GEOMETRY_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SVD" 16 | #include "LU" 17 | #include 18 | 19 | /** \defgroup Geometry_Module Geometry module 20 | * 21 | * This module provides support for: 22 | * - fixed-size homogeneous transformations 23 | * - translation, scaling, 2D and 3D rotations 24 | * - \link Quaternion quaternions \endlink 25 | * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) 26 | * - orthognal vector generation (\ref MatrixBase::unitOrthogonal) 27 | * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink 28 | * - \link AlignedBox axis aligned bounding boxes \endlink 29 | * - \link umeyama least-square transformation fitting \endlink 30 | * 31 | * \code 32 | * #include 33 | * \endcode 34 | */ 35 | 36 | #include "src/Geometry/OrthoMethods.h" 37 | #include "src/Geometry/EulerAngles.h" 38 | 39 | #include "src/Geometry/Homogeneous.h" 40 | #include "src/Geometry/RotationBase.h" 41 | #include "src/Geometry/Rotation2D.h" 42 | #include "src/Geometry/Quaternion.h" 43 | #include "src/Geometry/AngleAxis.h" 44 | #include "src/Geometry/Transform.h" 45 | #include "src/Geometry/Translation.h" 46 | #include "src/Geometry/Scaling.h" 47 | #include "src/Geometry/Hyperplane.h" 48 | #include "src/Geometry/ParametrizedLine.h" 49 | #include "src/Geometry/AlignedBox.h" 50 | #include "src/Geometry/Umeyama.h" 51 | 52 | // Use the SSE optimized version whenever possible. At the moment the 53 | // SSE version doesn't compile when AVX is enabled 54 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 55 | #include "src/Geometry/arch/Geometry_SSE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_GEOMETRY_MODULE_H 61 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 62 | 63 | -------------------------------------------------------------------------------- /lib/Eigen/src/SparseLU/SparseLU_Utils.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | 11 | #ifndef EIGEN_SPARSELU_UTILS_H 12 | #define EIGEN_SPARSELU_UTILS_H 13 | 14 | namespace Eigen { 15 | namespace internal { 16 | 17 | /** 18 | * \brief Count Nonzero elements in the factors 19 | */ 20 | template 21 | void SparseLUImpl::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu) 22 | { 23 | nnzL = 0; 24 | nnzU = (glu.xusub)(n); 25 | Index nsuper = (glu.supno)(n); 26 | Index jlen; 27 | Index i, j, fsupc; 28 | if (n <= 0 ) return; 29 | // For each supernode 30 | for (i = 0; i <= nsuper; i++) 31 | { 32 | fsupc = glu.xsup(i); 33 | jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 34 | 35 | for (j = fsupc; j < glu.xsup(i+1); j++) 36 | { 37 | nnzL += jlen; 38 | nnzU += j - fsupc + 1; 39 | jlen--; 40 | } 41 | } 42 | } 43 | 44 | /** 45 | * \brief Fix up the data storage lsub for L-subscripts. 46 | * 47 | * It removes the subscripts sets for structural pruning, 48 | * and applies permutation to the remaining subscripts 49 | * 50 | */ 51 | template 52 | void SparseLUImpl::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu) 53 | { 54 | Index fsupc, i, j, k, jstart; 55 | 56 | StorageIndex nextl = 0; 57 | Index nsuper = (glu.supno)(n); 58 | 59 | // For each supernode 60 | for (i = 0; i <= nsuper; i++) 61 | { 62 | fsupc = glu.xsup(i); 63 | jstart = glu.xlsub(fsupc); 64 | glu.xlsub(fsupc) = nextl; 65 | for (j = jstart; j < glu.xlsub(fsupc + 1); j++) 66 | { 67 | glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A 68 | nextl++; 69 | } 70 | for (k = fsupc+1; k < glu.xsup(i+1); k++) 71 | glu.xlsub(k) = nextl; // other columns in supernode i 72 | } 73 | 74 | glu.xlsub(n) = nextl; 75 | } 76 | 77 | } // end namespace internal 78 | 79 | } // end namespace Eigen 80 | #endif // EIGEN_SPARSELU_UTILS_H 81 | -------------------------------------------------------------------------------- /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 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 | -------------------------------------------------------------------------------- /lib/Eigen/src/SparseCore/MappedSparseMatrix.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MAPPED_SPARSEMATRIX_H 11 | #define EIGEN_MAPPED_SPARSEMATRIX_H 12 | 13 | namespace Eigen { 14 | 15 | /** \deprecated Use Map > 16 | * \class MappedSparseMatrix 17 | * 18 | * \brief Sparse matrix 19 | * 20 | * \param _Scalar the scalar type, i.e. the type of the coefficients 21 | * 22 | * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme. 23 | * 24 | */ 25 | namespace internal { 26 | template 27 | struct traits > : traits > 28 | {}; 29 | } // end namespace internal 30 | 31 | template 32 | class MappedSparseMatrix 33 | : public Map > 34 | { 35 | typedef Map > Base; 36 | 37 | public: 38 | 39 | typedef typename Base::StorageIndex StorageIndex; 40 | typedef typename Base::Scalar Scalar; 41 | 42 | inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0) 43 | : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr) 44 | {} 45 | 46 | /** Empty destructor */ 47 | inline ~MappedSparseMatrix() {} 48 | }; 49 | 50 | namespace internal { 51 | 52 | template 53 | struct evaluator > 54 | : evaluator > > 55 | { 56 | typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType; 57 | typedef evaluator > Base; 58 | 59 | evaluator() : Base() {} 60 | explicit evaluator(const XprType &mat) : Base(mat) {} 61 | }; 62 | 63 | } 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN_MAPPED_SPARSEMATRIX_H 68 | -------------------------------------------------------------------------------- /lib/Eigen/SuperLUSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SUPERLUSUPPORT_MODULE_H 9 | #define EIGEN_SUPERLUSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #ifdef EMPTY 16 | #define EIGEN_EMPTY_WAS_ALREADY_DEFINED 17 | #endif 18 | 19 | typedef int int_t; 20 | #include 21 | #include 22 | #include 23 | 24 | // slu_util.h defines a preprocessor token named EMPTY which is really polluting, 25 | // so we remove it in favor of a SUPERLU_EMPTY token. 26 | // If EMPTY was already defined then we don't undef it. 27 | 28 | #if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED) 29 | # undef EIGEN_EMPTY_WAS_ALREADY_DEFINED 30 | #elif defined(EMPTY) 31 | # undef EMPTY 32 | #endif 33 | 34 | #define SUPERLU_EMPTY (-1) 35 | 36 | namespace Eigen { struct SluMatrix; } 37 | 38 | /** \ingroup Support_modules 39 | * \defgroup SuperLUSupport_Module SuperLUSupport module 40 | * 41 | * This module provides an interface to the SuperLU library. 42 | * It provides the following factorization class: 43 | * - class SuperLU: a supernodal sequential LU factorization. 44 | * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). 45 | * 46 | * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported. 47 | * 48 | * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. 49 | * 50 | * \code 51 | * #include 52 | * \endcode 53 | * 54 | * In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies. 55 | * The dependencies depend on how superlu has been compiled. 56 | * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task. 57 | * 58 | */ 59 | 60 | #include "src/SuperLUSupport/SuperLUSupport.h" 61 | 62 | #include "src/Core/util/ReenableStupidWarnings.h" 63 | 64 | #endif // EIGEN_SUPERLUSUPPORT_MODULE_H 65 | -------------------------------------------------------------------------------- /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/Simulation/SimulatedMag.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "SimulatedQuadSensor.h" 4 | #include "QuadDynamics.h" 5 | #include "Math/Random.h" 6 | #include "BaseQuadEstimator.h" 7 | 8 | class SimulatedMag : public SimulatedQuadSensor 9 | { 10 | public: 11 | SimulatedMag(string config, string name) : SimulatedQuadSensor(config, name) { Init(); } 12 | 13 | virtual void Init() 14 | { 15 | SimulatedQuadSensor::Init(); 16 | ParamsHandle paramSys = SimpleConfig::GetInstance(); 17 | _magStd = paramSys->Get(_config + ".Std", 0); 18 | _measDT = paramSys->Get(_config + ".dt", .01f); 19 | _magYaw = 0; 20 | } 21 | 22 | // if it's time, generates a new sensor measurement, saves it internally (for graphing), and calls appropriate estimator update function 23 | virtual void Update(QuadDynamics& quad, shared_ptr estimator, float dt, int& idum) 24 | { 25 | _timeAccum += dt; 26 | if (_timeAccum < _measDT) 27 | { 28 | return; 29 | } 30 | 31 | _timeAccum = (_timeAccum - _measDT); 32 | 33 | // position 34 | float magError = gasdev_f(idum) * _magStd; 35 | _magYaw = quad.Attitude().Yaw() + magError; 36 | if (_magYaw > F_PI) _magYaw -= 2.f*F_PI; 37 | if (_magYaw < -F_PI) _magYaw += 2.f*F_PI; 38 | 39 | _freshMeas = true; 40 | 41 | if (estimator) 42 | { 43 | estimator->UpdateFromMag(_magYaw); 44 | } 45 | }; 46 | 47 | // Access functions for graphing variables 48 | // note that GetData will only return true if a fresh measurement was generated last Update() 49 | virtual bool GetData(const string& name, float& ret) const 50 | { 51 | if (!_freshMeas) return false; 52 | 53 | if (name.find_first_of(".") == string::npos) return false; 54 | string leftPart = LeftOf(name, '.'); 55 | string rightPart = RightOf(name, '.'); 56 | 57 | if (ToUpper(leftPart) == ToUpper(_name)) 58 | { 59 | #define GETTER_HELPER(A,B) if (SLR::ToUpper(rightPart) == SLR::ToUpper(A)){ ret=(B); return true; } 60 | GETTER_HELPER("magYaw", _magYaw); 61 | #undef GETTER_HELPER 62 | } 63 | return false; 64 | }; 65 | 66 | virtual vector GetFields() const 67 | { 68 | vector ret = SimulatedQuadSensor::GetFields(); 69 | ret.push_back(_name + ".MagYaw"); 70 | return ret; 71 | }; 72 | 73 | float _magYaw; // last yaw measurement from magnetometer 74 | float _magStd; // std deviation of noise for magnetometer measurements 75 | float _measDT; // time (in seconds) between measurements 76 | }; 77 | -------------------------------------------------------------------------------- /lib/Eigen/SparseCore: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSECORE_MODULE_H 9 | #define EIGEN_SPARSECORE_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | /** 22 | * \defgroup SparseCore_Module SparseCore module 23 | * 24 | * This module provides a sparse matrix representation, and basic associated matrix manipulations 25 | * and operations. 26 | * 27 | * See the \ref TutorialSparse "Sparse tutorial" 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | * 33 | * This module depends on: Core. 34 | */ 35 | 36 | #include "src/SparseCore/SparseUtil.h" 37 | #include "src/SparseCore/SparseMatrixBase.h" 38 | #include "src/SparseCore/SparseAssign.h" 39 | #include "src/SparseCore/CompressedStorage.h" 40 | #include "src/SparseCore/AmbiVector.h" 41 | #include "src/SparseCore/SparseCompressedBase.h" 42 | #include "src/SparseCore/SparseMatrix.h" 43 | #include "src/SparseCore/SparseMap.h" 44 | #include "src/SparseCore/MappedSparseMatrix.h" 45 | #include "src/SparseCore/SparseVector.h" 46 | #include "src/SparseCore/SparseRef.h" 47 | #include "src/SparseCore/SparseCwiseUnaryOp.h" 48 | #include "src/SparseCore/SparseCwiseBinaryOp.h" 49 | #include "src/SparseCore/SparseTranspose.h" 50 | #include "src/SparseCore/SparseBlock.h" 51 | #include "src/SparseCore/SparseDot.h" 52 | #include "src/SparseCore/SparseRedux.h" 53 | #include "src/SparseCore/SparseView.h" 54 | #include "src/SparseCore/SparseDiagonalProduct.h" 55 | #include "src/SparseCore/ConservativeSparseSparseProduct.h" 56 | #include "src/SparseCore/SparseSparseProductWithPruning.h" 57 | #include "src/SparseCore/SparseProduct.h" 58 | #include "src/SparseCore/SparseDenseProduct.h" 59 | #include "src/SparseCore/SparseSelfAdjointView.h" 60 | #include "src/SparseCore/SparseTriangularView.h" 61 | #include "src/SparseCore/TriangularSolver.h" 62 | #include "src/SparseCore/SparsePermutation.h" 63 | #include "src/SparseCore/SparseFuzzy.h" 64 | #include "src/SparseCore/SparseSolverBase.h" 65 | 66 | #include "src/Core/util/ReenableStupidWarnings.h" 67 | 68 | #endif // EIGEN_SPARSECORE_MODULE_H 69 | 70 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /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 | #include "Utility/FixedQueue.h" 8 | 9 | #ifdef _MSC_VER 10 | #pragma warning(push) 11 | #pragma warning(disable: 4100) // unreferenced formal parameter 12 | #endif 13 | 14 | using SLR::Quaternion; 15 | class Trajectory; 16 | 17 | class BaseDynamics : public DataSource 18 | { 19 | public: 20 | BaseDynamics(string name=""); 21 | virtual ~BaseDynamics() {}; // destructor 22 | 23 | virtual int Initialize(); 24 | 25 | virtual void Run(float dt, float simulationTime, int &idum, // updates the simulation 26 | V3F externalForceInGlobalFrame = V3F(), // required to take net forces into account 27 | V3F externalMomentInBodyFrame = V3F()) // required to take net moments into account 28 | {} 29 | 30 | virtual void SetCommands(const VehicleCommand& cmd) {}; // update commands in the simulator coming from a command2 packet 31 | 32 | // inheritors have no reason to alter the following functions and therefore no sense demanding that they do 33 | GlobalPose GenerateGP () ; // returns the current simulation state in Vicon format - const? 34 | 35 | V3F Position() const { return pos; }; 36 | V3F Velocity() const { return vel; }; 37 | V3F Acceleration() const { return acc; }; 38 | V3F Omega() const { return omega; }; 39 | Quaternion Attitude() const { return quat; } 40 | 41 | void SetPosition(const V3F& p) { pos = p; } 42 | void SetVelocity(const V3F& v) { vel = v; } 43 | void SetOmega(const V3F& o) { omega = o; } 44 | void SetAttitude(const Quaternion& q){quat = q;} 45 | 46 | virtual bool GetData(const string& name, float& ret) const; 47 | virtual vector GetFields() const; 48 | 49 | virtual double GetRotDistInt() { return 0;}; 50 | virtual double GetXyzDistInt() {return 0;}; 51 | virtual double GetRotDistBW() {return 0;}; 52 | virtual double GetXyzDistBW() {return 0;}; 53 | virtual double GetGyroNoiseInt() {return 0;}; 54 | 55 | void ResetState(V3F pos=V3F(), V3F vel=V3F(), Quaternion att=Quaternion(), V3F omega=V3F()); 56 | 57 | FixedQueue _followedPos; 58 | FixedQueue> _followedAtt; 59 | 60 | protected: 61 | string _name; 62 | 63 | // pos, vel, acc are in the global frame while omega is in the local frame (body rates) 64 | V3F pos, vel, acc, omega, old_omega; 65 | Quaternion quat; 66 | 67 | // vehicle geometry and mass properties 68 | float M; // veh mass, kg 69 | float Ixx,Iyy,Izz; 70 | float xMin,yMin,zMin,xMax,yMax,zMax; 71 | 72 | float _lastTrajPointTime; 73 | float _trajLogStepTime; 74 | }; 75 | 76 | #ifdef _MSC_VER 77 | #pragma warning(pop) 78 | #endif 79 | -------------------------------------------------------------------------------- /lib/Eigen/OrderingMethods: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ORDERINGMETHODS_MODULE_H 9 | #define EIGEN_ORDERINGMETHODS_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** 16 | * \defgroup OrderingMethods_Module OrderingMethods module 17 | * 18 | * This module is currently for internal use only 19 | * 20 | * It defines various built-in and external ordering methods for sparse matrices. 21 | * They are typically used to reduce the number of elements during 22 | * the sparse matrix decomposition (LLT, LU, QR). 23 | * Precisely, in a preprocessing step, a permutation matrix P is computed using 24 | * those ordering methods and applied to the columns of the matrix. 25 | * Using for instance the sparse Cholesky decomposition, it is expected that 26 | * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A). 27 | * 28 | * 29 | * Usage : 30 | * \code 31 | * #include 32 | * \endcode 33 | * 34 | * A simple usage is as a template parameter in the sparse decomposition classes : 35 | * 36 | * \code 37 | * SparseLU > solver; 38 | * \endcode 39 | * 40 | * \code 41 | * SparseQR > solver; 42 | * \endcode 43 | * 44 | * It is possible as well to call directly a particular ordering method for your own purpose, 45 | * \code 46 | * AMDOrdering ordering; 47 | * PermutationMatrix perm; 48 | * SparseMatrix A; 49 | * //Fill the matrix ... 50 | * 51 | * ordering(A, perm); // Call AMD 52 | * \endcode 53 | * 54 | * \note Some of these methods (like AMD or METIS), need the sparsity pattern 55 | * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 56 | * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method. 57 | * If your matrix is already symmetric (at leat in structure), you can avoid that 58 | * by calling the method with a SelfAdjointView type. 59 | * 60 | * \code 61 | * // Call the ordering on the pattern of the lower triangular matrix A 62 | * ordering(A.selfadjointView(), perm); 63 | * \endcode 64 | */ 65 | 66 | #ifndef EIGEN_MPL2_ONLY 67 | #include "src/OrderingMethods/Amd.h" 68 | #endif 69 | 70 | #include "src/OrderingMethods/Ordering.h" 71 | #include "src/Core/util/ReenableStupidWarnings.h" 72 | 73 | #endif // EIGEN_ORDERINGMETHODS_MODULE_H 74 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/arch/CUDA/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MATH_FUNCTIONS_CUDA_H 11 | #define EIGEN_MATH_FUNCTIONS_CUDA_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // Make sure this is only available when targeting a GPU: we don't want to 18 | // introduce conflicts between these packet_traits definitions and the ones 19 | // we'll use on the host side (SSE, AVX, ...) 20 | #if defined(__CUDACC__) && defined(EIGEN_USE_GPU) 21 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 22 | float4 plog(const float4& a) 23 | { 24 | return make_float4(logf(a.x), logf(a.y), logf(a.z), logf(a.w)); 25 | } 26 | 27 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 28 | double2 plog(const double2& a) 29 | { 30 | using ::log; 31 | return make_double2(log(a.x), log(a.y)); 32 | } 33 | 34 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 35 | float4 plog1p(const float4& a) 36 | { 37 | return make_float4(log1pf(a.x), log1pf(a.y), log1pf(a.z), log1pf(a.w)); 38 | } 39 | 40 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 41 | double2 plog1p(const double2& a) 42 | { 43 | return make_double2(log1p(a.x), log1p(a.y)); 44 | } 45 | 46 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 47 | float4 pexp(const float4& a) 48 | { 49 | return make_float4(expf(a.x), expf(a.y), expf(a.z), expf(a.w)); 50 | } 51 | 52 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 53 | double2 pexp(const double2& a) 54 | { 55 | using ::exp; 56 | return make_double2(exp(a.x), exp(a.y)); 57 | } 58 | 59 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 60 | float4 psqrt(const float4& a) 61 | { 62 | return make_float4(sqrtf(a.x), sqrtf(a.y), sqrtf(a.z), sqrtf(a.w)); 63 | } 64 | 65 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 66 | double2 psqrt(const double2& a) 67 | { 68 | using ::sqrt; 69 | return make_double2(sqrt(a.x), sqrt(a.y)); 70 | } 71 | 72 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 73 | float4 prsqrt(const float4& a) 74 | { 75 | return make_float4(rsqrtf(a.x), rsqrtf(a.y), rsqrtf(a.z), rsqrtf(a.w)); 76 | } 77 | 78 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 79 | double2 prsqrt(const double2& a) 80 | { 81 | return make_double2(rsqrt(a.x), rsqrt(a.y)); 82 | } 83 | 84 | 85 | #endif 86 | 87 | } // end namespace internal 88 | 89 | } // end namespace Eigen 90 | 91 | #endif // EIGEN_MATH_FUNCTIONS_CUDA_H 92 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/Swap.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2006-2008 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SWAP_H 11 | #define EIGEN_SWAP_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // Overload default assignPacket behavior for swapping them 18 | template 19 | class generic_dense_assignment_kernel, Specialized> 20 | : public generic_dense_assignment_kernel, BuiltIn> 21 | { 22 | protected: 23 | typedef generic_dense_assignment_kernel, BuiltIn> Base; 24 | using Base::m_dst; 25 | using Base::m_src; 26 | using Base::m_functor; 27 | 28 | public: 29 | typedef typename Base::Scalar Scalar; 30 | typedef typename Base::DstXprType DstXprType; 31 | typedef swap_assign_op Functor; 32 | 33 | EIGEN_DEVICE_FUNC generic_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr) 34 | : Base(dst, src, func, dstExpr) 35 | {} 36 | 37 | template 38 | void assignPacket(Index row, Index col) 39 | { 40 | PacketType tmp = m_src.template packet(row,col); 41 | const_cast(m_src).template writePacket(row,col, m_dst.template packet(row,col)); 42 | m_dst.template writePacket(row,col,tmp); 43 | } 44 | 45 | template 46 | void assignPacket(Index index) 47 | { 48 | PacketType tmp = m_src.template packet(index); 49 | const_cast(m_src).template writePacket(index, m_dst.template packet(index)); 50 | m_dst.template writePacket(index,tmp); 51 | } 52 | 53 | // TODO find a simple way not to have to copy/paste this function from generic_dense_assignment_kernel, by simple I mean no CRTP (Gael) 54 | template 55 | void assignPacketByOuterInner(Index outer, Index inner) 56 | { 57 | Index row = Base::rowIndexByOuterInner(outer, inner); 58 | Index col = Base::colIndexByOuterInner(outer, inner); 59 | assignPacket(row, col); 60 | } 61 | }; 62 | 63 | } // namespace internal 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN_SWAP_H 68 | -------------------------------------------------------------------------------- /src/QuadEstimatorEKF.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "BaseQuadEstimator.h" 4 | #include "matrix/math.hpp" 5 | #include "Math/Quaternion.h" 6 | 7 | using matrix::Vector; 8 | using matrix::Matrix; 9 | using matrix::SquareMatrix; 10 | 11 | #include "Eigen/Dense" 12 | #include "Eigen/SVD" 13 | using Eigen::MatrixXf; 14 | using Eigen::VectorXf; 15 | 16 | class QuadEstimatorEKF : public BaseQuadEstimator 17 | { 18 | public: 19 | QuadEstimatorEKF(string config, string name); 20 | virtual ~QuadEstimatorEKF(); 21 | 22 | virtual void Init(); 23 | 24 | virtual void Predict(float dt, V3F accel, V3F gyro); 25 | 26 | // helper functions for Predict 27 | VectorXf PredictState(VectorXf curState, float dt, V3F accel, V3F gyro); 28 | MatrixXf GetRbgPrime(float roll, float pitch, float yaw); 29 | 30 | virtual void UpdateFromIMU(V3F accel, V3F gyro); 31 | virtual void UpdateFromGPS(V3F pos, V3F vel); 32 | virtual void UpdateFromBaro(float z) {}; 33 | virtual void UpdateFromMag(float magYaw); 34 | 35 | static const int QUAD_EKF_NUM_STATES = 7; 36 | 37 | // process covariance 38 | MatrixXf Q; 39 | 40 | // GPS measurement covariance 41 | MatrixXf R_GPS; 42 | 43 | // Magnetometer measurement covariance 44 | MatrixXf R_Mag; 45 | 46 | // attitude filter state 47 | float pitchEst, rollEst; 48 | float accelPitch, accelRoll; // raw pitch/roll angles as calculated from last accelerometer.. purely for graphing. 49 | V3F accelG; 50 | V3F lastGyro; 51 | 52 | // generic EKF update 53 | // z: measurement 54 | // H: Jacobian of observation function evaluated at the current estimated state 55 | // R: observation error model covariance 56 | // zFromX: measurement prediction based on current state 57 | void Update(VectorXf& z, MatrixXf& H, MatrixXf& R, VectorXf& zFromX); 58 | 59 | // EKF state and covariance 60 | VectorXf ekfState; 61 | MatrixXf ekfCov; 62 | 63 | // params 64 | float attitudeTau; 65 | float dtIMU; 66 | 67 | // Access functions for graphing variables 68 | virtual bool GetData(const string& name, float& ret) const; 69 | virtual vector GetFields() const; 70 | string _name; 71 | 72 | // error vs ground truth (trueError = estimated-actual) 73 | virtual void UpdateTrueError(V3F truePos, V3F trueVel, SLR::Quaternion trueAtt); 74 | VectorXf trueError; 75 | float pitchErr, rollErr, maxEuler; 76 | 77 | float posErrorMag, velErrorMag; 78 | 79 | virtual V3F EstimatedPosition() 80 | { 81 | return V3F(ekfState(0), ekfState(1), ekfState(2)); 82 | } 83 | 84 | virtual V3F EstimatedVelocity() 85 | { 86 | return V3F(ekfState(3), ekfState(4), ekfState(5)); 87 | } 88 | 89 | virtual Quaternion EstimatedAttitude() 90 | { 91 | return Quaternion::FromEuler123_RPY(rollEst, pitchEst, ekfState(6)); 92 | } 93 | 94 | virtual V3F EstimatedOmega() 95 | { 96 | return lastGyro; 97 | } 98 | 99 | float CovConditionNumber() const; 100 | }; 101 | -------------------------------------------------------------------------------- /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 name, string config) 10 | { 11 | _name = name; 12 | _config = config; 13 | Init(); 14 | } 15 | 16 | void BaseController::Init() 17 | { 18 | #ifndef __PX4_NUTTX 19 | ParamsHandle config = SimpleConfig::GetInstance(); 20 | 21 | mass = config->Get(_config+".Mass", 1.f); 22 | L = config->Get(_config+".L", 0.1f); 23 | Ixx = config->Get(_config+".Ixx", 0.001f); 24 | Iyy = config->Get(_config + ".Iyy", 0.001f); 25 | Izz = config->Get(_config + ".Izz", 0.002f); 26 | kappa = config->Get(_config + ".kappa", 0.01f); 27 | 28 | trajectory.Clear(); 29 | string trajFile = config->Get(_config + ".Trajectory",""); 30 | if (!trajectory.ReadFile(string("../config/")+trajFile)) 31 | { 32 | TrajectoryPoint tmp; 33 | tmp.position = config->Get(_config + ".Trajectory", V3F()); 34 | trajectory.AddTrajectoryPoint(tmp); 35 | } 36 | #else 37 | 38 | #endif 39 | } 40 | 41 | void BaseController::Reset() 42 | { 43 | // Reinitialise the physical parameters 44 | Init(); 45 | } 46 | 47 | void BaseController::UpdateEstimates(V3F pos, V3F vel, Quaternion attitude, V3F omega) 48 | { 49 | estAtt = attitude; 50 | estOmega = omega; 51 | estPos = pos; 52 | estVel = vel; 53 | } 54 | 55 | TrajectoryPoint BaseController::GetNextTrajectoryPoint(float mission_time) 56 | { 57 | TrajectoryPoint pt = trajectory.NextTrajectoryPoint(mission_time + _trajectoryTimeOffset); 58 | pt.position += _trajectoryOffset; 59 | return pt; 60 | } 61 | 62 | // Access functions for graphing variables 63 | bool BaseController::GetData(const string& name, float& ret) const 64 | { 65 | if (name.find_first_of(".") == string::npos) return false; 66 | string leftPart = LeftOf(name, '.'); 67 | string rightPart = RightOf(name, '.'); 68 | 69 | if (ToUpper(leftPart) == ToUpper(_name)) 70 | { 71 | #define GETTER_HELPER(A,B) if (SLR::ToUpper(rightPart) == SLR::ToUpper(A)){ ret=(B); return true; } 72 | // UDACITY CONVENTION 73 | GETTER_HELPER("Ref.X", curTrajPoint.position.x); 74 | GETTER_HELPER("Ref.Y", curTrajPoint.position.y); 75 | GETTER_HELPER("Ref.Z", curTrajPoint.position.z); 76 | GETTER_HELPER("Ref.VX", curTrajPoint.velocity.x); 77 | GETTER_HELPER("Ref.VY", curTrajPoint.velocity.y); 78 | GETTER_HELPER("Ref.VZ", curTrajPoint.velocity.z); 79 | GETTER_HELPER("Ref.Yaw", curTrajPoint.attitude.Yaw()); 80 | #undef GETTER_HELPER 81 | } 82 | return false; 83 | } 84 | 85 | vector BaseController::GetFields() const 86 | { 87 | vector ret; 88 | ret.push_back(_name + ".Ref.X"); 89 | ret.push_back(_name + ".Ref.Y"); 90 | ret.push_back(_name + ".Ref.Z"); 91 | ret.push_back(_name + ".Ref.VX"); 92 | ret.push_back(_name + ".Ref.VY"); 93 | ret.push_back(_name + ".Ref.VZ"); 94 | ret.push_back(_name + ".Ref.Yaw"); 95 | return ret; 96 | } 97 | -------------------------------------------------------------------------------- /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 | }; -------------------------------------------------------------------------------- /lib/Eigen/src/misc/Kernel.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MISC_KERNEL_H 11 | #define EIGEN_MISC_KERNEL_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | /** \class kernel_retval_base 18 | * 19 | */ 20 | template 21 | struct traits > 22 | { 23 | typedef typename DecompositionType::MatrixType MatrixType; 24 | typedef Matrix< 25 | typename MatrixType::Scalar, 26 | MatrixType::ColsAtCompileTime, // the number of rows in the "kernel matrix" 27 | // is the number of cols of the original matrix 28 | // so that the product "matrix * kernel = zero" makes sense 29 | Dynamic, // we don't know at compile-time the dimension of the kernel 30 | MatrixType::Options, 31 | MatrixType::MaxColsAtCompileTime, // see explanation for 2nd template parameter 32 | MatrixType::MaxColsAtCompileTime // the kernel is a subspace of the domain space, 33 | // whose dimension is the number of columns of the original matrix 34 | > ReturnType; 35 | }; 36 | 37 | template struct kernel_retval_base 38 | : public ReturnByValue > 39 | { 40 | typedef _DecompositionType DecompositionType; 41 | typedef ReturnByValue Base; 42 | 43 | explicit kernel_retval_base(const DecompositionType& dec) 44 | : m_dec(dec), 45 | m_rank(dec.rank()), 46 | m_cols(m_rank==dec.cols() ? 1 : dec.cols() - m_rank) 47 | {} 48 | 49 | inline Index rows() const { return m_dec.cols(); } 50 | inline Index cols() const { return m_cols; } 51 | inline Index rank() const { return m_rank; } 52 | inline const DecompositionType& dec() const { return m_dec; } 53 | 54 | template inline void evalTo(Dest& dst) const 55 | { 56 | static_cast*>(this)->evalTo(dst); 57 | } 58 | 59 | protected: 60 | const DecompositionType& m_dec; 61 | Index m_rank, m_cols; 62 | }; 63 | 64 | } // end namespace internal 65 | 66 | #define EIGEN_MAKE_KERNEL_HELPERS(DecompositionType) \ 67 | typedef typename DecompositionType::MatrixType MatrixType; \ 68 | typedef typename MatrixType::Scalar Scalar; \ 69 | typedef typename MatrixType::RealScalar RealScalar; \ 70 | typedef Eigen::internal::kernel_retval_base Base; \ 71 | using Base::dec; \ 72 | using Base::rank; \ 73 | using Base::rows; \ 74 | using Base::cols; \ 75 | kernel_retval(const DecompositionType& dec) : Base(dec) {} 76 | 77 | } // end namespace Eigen 78 | 79 | #endif // EIGEN_MISC_KERNEL_H 80 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/Assign.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2007 Michael Olbrich 5 | // Copyright (C) 2006-2010 Benoit Jacob 6 | // Copyright (C) 2008 Gael Guennebaud 7 | // 8 | // This Source Code Form is subject to the terms of the Mozilla 9 | // Public License v. 2.0. If a copy of the MPL was not distributed 10 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 11 | 12 | #ifndef EIGEN_ASSIGN_H 13 | #define EIGEN_ASSIGN_H 14 | 15 | namespace Eigen { 16 | 17 | template 18 | template 19 | EIGEN_STRONG_INLINE Derived& DenseBase 20 | ::lazyAssign(const DenseBase& other) 21 | { 22 | enum{ 23 | SameType = internal::is_same::value 24 | }; 25 | 26 | EIGEN_STATIC_ASSERT_LVALUE(Derived) 27 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived) 28 | EIGEN_STATIC_ASSERT(SameType,YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 29 | 30 | eigen_assert(rows() == other.rows() && cols() == other.cols()); 31 | internal::call_assignment_no_alias(derived(),other.derived()); 32 | 33 | return derived(); 34 | } 35 | 36 | template 37 | template 38 | EIGEN_DEVICE_FUNC 39 | EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) 40 | { 41 | internal::call_assignment(derived(), other.derived()); 42 | return derived(); 43 | } 44 | 45 | template 46 | EIGEN_DEVICE_FUNC 47 | EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) 48 | { 49 | internal::call_assignment(derived(), other.derived()); 50 | return derived(); 51 | } 52 | 53 | template 54 | EIGEN_DEVICE_FUNC 55 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const MatrixBase& other) 56 | { 57 | internal::call_assignment(derived(), other.derived()); 58 | return derived(); 59 | } 60 | 61 | template 62 | template 63 | EIGEN_DEVICE_FUNC 64 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const DenseBase& other) 65 | { 66 | internal::call_assignment(derived(), other.derived()); 67 | return derived(); 68 | } 69 | 70 | template 71 | template 72 | EIGEN_DEVICE_FUNC 73 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const EigenBase& other) 74 | { 75 | internal::call_assignment(derived(), other.derived()); 76 | return derived(); 77 | } 78 | 79 | template 80 | template 81 | EIGEN_DEVICE_FUNC 82 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const ReturnByValue& other) 83 | { 84 | other.derived().evalTo(derived()); 85 | return derived(); 86 | } 87 | 88 | } // end namespace Eigen 89 | 90 | #endif // EIGEN_ASSIGN_H 91 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/MathFunctionsImpl.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Pedro Gonnet (pedro.gonnet@gmail.com) 5 | // Copyright (C) 2016 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_MATHFUNCTIONSIMPL_H 12 | #define EIGEN_MATHFUNCTIONSIMPL_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | /** \internal \returns the hyperbolic tan of \a a (coeff-wise) 19 | Doesn't do anything fancy, just a 13/6-degree rational interpolant which 20 | is accurate up to a couple of ulp in the range [-9, 9], outside of which 21 | the tanh(x) = +/-1. 22 | 23 | This implementation works on both scalars and packets. 24 | */ 25 | template 26 | T generic_fast_tanh_float(const T& a_x) 27 | { 28 | // Clamp the inputs to the range [-9, 9] since anything outside 29 | // this range is +/-1.0f in single-precision. 30 | const T plus_9 = pset1(9.f); 31 | const T minus_9 = pset1(-9.f); 32 | // NOTE GCC prior to 6.3 might improperly optimize this max/min 33 | // step such that if a_x is nan, x will be either 9 or -9, 34 | // and tanh will return 1 or -1 instead of nan. 35 | // This is supposed to be fixed in gcc6.3, 36 | // see: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867 37 | const T x = pmax(minus_9,pmin(plus_9,a_x)); 38 | // The monomial coefficients of the numerator polynomial (odd). 39 | const T alpha_1 = pset1(4.89352455891786e-03f); 40 | const T alpha_3 = pset1(6.37261928875436e-04f); 41 | const T alpha_5 = pset1(1.48572235717979e-05f); 42 | const T alpha_7 = pset1(5.12229709037114e-08f); 43 | const T alpha_9 = pset1(-8.60467152213735e-11f); 44 | const T alpha_11 = pset1(2.00018790482477e-13f); 45 | const T alpha_13 = pset1(-2.76076847742355e-16f); 46 | 47 | // The monomial coefficients of the denominator polynomial (even). 48 | const T beta_0 = pset1(4.89352518554385e-03f); 49 | const T beta_2 = pset1(2.26843463243900e-03f); 50 | const T beta_4 = pset1(1.18534705686654e-04f); 51 | const T beta_6 = pset1(1.19825839466702e-06f); 52 | 53 | // Since the polynomials are odd/even, we need x^2. 54 | const T x2 = pmul(x, x); 55 | 56 | // Evaluate the numerator polynomial p. 57 | T p = pmadd(x2, alpha_13, alpha_11); 58 | p = pmadd(x2, p, alpha_9); 59 | p = pmadd(x2, p, alpha_7); 60 | p = pmadd(x2, p, alpha_5); 61 | p = pmadd(x2, p, alpha_3); 62 | p = pmadd(x2, p, alpha_1); 63 | p = pmul(x, p); 64 | 65 | // Evaluate the denominator polynomial p. 66 | T q = pmadd(x2, beta_6, beta_4); 67 | q = pmadd(x2, q, beta_2); 68 | q = pmadd(x2, q, beta_0); 69 | 70 | // Divide the numerator by the denominator. 71 | return pdiv(p, q); 72 | } 73 | 74 | } // end namespace internal 75 | 76 | } // end namespace Eigen 77 | 78 | #endif // EIGEN_MATHFUNCTIONSIMPL_H 79 | -------------------------------------------------------------------------------- /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/Eigen/src/StlSupport/details.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STL_DETAILS_H 12 | #define EIGEN_STL_DETAILS_H 13 | 14 | #ifndef EIGEN_ALIGNED_ALLOCATOR 15 | #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator 16 | #endif 17 | 18 | namespace Eigen { 19 | 20 | // This one is needed to prevent reimplementing the whole std::vector. 21 | template 22 | class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR 23 | { 24 | public: 25 | typedef std::size_t size_type; 26 | typedef std::ptrdiff_t difference_type; 27 | typedef T* pointer; 28 | typedef const T* const_pointer; 29 | typedef T& reference; 30 | typedef const T& const_reference; 31 | typedef T value_type; 32 | 33 | template 34 | struct rebind 35 | { 36 | typedef aligned_allocator_indirection other; 37 | }; 38 | 39 | aligned_allocator_indirection() {} 40 | aligned_allocator_indirection(const aligned_allocator_indirection& ) : EIGEN_ALIGNED_ALLOCATOR() {} 41 | aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR& ) {} 42 | template 43 | aligned_allocator_indirection(const aligned_allocator_indirection& ) {} 44 | template 45 | aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR& ) {} 46 | ~aligned_allocator_indirection() {} 47 | }; 48 | 49 | #if EIGEN_COMP_MSVC 50 | 51 | // sometimes, MSVC detects, at compile time, that the argument x 52 | // in std::vector::resize(size_t s,T x) won't be aligned and generate an error 53 | // even if this function is never called. Whence this little wrapper. 54 | #define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \ 55 | typename Eigen::internal::conditional< \ 56 | Eigen::internal::is_arithmetic::value, \ 57 | T, \ 58 | Eigen::internal::workaround_msvc_stl_support \ 59 | >::type 60 | 61 | namespace internal { 62 | template struct workaround_msvc_stl_support : public T 63 | { 64 | inline workaround_msvc_stl_support() : T() {} 65 | inline workaround_msvc_stl_support(const T& other) : T(other) {} 66 | inline operator T& () { return *static_cast(this); } 67 | inline operator const T& () const { return *static_cast(this); } 68 | template 69 | inline T& operator=(const OtherT& other) 70 | { T::operator=(other); return *this; } 71 | inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other) 72 | { T::operator=(other); return *this; } 73 | }; 74 | } 75 | 76 | #else 77 | 78 | #define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T 79 | 80 | #endif 81 | 82 | } 83 | 84 | #endif // EIGEN_STL_DETAILS_H 85 | -------------------------------------------------------------------------------- /lib/Eigen/src/Core/arch/NEON/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | /* The sin, cos, exp, and log functions of this file come from 9 | * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/ 10 | */ 11 | 12 | #ifndef EIGEN_MATH_FUNCTIONS_NEON_H 13 | #define EIGEN_MATH_FUNCTIONS_NEON_H 14 | 15 | namespace Eigen { 16 | 17 | namespace internal { 18 | 19 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 20 | Packet4f pexp(const Packet4f& _x) 21 | { 22 | Packet4f x = _x; 23 | Packet4f tmp, fx; 24 | 25 | _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f); 26 | _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f); 27 | _EIGEN_DECLARE_CONST_Packet4i(0x7f, 0x7f); 28 | _EIGEN_DECLARE_CONST_Packet4f(exp_hi, 88.3762626647950f); 29 | _EIGEN_DECLARE_CONST_Packet4f(exp_lo, -88.3762626647949f); 30 | _EIGEN_DECLARE_CONST_Packet4f(cephes_LOG2EF, 1.44269504088896341f); 31 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C1, 0.693359375f); 32 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C2, -2.12194440e-4f); 33 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p0, 1.9875691500E-4f); 34 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p1, 1.3981999507E-3f); 35 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p2, 8.3334519073E-3f); 36 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p3, 4.1665795894E-2f); 37 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f); 38 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f); 39 | 40 | x = vminq_f32(x, p4f_exp_hi); 41 | x = vmaxq_f32(x, p4f_exp_lo); 42 | 43 | /* express exp(x) as exp(g + n*log(2)) */ 44 | fx = vmlaq_f32(p4f_half, x, p4f_cephes_LOG2EF); 45 | 46 | /* perform a floorf */ 47 | tmp = vcvtq_f32_s32(vcvtq_s32_f32(fx)); 48 | 49 | /* if greater, substract 1 */ 50 | Packet4ui mask = vcgtq_f32(tmp, fx); 51 | mask = vandq_u32(mask, vreinterpretq_u32_f32(p4f_1)); 52 | 53 | fx = vsubq_f32(tmp, vreinterpretq_f32_u32(mask)); 54 | 55 | tmp = vmulq_f32(fx, p4f_cephes_exp_C1); 56 | Packet4f z = vmulq_f32(fx, p4f_cephes_exp_C2); 57 | x = vsubq_f32(x, tmp); 58 | x = vsubq_f32(x, z); 59 | 60 | Packet4f y = vmulq_f32(p4f_cephes_exp_p0, x); 61 | z = vmulq_f32(x, x); 62 | y = vaddq_f32(y, p4f_cephes_exp_p1); 63 | y = vmulq_f32(y, x); 64 | y = vaddq_f32(y, p4f_cephes_exp_p2); 65 | y = vmulq_f32(y, x); 66 | y = vaddq_f32(y, p4f_cephes_exp_p3); 67 | y = vmulq_f32(y, x); 68 | y = vaddq_f32(y, p4f_cephes_exp_p4); 69 | y = vmulq_f32(y, x); 70 | y = vaddq_f32(y, p4f_cephes_exp_p5); 71 | 72 | y = vmulq_f32(y, z); 73 | y = vaddq_f32(y, x); 74 | y = vaddq_f32(y, p4f_1); 75 | 76 | /* build 2^n */ 77 | int32x4_t mm; 78 | mm = vcvtq_s32_f32(fx); 79 | mm = vaddq_s32(mm, p4i_0x7f); 80 | mm = vshlq_n_s32(mm, 23); 81 | Packet4f pow2n = vreinterpretq_f32_s32(mm); 82 | 83 | y = vmulq_f32(y, pow2n); 84 | return y; 85 | } 86 | 87 | } // end namespace internal 88 | 89 | } // end namespace Eigen 90 | 91 | #endif // EIGEN_MATH_FUNCTIONS_NEON_H 92 | -------------------------------------------------------------------------------- /lib/Eigen/src/SparseLU/SparseLU_relax_snode.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | /* This file is a modified version of heap_relax_snode.c file in SuperLU 11 | * -- SuperLU routine (version 3.0) -- 12 | * Univ. of California Berkeley, Xerox Palo Alto Research Center, 13 | * and Lawrence Berkeley National Lab. 14 | * October 15, 2003 15 | * 16 | * Copyright (c) 1994 by Xerox Corporation. All rights reserved. 17 | * 18 | * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY 19 | * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. 20 | * 21 | * Permission is hereby granted to use or copy this program for any 22 | * purpose, provided the above notices are retained on all copies. 23 | * Permission to modify the code and to distribute modified code is 24 | * granted, provided the above notices are retained, and a notice that 25 | * the code was modified is included with the above copyright notice. 26 | */ 27 | 28 | #ifndef SPARSELU_RELAX_SNODE_H 29 | #define SPARSELU_RELAX_SNODE_H 30 | 31 | namespace Eigen { 32 | 33 | namespace internal { 34 | 35 | /** 36 | * \brief Identify the initial relaxed supernodes 37 | * 38 | * This routine is applied to a column elimination tree. 39 | * It assumes that the matrix has been reordered according to the postorder of the etree 40 | * \param n the number of columns 41 | * \param et elimination tree 42 | * \param relax_columns Maximum number of columns allowed in a relaxed snode 43 | * \param descendants Number of descendants of each node in the etree 44 | * \param relax_end last column in a supernode 45 | */ 46 | template 47 | void SparseLUImpl::relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end) 48 | { 49 | 50 | // compute the number of descendants of each node in the etree 51 | Index parent; 52 | relax_end.setConstant(emptyIdxLU); 53 | descendants.setZero(); 54 | for (Index j = 0; j < n; j++) 55 | { 56 | parent = et(j); 57 | if (parent != n) // not the dummy root 58 | descendants(parent) += descendants(j) + 1; 59 | } 60 | // Identify the relaxed supernodes by postorder traversal of the etree 61 | Index snode_start; // beginning of a snode 62 | for (Index j = 0; j < n; ) 63 | { 64 | parent = et(j); 65 | snode_start = j; 66 | while ( parent != n && descendants(parent) < relax_columns ) 67 | { 68 | j = parent; 69 | parent = et(j); 70 | } 71 | // Found a supernode in postordered etree, j is the last column 72 | relax_end(snode_start) = StorageIndex(j); // Record last column 73 | j++; 74 | // Search for a new leaf 75 | while (descendants(j) != 0 && j < n) j++; 76 | } // End postorder traversal of the etree 77 | 78 | } 79 | 80 | } // end namespace internal 81 | 82 | } // end namespace Eigen 83 | #endif 84 | --------------------------------------------------------------------------------