Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 13:27:32 +08:00
Code Issues Actions 24 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
+1
View File
@@ -206,6 +206,7 @@
<message name="IMU_MAG_SCALED" period=".2"/> <message name="IMU_MAG_SCALED" period=".2"/>
<message name="LIDAR" period="0.05"/> <message name="LIDAR" period="0.05"/>
<message name="ESC" period="0.05"/> <message name="ESC" period="0.05"/>
<message name="STAB_ATTITUDE" period=".1"/>
</mode> </mode>
</process> </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 angular_acceleration[3] = {0., 0., 0.};
float actuator_state[INDI_NUM_ACT]; float actuator_state[INDI_NUM_ACT];
float indi_u[INDI_NUM_ACT]; float indi_u[INDI_NUM_ACT];
float rate_vect_prev[3] = {0., 0., 0.};
float q_filt = 0.0; float q_filt = 0.0;
float r_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; float zero = 0.0;
struct FloatRates *body_rates = stateGetBodyRates_f(); struct FloatRates *body_rates = stateGetBodyRates_f();
struct FloatEulers att_sp; struct FloatEulers att, att_sp;
EULERS_FLOAT_OF_BFP(att_sp, stab_att_sp_euler);
#if GUIDANCE_INDI_HYBRID #if GUIDANCE_INDI_HYBRID
struct FloatEulers att;
float_eulers_of_quat_zxy(&att, stateGetNedToBodyQuat_f()); 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 #else
struct FloatEulers att = *stateGetNedToBodyEulers_f(); att = *stateGetNedToBodyEulers_f();
EULERS_FLOAT_OF_BFP(att_sp, stab_att_sp_euler);
#endif #endif
pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID, pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
&att.phi, &att.theta, &att.psi, // att &att.phi, &att.theta, &att.psi, // att
@@ -434,11 +435,6 @@ void stabilization_indi_init(void)
*/ */
void stabilization_indi_enter(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(du_estimation, INDI_NUM_ACT);
float_vect_zero(ddu_estimation, INDI_NUM_ACT); float_vect_zero(ddu_estimation, INDI_NUM_ACT);
} }