added function to handle air friction for 3 directions

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(

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(