Merge pull request #182 from rosflight/180-forces-and-moments-update

180 forces and moments update

rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp

@@ -3,6 +3,7 @@
  *
  * Copyright (c) 2017 Daniel Koch, James Jackson and Gary Ellingson, BYU MAGICC Lab.
  * Copyright (c) 2023 Brandon Sutherland, AeroVironment Inc.
+ * Copyright (c) 2024 Ian Reid, BYU MAGICC Lab.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -35,6 +36,7 @@
 #ifndef ROSFLIGHT_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H
 #define ROSFLIGHT_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H
 
+#include <cstdint>
 #include <eigen3/Eigen/Dense>
 #include <rclcpp/rclcpp.hpp>
 
@@ -56,44 +58,45 @@ class Multirotor : public MAVForcesAndMoments
 private:
   rclcpp::Node::SharedPtr node_;
   Eigen::Vector3d wind_;
-
-  double prev_time_;
-
-  struct Rotor
+
+  struct Prop
   {
-    double max;
-    std::vector<double> F_poly;
-    std::vector<double> T_poly;
-    double tau_up; // time constants for response
-    double tau_down;
+    // Prop thrust constant coeffs.
+    double CT_0;
+    double CT_1;
+    double CT_2;
+
+    // Prop torque constant coeffs.
+    double CQ_0;
+    double CQ_1;
+    double CQ_2;
+
+    double diam;
   };
 
   struct Motor
   {
-    Rotor rotor;
-    Eigen::Vector3d position;
-    Eigen::Vector3d normal;
-    int direction; // 1 for CW -1 for CCW
+    double dist;
+    // The motor's radial angle from the body north unit vector in radians,
+    // for example: quad copter x frame this is 45,135,225,315, except in radians. 
+    double radial_angle;
+    // The direction that the motor spins, (-1 is clockwise and 1 is counter clocwise, looking from above vehicle).
+    int64_t direction;
+    // The current command for the motor, from 0 to 1
+    double command;
+    // Resistance of motor.
+    double R;
+    // Torque constant of motor.
+    double KQ;
+    // No load current draw of motor.
+    double I_0;
+    // Prop data
+    Prop prop;
   };
 
   int num_rotors_;
   std::vector<Motor> motors_;
 
-  double linear_mu_;
-  double angular_mu_;
-  std::vector<double> ground_effect_;
-
-  Eigen::MatrixXd rotor_position_;
-  Eigen::MatrixXd rotor_plane_normal_;
-  Eigen::VectorXd rotor_rotation_direction_;
-
-  Eigen::MatrixXd force_allocation_matrix_;
-  Eigen::MatrixXd torque_allocation_matrix_;
-  Eigen::VectorXd desired_forces_;
-  Eigen::VectorXd desired_torques_;
-  Eigen::VectorXd actual_forces_;
-  Eigen::VectorXd actual_torques_;
-
   /**
    * @brief Declares ROS parameters. Must be called in the constructor.
    */

rosflight_sim/params/multirotor_dynamics.yaml

@@ -4,25 +4,45 @@
 /multirotor/rosflight_sil:
   ros__parameters:
     # Common Global Physical Parameters
-    linear_mu: 0.05
-    angular_mu: 0.0005
-    ground_effect: [ -55.3516, 181.8265, -203.9874, 85.3735, -7.6619 ]
+    rho: 1.225
 
     # Dynamics
     num_rotors: 4
     rotor_positions: [ 0.1926,  0.230, -0.0762,
                       -0.1907,  0.205, -0.0762,
                       -0.1907, -0.205, -0.0762,
                        0.1926, -0.230, -0.0762 ]
+
+    rotor_dists: [.25, .25, .25, .25]
+    rotor_radial_angles: [45., 135., 225., 315.]
+    rotor_rotation_directions: [ -1, 1, -1, 1 ]
+
+    # Prop Model
+    # Thrust
+    CT_0: 0.1196850
+    CT_1: 0.0399995
+    CT_2: -0.400139
 
-    rotor_vector_normal: [ -0.02674078,  0.0223925,  -0.99939157,
-                            0.02553726,  0.02375588, -0.99939157,
-                            0.02553726, -0.02375588, -0.99939157,
-                           -0.02674078, -0.0223925,  -0.99939157 ]
+    # Torque
+    CQ_0: 0.0097976
+    CQ_1: -0.012271
+    CQ_2: -0.014743
 
-    rotor_rotation_directions: [ -1, 1, -1, 1 ]
-    rotor_max_thrust: 14.961
-    rotor_F: [ 1.5e-5, -0.024451, 9.00225 ]
-    rotor_T: [ 2.22e-7,-3.51e-4,  0.12531 ]
-    rotor_tau_up: 0.2164
-    rotor_tau_down: 0.1644
+    # Diameter (m)
+    D_prop: 0.381
+
+    # Motor Specs
+    V_max : 24.0                  # voltage for 6s battery at 4 volts per cell
+    R_motor : 0.042               # ohms
+    I_0 : 1.5                     # no-load (zero torque) current (A)
+    KQ : 0.028937                 #((1 /(KV_rpm_per_volt)) *60) / (2 *M_PI) Back-emf constant, KV in V-s/rad, Motor torque constant, KQ in N-m/A
+
+    # Drag Model
+    CD: 2.05                      # Coeffecient of drag for a rough cube face on into flow.
+    A_c: 0.04                     # Crossectional Area to use in Drag.
+
+    # Induced Drag Model
+    CD_induced: 0.05
+
+    # Ground Effect Model
+    ground_effect: [ -55.3516, 181.8265, -203.9874, 85.3735, -7.6619 ]