Flight dynamics and IMU simulation using the multicopterDynamicsSim l…

…ibrary.

A description of dynamics and IMU simulation equations can be found at: https://arxiv.org/pdf/1905.11377.pdf

Additional changes:
- the vehicle is armed by publishing a message to the topic /uav/input/arm.
- the topic /uav/input/motorspeed was added to directly command the individual motor speeds.
- the topic /uav/input/reset was implemented to reset the vehicle to its initial condition.
- vehicle properties can now be set separately for the PID controller and the (truth) dynamics simulation.

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "flightgoggles_uav_dynamics/libs/multicopterDynamicsSim"]
+ path = flightgoggles_uav_dynamics/libs/multicopterDynamicsSim
+ url = [email protected]:mit-fast/multicopterDynamicsSim.git 

flightgoggles/config/drone/drone.yaml

@@ -8,21 +8,29 @@ flightgoggles_uav_dynamics:
  vehicle_mass: 1.0
  vehicle_inertia_xx: 0.0049
  vehicle_inertia_yy: 0.0049
- vehicle_inertia_zz: 0.0049
+ vehicle_inertia_zz: 0.0069
  max_prop_speed: 2200.0
  moment_arm: 0.08
  motor_time_constant: 0.02
+ motor_rotational_inertia: 6.62e-6
  thrust_coefficient: 1.91e-6
  torque_coefficient: 2.6e-7
  drag_coefficient: 0.1
- angular_process_noise: 0.00025
- linear_process_noise: 0.0005
- reset_timeout: 0.1
- min_arming_thrust: 9.9
+ aeromoment_coefficient_xx: 0.003
+ aeromoment_coefficient_yy: 0.003
+ aeromoment_coefficient_zz: 0.003
+ moment_process_noise: 1.25e-7
+ force_process_noise: 0.0005
+ use_ratethrust_controller: true
+ use_rungekutta4integrator: false
 
 flightgoggles_imu:
  accelerometer_variance: 0.005
  gyroscope_variance: 0.003
+ accelerometer_biasprocess: 1.0e-7
+ gyroscope_biasprocess: 1.0e-7
+ accelerometer_biasinitvar: 0.0
+ gyroscope_biasinitvar: 0.0
 
 flightgoggles_laser:
  rangefinder_variance: 0.009
@@ -45,6 +53,12 @@ flightgoggles_pid:
  int_bound_roll: 1000.0
  int_bound_pitch: 1000.0
  int_bound_yaw: 1000.0
+ vehicle_inertia_xx: 0.0049
+ vehicle_inertia_yy: 0.0049
+ vehicle_inertia_zz: 0.0069
+ moment_arm: 0.08
+ thrust_coefficient: 1.91e-6
+ torque_coefficient: 2.6e-7
 
 flightgoggles_ros_bridge:
  image_width: 1024

flightgoggles_uav_dynamics/CMakeLists.txt

@@ -134,6 +134,9 @@ include_directories(
 # src/${PROJECT_NAME}/flightgoggles_uav_dynamics.cpp
 # )
 
+add_library(inertialMeasurementSim libs/multicopterDynamicsSim/inertialMeasurementSim.cpp)
+add_library(multicopterDynamicsSim libs/multicopterDynamicsSim/multicopterDynamicsSim.cpp)
+
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
@@ -159,6 +162,9 @@ target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
 )
 
+target_link_libraries(multicopterDynamicsSim inertialMeasurementSim)
+target_link_libraries(${PROJECT_NAME}_node multicopterDynamicsSim)
+
 #############
 ## Install ##
 #############