Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[rotorcraft] fix some attitude ref telemety message sending

Browse Source
This commit is contained in:
Felix Ruess
2013-11-15 15:09:06 +01:00
parent f570411036
commit 96d7347f4a
1 changed files with 22 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+22 -4
View File
@@ -92,6 +92,22 @@ static void send_att(void) { //FIXME really use this message here ?
} }
static void send_att_ref(void) { static void send_att_ref(void) {
DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice,
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stab_att_ref_euler.phi,
&stab_att_ref_euler.theta,
&stab_att_ref_euler.psi,
&stab_att_ref_rate.p,
&stab_att_ref_rate.q,
&stab_att_ref_rate.r,
&stab_att_ref_accel.p,
&stab_att_ref_accel.q,
&stab_att_ref_accel.r);
}
static void send_ahrs_ref_quat(void) {
struct Int32Quat* quat = stateGetNedToBodyQuat_i(); struct Int32Quat* quat = stateGetNedToBodyQuat_i();
DOWNLINK_SEND_AHRS_REF_QUAT(DefaultChannel, DefaultDevice, DOWNLINK_SEND_AHRS_REF_QUAT(DefaultChannel, DefaultDevice,
&stab_att_ref_quat.qi, &stab_att_ref_quat.qi,
@@ -111,6 +127,12 @@ void stabilization_attitude_init(void) {
INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err ); INT_EULERS_ZERO( stabilization_att_sum_err );
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
register_periodic_telemetry(DefaultPeriodic, "AHRS_REF_QUAT", send_ahrs_ref_quat);
#endif
} }
void stabilization_attitude_enter(void) { void stabilization_attitude_enter(void) {
@@ -123,10 +145,6 @@ void stabilization_attitude_enter(void) {
INT32_QUAT_ZERO(stabilization_att_sum_err_quat); INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err); INT_EULERS_ZERO(stabilization_att_sum_err);
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_AHRS_REF_QUAT", send_att_ref);
#endif
} }
void stabilization_attitude_set_failsafe_setpoint(void) { void stabilization_attitude_set_failsafe_setpoint(void) {