Skip to content

Commit

Permalink
Merge branch 'develop' of github.com:toppers/hakoniwa-px4sim into dev…
Browse files Browse the repository at this point in the history 
…elop
  • Loading branch information
@tmori
tmori committed Nov 13, 2024
2 parents 1cf9fff + 0d618cb commit 506663e
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 37 deletions.
46 changes: 23 additions & 23 deletions drone_physics/rotor_physics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,52 +97,54 @@ double rotor_current( /* current in [A] */

/* thrust from omega in radian/sec eq.(2.50)*/
double rotor_thrust( /* thrust in [N] */
double A, /* the A parameter in Trust = A*(Omega)^2 */
double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2]*/
double omega /* in [rad/s] */ )
{
/**
* Nonami's book (2.50)
* T = A * (Omega)^2
* A is referred to as Ct in Kohei's doc.
* T = Ct * (Omega)^2
* The name 'Ct' is from Kohei's doc and referred to as 'A' in Nonami's eq.(2.50)
* Kohei's doc https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p2
*/
return A * (omega * omega);
return Ct * (omega * omega);
}

/* this makes z-axis rotation eq.(2.56) */
double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/
double B, /* torque coefficient (referred to as Cq in Kohei's doc) in [N m s^2/rad^2] */
double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */
double Jr, /* torque coefficient for 2nd order rotation */
double omega, /* in [rad/s] */
double omega_acceleratoin, /* in [rad/s^2] */
double ccw /* 1 or -1 */ )
{
/**
* See Nonami's book eq.(2.56)
* Ta = B * (Omega)^2 + Jr * (d(Omega)/dt)
* B is referred to as Cq in Kohei's doc.
* Ta = Cq * (Omega)^2 + Jr * (d(Omega)/dt)
* The name 'Cq' is from Kohei's doc and referred to as 'B' in Nonami's eq.(2.56)
* Kohei's doc https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p2
*/
return ccw * ( B * omega * omega + Jr * omega_acceleratoin );
return ccw * ( Cq * omega * omega + Jr * omega_acceleratoin );
}


/* the sum of the n trust from the rotors eq.(2.61) */
double body_thrust( /* thrust in [N] */
double A, /* parameter A in Trust = A*(Omega)^2 in each motor */
double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */
unsigned n, /* number of rotors */
double omega[] /* in radian/sec */ )
{
double thrust = 0;
for (unsigned i = 0; i < n; i++) {
thrust += rotor_thrust(A, omega[i]);
thrust += rotor_thrust(Ct, omega[i]);
}
return thrust;
}

