[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4220] Move generation of booz attitude reference qu
|
From: |
Allen Ibara |
|
Subject: |
[paparazzi-commits] [4220] Move generation of booz attitude reference quaternion from macro to function and add support for |
|
Date: |
Thu, 01 Oct 2009 19:00:34 +0000 |
Revision: 4220
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4220
Author: aibara
Date: 2009-10-01 19:00:31 +0000 (Thu, 01 Oct 2009)
Log Message:
-----------
Move generation of booz attitude reference quaternion from macro to function
and add support for
setting psi rate from beta vane sensor
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
2009-10-01 18:59:14 UTC (rev 4219)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
2009-10-01 19:00:31 UTC (rev 4220)
@@ -28,6 +28,7 @@
#include STABILISATION_ATTITUDE_H
extern void booz_stabilization_attitude_init(void);
extern void booz_stabilization_attitude_read_rc(bool_t in_flight);
+extern void booz_stabilization_attitude_read_beta_vane(float beta);
extern void booz_stabilization_attitude_enter(void);
extern void booz_stabilization_attitude_run(bool_t in_flight);
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
2009-10-01 18:59:14 UTC (rev 4219)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
2009-10-01 19:00:31 UTC (rev 4220)
@@ -71,17 +71,55 @@
}
+static void reset_psi_ref_from_body(void) {
+ booz_stab_att_sp_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
+ booz_stab_att_ref_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
+ booz_stab_att_ref_rate.r = 0;
+ booz_stab_att_ref_accel.r = 0;
+}
+static void update_quats_from_euler(void) {
+ struct FloatRMat sp_rmat, ref_rmat;
+
+ FLOAT_RMAT_OF_EULERS_312(sp_rmat, booz_stab_att_sp_euler);
+ /* FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp);*/
+ FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat);
+
+ FLOAT_RMAT_OF_EULERS_312(ref_rmat, booz_stab_att_ref_euler);
+ FLOAT_QUAT_OF_RMAT(booz_stab_att_ref_quat, ref_rmat);
+}
+
+void booz_stabilization_attitude_read_beta_vane(float beta)
+{
+ booz_stab_att_ref_rate.r += beta / 4;
+ update_quats_from_euler();
+}
+
void booz_stabilization_attitude_read_rc(bool_t in_flight) {
- BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
+ booz_stab_att_sp_euler.phi = radio_control.values[RADIO_CONTROL_ROLL] *
ROLL_COEF;
+ booz_stab_att_sp_euler.theta = radio_control.values[RADIO_CONTROL_PITCH] *
PITCH_COEF;
+ if (in_flight) {
+ if (YAW_DEADBAND_EXCEEDED()) {
+ booz_stab_att_sp_euler.psi += radio_control.values[RADIO_CONTROL_YAW]
* YAW_COEF;
+ FLOAT_ANGLE_NORMALIZE(booz_stab_att_sp_euler.psi);
+ }
+ } else { /* if not flying, use current yaw as setpoint */
+ reset_psi_ref_from_body();
+ booz_stab_att_ref_rate.r =
RC_UPDATE_FREQ*radio_control.values[RADIO_CONTROL_YAW]*YAW_COEF;
+ }
+
+ update_quats_from_euler();
}
+
void booz_stabilization_attitude_enter(void) {
- BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
+ reset_psi_ref_from_body();
+ update_quats_from_euler();
+
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
FLOAT_QUAT_ZERO( booz_stabilization_sum_err );
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
2009-10-01 18:59:14 UTC (rev 4219)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
2009-10-01 19:00:31 UTC (rev 4220)
@@ -40,46 +40,6 @@
radio_control.values[RADIO_CONTROL_YAW] <
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
-#define BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \
- _sp.psi = booz_ahrs_float.ltp_to_body_euler.psi; \
- booz_stab_att_ref_euler.psi = _sp.psi; \
- booz_stab_att_ref_rate.r = 0; \
- booz_stab_att_ref_accel.r = 0; \
- struct FloatRMat sp_rmat;
\
- FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp); \
- /* FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp);*/ \
- FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat);
\
- FLOAT_RMAT_OF_EULERS_312(sp_rmat, booz_stab_att_ref_euler);
\
- FLOAT_QUAT_OF_RMAT(booz_stab_att_ref_quat, sp_rmat); \
- }
-
-
-#define BOOZ_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
- \
- _sp.phi = radio_control.values[RADIO_CONTROL_ROLL] * ROLL_COEF; \
- _sp.theta = radio_control.values[RADIO_CONTROL_PITCH] * PITCH_COEF; \
- if (_inflight) { \
- if (YAW_DEADBAND_EXCEEDED()) { \
- _sp.psi += radio_control.values[RADIO_CONTROL_YAW] * YAW_COEF; \
- FLOAT_ANGLE_NORMALIZE(_sp.psi); \
- }
\
- struct FloatRMat sp_rmat; \
- FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp); \
- /*FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp);*/ \
- FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat); \
- /*FLOAT_EULERS_OF_QUAT(sp_euler321, sp_quat);*/ \
- } \
- else { /* if not flying, use current yaw as setpoint */ \
- BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp); \
- if (YAW_DEADBAND_EXCEEDED()) { \
- booz_stab_att_ref_rate.r =
RC_UPDATE_FREQ*radio_control.values[RADIO_CONTROL_YAW]*YAW_COEF; \
- }
\
- } \
- \
- }
-
-
-
#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4220] Move generation of booz attitude reference quaternion from macro to function and add support for,
Allen Ibara <=