Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

sih: using Quatf::expq(), fuselage added

Browse Source
This commit is contained in:
romain-chiap
2021-08-12 16:10:03 +02:00
committed by GitHub
parent 265c77b02a
commit c8346534f1
2 changed files with 9 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sih/sih.cpp
+7 -21
View File
@@ -305,10 +305,12 @@ void Sih::generate_aerodynamics()
_wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX); _wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
_tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]); _tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]);
_fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]); _fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
_Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa()) - _KDV * _fuselage.update_aero(_v_B, _w_B, altitude);
_v_I; // sum of aerodynamic forces _Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa())
- _KDV * _v_I; // sum of aerodynamic forces
// _Ma_B = wing_l.Ma + wing_r.Ma + tailplane.Ma + fin.Ma + flap_moments() -_KDW * _w_B; // aerodynamic moments // _Ma_B = wing_l.Ma + wing_r.Ma + tailplane.Ma + fin.Ma + flap_moments() -_KDW * _w_B; // aerodynamic moments
_Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() - _KDW * _w_B; // aerodynamic moments _Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() + _fuselage.get_Ma() - _KDW *
_w_B; // aerodynamic moments
} }
// apply the equations of motion of a rigid body and integrate one step // apply the equations of motion of a rigid body and integrate one step
@@ -320,7 +322,7 @@ void Sih::equations_of_motion()
_p_I_dot = _v_I; // position differential _p_I_dot = _v_I; // position differential
_v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum _v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum
// _q_dot = _q.derivative1(_w_B); // attitude differential // _q_dot = _q.derivative1(_w_B); // attitude differential
_dq = expq(0.5f * _dt * _w_B); _dq = Quatf::expq(0.5f * _dt * _w_B);
_w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum _w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum
// fake ground, avoid free fall // fake ground, avoid free fall
@@ -517,22 +519,6 @@ void Sih::publish_sih()
_gpos_gt_pub.publish(_gpos_gt); _gpos_gt_pub.publish(_gpos_gt);
} }
// quaternion exponential as defined in [3]
Quatf Sih::expq(const matrix::Vector3f &u)
{
float u_norm = u.norm();
Vector3f v;
if (u_norm < 1.0e-6f) { // error will be smaller than 1e-18
v = (1.0f - u_norm * u_norm / 6.0f) * u; // first taylor serie term of sin(x)/x
} else {
v = sinf(u_norm) / u_norm * u;
}
return Quatf(cosf(u_norm), v(0), v(1), v(2));
}
float Sih::generate_wgn() // generate white Gaussian noise sample with std=1 float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
{ {
// algorithm 1: // algorithm 1:
@@ -636,7 +622,7 @@ int Sih::print_usage(const char *reason)
PRINT_MODULE_DESCRIPTION( PRINT_MODULE_DESCRIPTION(
R"DESCR_STR( R"DESCR_STR(
### Description ### Description
This module provide a simulator for quadrotors running fully This module provide a simulator for quadrotors and fixed-wings running fully
inside the hardware autopilot. inside the hardware autopilot.
This simulator subscribes to "actuator_outputs" which are the actuator pwm This simulator subscribes to "actuator_outputs" which are the actuator pwm
src/modules/sih/sih.hpp
+2 -5
View File
@@ -166,10 +166,6 @@ private:
void send_dist_snsr(); void send_dist_snsr();
void publish_sih(); void publish_sih();
void generate_aerodynamics(); void generate_aerodynamics();
float sincf(float x); // sin cardinal = sin(x)/x
matrix::Quatf expq(const matrix::Vector3f &u); // quaternion exponential as defined in [3]
// States eom_f(States); // equations of motion f: x'=f(x)
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
@@ -210,7 +206,8 @@ private:
AeroSeg _wing_r = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, SPAN / 4.0f, 0.0f), -3.0f, AeroSeg _wing_r = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, SPAN / 4.0f, 0.0f), -3.0f,
SPAN / MAC, MAC / 3.0f); SPAN / MAC, MAC / 3.0f);
AeroSeg _tailplane = AeroSeg(0.3f, 0.1f, 0.0f, matrix::Vector3f(-0.4f, 0.0f, 0.0f), 0.0f, -1.0f, 0.05f, RP); AeroSeg _tailplane = AeroSeg(0.3f, 0.1f, 0.0f, matrix::Vector3f(-0.4f, 0.0f, 0.0f), 0.0f, -1.0f, 0.05f, RP);
AeroSeg _fin = AeroSeg(0.25, 0.15, 0.0f, matrix::Vector3f(-0.45f, 0.0f, -0.1f), -90.0f, -1.0f, 0.08f, RP); AeroSeg _fin = AeroSeg(0.25, 0.18, 0.0f, matrix::Vector3f(-0.45f, 0.0f, -0.1f), -90.0f, -1.0f, 0.12f, RP);
AeroSeg _fuselage = AeroSeg(0.2, 0.8, 0.0f, matrix::Vector3f(0.0f, 0.0f, 0.0f), -90.0f);
// sensors reconstruction // sensors reconstruction
matrix::Vector3f _acc; matrix::Vector3f _acc;