/* the sum of the n torques from the rotors including anti-torque */
/* eq.(2.60)-(2.62)*/
TorqueType body_torque( /* torque in [Nm] */
double A, /* parameter A in Trust = A*(Omega)^2 */
double B, /* anti-torque parameter B in Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */
double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */
double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */
double Jr,
unsigned n, /* number of rotors */
VectorType position[], /* position of each rotor in [m] */
Expand All @@ -161,12 +163,12 @@ TorqueType body_torque( /* torque in [Nm] */
*/
// (1)thrust(calculated always +) is upper direction. change to alight z-axis.
// then the torque is (position vector) x (thrust vector).
const VectorType thrust_vector = { 0, 0, -rotor_thrust(A, omega[i]) };
const VectorType thrust_vector = { 0, 0, -rotor_thrust(Ct, omega[i]) };
auto thrust_torque = cross(position[i], thrust_vector);

// (2) anti-torque around z-axis = yaw.
const auto anti_torque =
rotor_anti_torque(B, Jr, omega[i], omega_acceleration[i], ccw[i]);
rotor_anti_torque(Cq, Jr, omega[i], omega_acceleration[i], ccw[i]);

total_torque += thrust_torque;
total_torque.z += anti_torque;
Expand All @@ -179,17 +181,15 @@ TorqueType body_torque( /* torque in [Nm] */
* used in jMAVsim implemntation.
* Used in comparing with the non-linear(our) model.
*/
double rotor_thrust_linear(
double A2, /* the A parameter in Trust = A*(Omega) */
double omega /* in radian/sec */ )
double rotor_thrust_linear(double Ct2, double omega)
{
return A2 * omega;
return Ct2 * omega;
}
double rotor_anti_torque_linear(double B2, double omega, double ccw)
{
return ccw * ( B2 * omega );
}
TorqueType body_torque_linear(double A2, double B2, unsigned n,
TorqueType body_torque_linear(double Ct2, double Cq2, unsigned n,
VectorType position[], double ccw[], double omega[])
{
TorqueType total_torque = {0, 0, 0};
Expand All @@ -203,23 +203,23 @@ TorqueType body_torque_linear(double A2, double B2, unsigned n,
*/
// (1)thrust(calculated always +) is upper direction. change to alight z-axis.
// then the torque is (position vector) x (thrust vector).
const VectorType thrust_vector = { 0, 0, -rotor_thrust_linear(A2, omega[i]) };
const VectorType thrust_vector = { 0, 0, -rotor_thrust_linear(Ct2, omega[i]) };
auto thrust_torque = cross(position[i], thrust_vector);

// (2) anti-torque around z-axis = yaw.
const auto anti_torque =
rotor_anti_torque_linear(B2, omega[i], ccw[i]);
rotor_anti_torque_linear(Cq2, omega[i], ccw[i]);

total_torque += thrust_torque;
total_torque.z += anti_torque;
}
return total_torque;
}
double body_thrust_linear(double A2, unsigned n, double omega[])
double body_thrust_linear(double Ct2, unsigned n, double omega[])
{
double thrust = 0;
for (unsigned i = 0; i < n; i++) {
thrust += rotor_thrust_linear(A2, omega[i]);
thrust += rotor_thrust_linear(Ct2, omega[i]);
}
return thrust;
}
Expand Down
24 changes: 10 additions & 14 deletions drone_physics/rotor_physics.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,12 +38,12 @@ double rotor_current( /* current in [A] */

/* thrust from omega in radian/sec eq.(2.50)*/
double rotor_thrust( /* thrust in [N] */
double A, /* parameter A in Trust = A*(Omega)^2 */
double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2]*/
double omega /* in [rad/s] */ );

/* this makes z-axis rotation eq.(2.56) */
double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/
double B, /* torque coefficient (referred to as Cq in Kohei's doc) in [N m s^2/rad^2] */
double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */
double Jr, /* torque coefficient for 2nd order rotation */
double omega, /* in [rad/s] */
double omega_acceleratoin, /* in [rad/s^2] */
Expand All @@ -56,16 +56,16 @@ double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/

/* the sum of the n trust from the rotors eq.(2.61) */
double body_thrust( /* thrust in [N] */
double A, /* parameter A in Trust = A*(Omega)^2 in each motor */
double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */
unsigned n, /* number of rotors */
double omega[] /* in radian/sec */ );

/* the sum of the n torques from the rotors including anti-torque */
/* eq.(2.60)-(2.62)*/
TorqueType body_torque( /* torque in [Nm] */
double A, /* parameter A in Trust = A*(Omega)^2 */
double B, /* anti-torque parameter B in Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */
double Jr,
double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */
double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */
double Jr, /* torque coefficient for 2nd order rotation */
unsigned n, /* number of rotors */
VectorType position[], /* position of each rotor in [m] */
double ccw[], /* 1 or -1 */
Expand All @@ -78,15 +78,11 @@ TorqueType body_torque( /* torque in [Nm] */
* used in jMAVsim implemntation.
* Used in comparing with the non-linear(our) model.
*/
double rotor_thrust_linear(
double A, /* the A parameter in Trust = A*(Omega) */
double omega /* in radian/sec */ );

double rotor_anti_torque_linear(double B2, double omega, double ccw);
TorqueType body_torque_linear(double A2, double B2, unsigned n,
double rotor_thrust_linear(double Ct2, double omega);
double rotor_anti_torque_linear(double Cq2, double omega, double ccw);
TorqueType body_torque_linear(double Ct2, double Cq2, unsigned n,
VectorType position[], double ccw[], double omega[]);

double body_thrust_linear(double A2, unsigned n, double omega[]);
double body_thrust_linear(double Ct2, unsigned n, double omega[]);



Expand Down

0 comments on commit 506663e

Please sign in to comment.