[paparazzi-commits] [4369] Add functions to read alpha/ beta vanes and adjust setpoint for booz_stabilization_attitude_quat_float

[Allen Ibara]

Revision: 4369
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4369
Author:   aibara
Date:     2009-12-08 07:11:05 +0000 (Tue, 08 Dec 2009)
Log Message:
-----------
Add functions to read alpha/beta vanes and adjust setpoint for 
booz_stabilization_attitude_quat_float

Modified Paths:
--------------
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
    
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
===================================================================
--- 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h   
    2009-12-08 07:09:07 UTC (rev 4368)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h   
    2009-12-08 07:11:05 UTC (rev 4369)
@@ -29,6 +29,7 @@
 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_read_alpha_vane(float alpha);
 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-12-08 07:09:07 UTC (rev 4368)
+++ 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
    2009-12-08 07:11:05 UTC (rev 4369)
@@ -145,10 +145,37 @@
 
 void booz_stabilization_attitude_read_beta_vane(float beta)
 {
-  booz_stab_att_sp_euler.psi += booz_ahrs_float.ltp_to_body_euler.theta * beta 
/ RC_UPDATE_FREQ;
-  update_sp_quat_from_eulers();
+  struct FloatEulers sticks_eulers;
+  struct FloatQuat sticks_quat, prev_sp_quat;
+
+  sticks_eulers.phi = beta / RC_UPDATE_FREQ;
+  sticks_eulers.theta = 0;
+  sticks_eulers.psi = 0;
+
+  // convert eulers to quaternion
+  FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
+  FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat)
+  
+  // rotate previous setpoint by commanded rotation
+  FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
 }
 
+void booz_stabilization_attitude_read_alpha_vane(float alpha)
+{
+  struct FloatEulers sticks_eulers;
+  struct FloatQuat sticks_quat, prev_sp_quat;
+  sticks_eulers.phi = 0;
+  sticks_eulers.theta = alpha / RC_UPDATE_FREQ;
+  sticks_eulers.psi = 0;
+
+  // convert eulers to quaternion
+  FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
+  FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat)
+  
+  // rotate previous setpoint by commanded rotation
+  FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
+}
+
 void booz_stabilization_attitude_read_rc(bool_t in_flight) {
 
   uint32_t rate_stick_mode = radio_control.values[RADIO_CONTROL_MODE] < -150;