Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[stabilization] fix reporting of attitude setpoint in hybrid mode (#3309)

Browse Source 
also add stab message to flight recorder
This commit is contained in:
Gautier Hattenberger
2024-06-17 14:25:09 +02:00
committed by GitHub
parent 45c79f824a
commit ccfc57ef15
2 changed files with 11 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/telemetry/default_rotorcraft.xml
+5 -4
View File
@@ -37,8 +37,8 @@
<message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>
<mode name="ppm">
@@ -111,8 +111,8 @@
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>
<mode name="vert_loop" key_press="v">
@@ -206,6 +206,7 @@
<message name="IMU_MAG_SCALED" period=".2"/>
<message name="LIDAR" period="0.05"/>
<message name="ESC" period="0.05"/>
<message name="STAB_ATTITUDE" period=".1"/>
</mode>
</process>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+6 -10
View File
@@ -212,7 +212,6 @@ struct FloatRates angular_rate_ref = {0., 0., 0.};
float angular_acceleration[3] = {0., 0., 0.};
float actuator_state[INDI_NUM_ACT];
float indi_u[INDI_NUM_ACT];
float rate_vect_prev[3] = {0., 0., 0.};
float q_filt = 0.0;
float r_filt = 0.0;
@@ -329,13 +328,15 @@ static void send_att_full_indi(struct transport_tx *trans, struct link_device *d
{
float zero = 0.0;
struct FloatRates *body_rates = stateGetBodyRates_f();
struct FloatEulers att_sp;
EULERS_FLOAT_OF_BFP(att_sp, stab_att_sp_euler);
struct FloatEulers att, att_sp;
#if GUIDANCE_INDI_HYBRID
struct FloatEulers att;
float_eulers_of_quat_zxy(&att, stateGetNedToBodyQuat_f());
struct FloatQuat stab_att_sp_quat_f;
QUAT_FLOAT_OF_BFP(stab_att_sp_quat_f, stab_att_sp_quat);
float_eulers_of_quat_zxy(&att_sp, &stab_att_sp_quat_f);
#else
struct FloatEulers att = *stateGetNedToBodyEulers_f();
att = *stateGetNedToBodyEulers_f();
EULERS_FLOAT_OF_BFP(att_sp, stab_att_sp_euler);
#endif
pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
&att.phi, &att.theta, &att.psi, // att
@@ -434,11 +435,6 @@ void stabilization_indi_init(void)
*/
void stabilization_indi_enter(void)
{
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
float_vect_zero(rate_vect_prev, 3);
float_vect_zero(du_estimation, INDI_NUM_ACT);
float_vect_zero(ddu_estimation, INDI_NUM_ACT);
}