diff --git a/drone_physics/body_physics.cpp b/drone_physics/body_physics.cpp index df7d421b..d9b19f76 100644 --- a/drone_physics/body_physics.cpp +++ b/drone_physics/body_physics.cpp @@ -219,8 +219,8 @@ AccelerationType acceleration_in_body_frame( const AngularVelocityType& body_angular_velocity, double thrust, double mass /* 0 is not allowed */, double gravity, /* usually 9.8 > 0*/ - double drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ - double drag2 /* air friction of 2-nd order(-d2*v*v) counter to velocity */) + const VectorType& drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ + const VectorType& drag2 /* air friction of 2-nd order(-d2*v*v) counter to velocity */) { assert(!is_zero(mass)); using std::sin; using std::cos; @@ -247,14 +247,29 @@ AccelerationType acceleration_in_body_frame( */ /*****************************************************************/ - double dot_u = - g * s_theta - (q*w - r*v) - d1/m * u - d2/m * u * u; - double dot_v = + g * c_theta * s_phi - (r*u - p*w) - d1/m * v - d2/m * v * v; - double dot_w = -T/m + g * c_theta * c_phi - (p*v - q*u) - d1/m * w - d2/m * w * w; + double dot_u = - g * s_theta - (q*w - r*v) - d1.x/m * u - d2.x/m * u * u; + double dot_v = + g * c_theta * s_phi - (r*u - p*w) - d1.y/m * v - d2.y/m * v * v; + double dot_w = -T/m + g * c_theta * c_phi - (p*v - q*u) - d1.z/m * w - d2.z/m * w * w; /*****************************************************************/ return {dot_u, dot_v, dot_w}; } +AccelerationType acceleration_in_body_frame( + const VelocityType& body_velocity, + const EulerType& angle, + const AngularVelocityType& body_angular_velocity, + double thrust, double mass /* 0 is not allowed */, + double gravity, /* usually 9.8 > 0*/ + double drag1, /* air friction of 1-st order(-d1*v) counter to velocity(assumed same in 3 directions) */ + double drag2 /* air friction of 2-nd order(-d2*v*v) counter to velocity(assumed same in 3 directions) */) +{ + return acceleration_in_body_frame( + body_velocity, angle, body_angular_velocity, thrust, mass, gravity, + {drag1, drag1, drag1}, {drag2, drag2, drag2}); +} + + /* Obsolete. for testing only. */ AccelerationType acceleration_in_body_frame_without_Coriolis_for_testing_only( diff --git a/drone_physics/body_physics.hpp b/drone_physics/body_physics.hpp index dcdb2cd4..428d3078 100644 --- a/drone_physics/body_physics.hpp +++ b/drone_physics/body_physics.hpp @@ -125,15 +125,26 @@ AccelerationType acceleration_in_body_frame_without_Coriolis_for_testing_only( double thrust, double mass, /* 0 is not allowed */ double gravity, double drag); -/* The right dynamics including Coriolis's Force */ +/* The translation dynamics. drags are the same in the three directions */ AccelerationType acceleration_in_body_frame( const VelocityType& body_velocity, const EulerType& angle, const AngularVelocityType& body_angular_velocity, /* for Coriolis */ double thrust, double mass, /* 0 is not allowed */ double gravity, /* usually 9.8 > 0*/ - double drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ - double drag2 = 0.0 /* air friction of 2-nd order(-d2*v*v) counter to velocity */); + double drag1, /* air friction of 1-st order(-d1*v) counter to velocity(assumed same in 3 directions) */ + double drag2 = 0 /* air friction of 2-nd order(-d2*v*v) counter to velocity(assumed same in 3 directions) */); + +/* The translation dynamics. drags are vectors in the three directions */ +AccelerationType acceleration_in_body_frame( + const VelocityType& body_velocity, + const EulerType& angle, + const AngularVelocityType& body_angular_velocity, /* for Coriolis */ + double thrust, double mass, /* 0 is not allowed */ + double gravity, /* usually 9.8 > 0*/ + const VectorType& drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ + const VectorType& drag2 /* air friction of 2-nd order(-d2*v*v) counter to velocity */); + /* angular acceleration in body frame based on JW' = W x JW =Tb ...eq.(1.137),(2.31) */ AngularAccelerationType angular_acceleration_in_body_frame(