Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

added a floating point version of the current attitude stabilization loop and a floating point version of a quaternion based attitude stabilization loop

Browse Source
This commit is contained in:
Antoine Drouin
2009-07-26 20:15:21 +00:00
parent 18342805dd
commit 91e2d0a346
34 changed files with 1378 additions and 607 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_a1.xml
+3 -5
View File
@@ -92,9 +92,9 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(45.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
@@ -198,8 +198,6 @@ include $(PAPARAZZI_SRC)/conf/autopilot/booz2_common.makefile
include $(CFG_BOOZ)/booz2_autopilot.makefile
include $(CFG_BOOZ)/booz2_test_progs.makefile
sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a1.h\"
#include $(CFG_BOOZ)/booz2_simulator.makefile
include $(CFG_BOOZ)/booz2_simulator_nps.makefile
ap.CFLAGS += -DMODEM_BAUD=B57600
conf/airframes/booz2_a2.xml
+33 -11
View File
@@ -72,21 +72,46 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(45.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-400"/>
<define name="PHI_THETA_DGAIN" value="-300"/>
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-100"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(300.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="RadOfDeg(300.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="SUPERVISION" prefix="SUPERVISION_">
@@ -149,9 +174,6 @@ include $(PAPARAZZI_SRC)/conf/autopilot/booz2_common.makefile
include $(CFG_BOOZ)/booz2_autopilot.makefile
include $(CFG_BOOZ)/booz2_test_progs.makefile
sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\"
include $(CFG_BOOZ)/booz2_simulator.makefile
ap.CFLAGS += -DMODEM_BAUD=B57600
include $(CFG_BOOZ)/subsystems/booz2_radio_control_ppm.makefile
include $(CFG_BOOZ)/subsystems/booz2_actuators_asctec.makefile
conf/airframes/booz2_a3.xml
+42 -16
View File
@@ -1,4 +1,4 @@
<airframe name="BOOZ2_A2">
<airframe name="BOOZ2_A3">
<servos min="0" neutral="0" max="0xff">
<servo name="PITCH" no="0" min="0" neutral="0" max="255"/>
@@ -72,21 +72,46 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(45.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-375"/>
<define name="PHI_THETA_DGAIN" value="-250"/>
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-50"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(300.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="RadOfDeg(300.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-375"/>
<define name="PHI_DGAIN" value="-250"/>
<define name="PHI_IGAIN" value="-50"/>
<define name="THETA_PGAIN" value="-375"/>
<define name="THETA_DGAIN" value="-250"/>
<define name="THETA_IGAIN" value="-50"/>
<define name="PSI_PGAIN" value="-300"/>
<define name="PSI_DGAIN" value="-130"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-50"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="SUPERVISION" prefix="SUPERVISION_">
@@ -146,16 +171,17 @@ BOARD_CFG = \"booz2_board_v1_0.h\"
#ap.CFLAGS += -DKILL_MOTORS
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_common.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_autopilot.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_test_progs.makefile
include $(CFG_BOOZ)/booz2_autopilot.makefile
include $(CFG_BOOZ)/booz2_test_progs.makefile
sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\"
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_simulator.makefile
include $(CFG_BOOZ)/booz2_simulator_nps.makefile
ap.CFLAGS += -DMODEM_BAUD=B57600
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_actuators_asctec.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_radio_control_spektrum.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_imu_b2v1_1.makefile
ap.CFLAGS += -DMODEM_BAUD=B57600
include $(CFG_BOOZ)/subsystems/booz2_radio_control_spektrum.makefile
include $(CFG_BOOZ)/subsystems/booz2_actuators_asctec.makefile
include $(CFG_BOOZ)/subsystems/booz2_imu_b2v1.makefile
# NO GPS, uses spektrum
</makefile>
conf/airframes/booz2_flixr.xml
-1
View File
@@ -189,7 +189,6 @@ include $(PAPARAZZI_SRC)/conf/autopilot/booz2_common.makefile
include $(CFG_BOOZ)/booz2_autopilot.makefile
include $(CFG_BOOZ)/booz2_test_progs.makefile
sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a1.h\"
include $(CFG_BOOZ)/booz2_simulator_nps.makefile
ap.CFLAGS += -DMODEM_BAUD=B57600
conf/airframes/booz2_g1.xml
+33 -8
View File
@@ -78,21 +78,46 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(45.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-1200"/>
<define name="PHI_THETA_DGAIN" value="-300"/>
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-200"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(300.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="RadOfDeg(300.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-1200"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="-1200"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="INS" prefix="BOOZ_INS_">
conf/airframes/booz2_s1.xml
+33 -10
View File
@@ -66,8 +66,6 @@
<define name="MAG_Z_SENS" value="-2.82215800" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(-11.))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
@@ -89,21 +87,46 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(45.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-400"/>
<define name="PHI_THETA_DGAIN" value="-300"/>
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-100"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(300.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="RadOfDeg(300.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="INS" prefix="BOOZ_INS_">
conf/airframes/booz2_x1.xml
+33 -8
View File
@@ -92,21 +92,46 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(45.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-400"/>
<define name="PHI_THETA_DGAIN" value="-300"/>
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-100"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(300.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="RadOfDeg(300.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="INS" prefix="BOOZ_INS_">
conf/autopilot/booz2_autopilot.makefile
+7 -1
View File
@@ -113,7 +113,13 @@ ap.srcs += $(SRC_BOOZ)/ahrs/booz2_filter_attitude_cmpl_euler.c
ap.srcs += math/pprz_trig_int.c
ap.srcs += $(SRC_BOOZ)/booz_stabilization.c
ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude.c
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c
ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c
ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
conf/autopilot/booz2_simulator_nps.makefile
+18 -3
View File
@@ -94,10 +94,25 @@ sim.srcs += $(SRC_BOOZ)/ahrs/booz2_filter_attitude_cmpl_euler.c
sim.srcs += $(SRC_BOOZ)/booz_stabilization.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_H=\"stabilization/booz_stabilization_attitude_euler.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler.c
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_FLOAT
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_float.h\"
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_float.h\"
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_float.c
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_float.c
sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_FLOAT
sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_float.h\"
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_float.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_quat_float.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c
conf/autopilot/subsystems/booz2_radio_control_spektrum.makefile
+5 -3
View File
@@ -1,8 +1,10 @@
#
# Autopilot
#
ap.CFLAGS += -DUSE_RADIO_CONTROL -DRADIO_CONTROL_LED=1
ap.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/booz_radio_control_spektrum.h\"
ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"impl/booz_radio_control_spektrum_dx7se.h\"
ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"radio_control/booz_radio_control_spektrum_dx7se.h\"
ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200
ap.CFLAGS += -DRADIO_CONTROL_LINK=Uart0
ap.srcs += $(SRC_BOOZ)/booz_radio_control.c \
$(SRC_BOOZ_IMPL)/booz_radio_control_spektrum.c \
$(SRC_BOOZ)/radio_control/booz_radio_control_spektrum.c \
conf/messages.xml
+65 -60
View File
@@ -644,30 +644,6 @@
<field name="throttle" type="uint16"/>
</message>
<message name="BOOZ2_STABILIZATION" id="130">
<field name="pd_ref" type="float" unit="rad/s2" alt_unit="deg/s2" alt_unit_coef="57.29578"/>
<field name="qd_ref" type="float" unit="rad/s2" alt_unit="deg/s2" alt_unit_coef="57.29578"/>
<field name="rd_ref" type="float" unit="rad/s2" alt_unit="deg/s2" alt_unit_coef="57.29578"/>
<field name="p_est" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
<field name="p_ref" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
<field name="q_est" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
<field name="q_ref" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
<field name="r_est" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
<field name="r_ref" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
<field name="phi_est" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="phi_ref" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="phi_sp" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="theta_est" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="theta_ref" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="theta_sp" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="psi_est" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="psi_ref" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="psi_sp" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="cmd_a" type="int32"/>
<field name="cmd_e" type="int32"/>
<field name="cmd_r" type="int32"/>
<field name="cmd_t" type="int32"/>
</message>
<message name="BOOZ2_GYRO" id="131">
<field name="gp" type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
@@ -755,53 +731,83 @@
<field name="qz" type="int32"/>
</message>
<message name="BOOZ2_STAB_ATTITUDE" id="140">
<field name="m_p" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="m_q" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="m_r" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="m_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="m_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="m_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="sp_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="sp_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="sp_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<message name="BOOZ2_STAB_ATTITUDE_INT" id="140">
<field name="est_p" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="est_q" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="est_r" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="est_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="est_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="est_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="sp_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="sp_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="sp_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="sum_err_phi" type="int32"/>
<field name="sum_err_theta" type="int32"/>
<field name="sum_err_psi" type="int32"/>
<field name="delta_a_fb" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_e_fb" type="int32"/>
<field name="delta_r_fb" type="int32"/>
<field name="delta_a_ff" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_e_ff" type="int32"/>
<field name="delta_r_ff" type="int32"/>
<field name="delta_a_fb" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_e_fb" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_r_fb" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_a_ff" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_e_ff" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_r_ff" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_a" type="int32"/>
<field name="delta_e" type="int32"/>
<field name="delta_r" type="int32"/>
</message>
<message name="BOOZ2_STAB_ATTITUDE_REF" id="141">
<field name="pd" type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
<field name="qd" type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
<field name="rd" type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
<field name="p" type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>
<field name="q" type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>
<field name="r" type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>
<field name="phi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="theta" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="psi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<message name="BOOZ2_STAB_ATTITUDE_REF_INT" id="141">
<field name="sp_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="sp_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="sp_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="ref_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="ref_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="ref_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="ref_p" type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>
<field name="ref_q" type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>
<field name="ref_r" type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>
<field name="ref_pd" type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
<field name="ref_qd" type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
<field name="ref_rd" type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
</message>
<message name="BOOZ2_STAB_ATTITUDE_HS_ROLL" id="142">
<field name="phi_ref" type="int32" alt_unit="degres" alt_unit_coef="0.0000546"/>
<field name="p_ref" type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>
<field name="pd_ref" type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
<field name="m_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="m_p" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="sum_err_phi" type="int32"/>
<field name="delta_a" type="int32"/>
<message name="BOOZ2_STAB_ATTITUDE_FLOAT" id="130">
<field name="est_p" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="est_q" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="est_r" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="est_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="est_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="est_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="ref_phi" type="float" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="ref_theta" type="int32" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="ref_psi" type="int32" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="sum_err_phi" type="float"/>
<field name="sum_err_theta" type="float"/>
<field name="sum_err_psi" type="float"/>
<field name="delta_a_fb" type="float" alt_unit_coef="0.0625"/>
<field name="delta_e_fb" type="float" alt_unit_coef="0.0625"/>
<field name="delta_r_fb" type="float" alt_unit_coef="0.0625"/>
<field name="delta_a_ff" type="float" alt_unit_coef="0.0625"/>
<field name="delta_e_ff" type="float" alt_unit_coef="0.0625"/>
<field name="delta_r_ff" type="float" alt_unit_coef="0.0625"/>
<field name="delta_a" type="int32"/>
<field name="delta_e" type="int32"/>
<field name="delta_r" type="int32"/>
</message>
<message name="BOOZ2_STAB_ATTITUDE_REF_FLOAT" id="142">
<field name="sp_phi" type="float" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="sp_theta" type="float" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="sp_psi" type="float" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="ref_phi" type="float" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="ref_theta" type="float" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="ref_psi" type="float" alt_unit="degres" alt_unit_coef="57.29578"/>
<field name="ref_p" type="float" alt_unit="degres/s" alt_unit_coef="57.29578"/>
<field name="ref_q" type="float" alt_unit="degres/s" alt_unit_coef="57.29578"/>
<field name="ref_r" type="float" alt_unit="degres/s" alt_unit_coef="57.29578"/>
<field name="ref_pd" type="float" alt_unit="degres/s2" alt_unit_coef="57.29578"/>
<field name="ref_qd" type="float" alt_unit="degres/s2" alt_unit_coef="57.29578"/>
<field name="ref_rd" type="float" alt_unit="degres/s2" alt_unit_coef="57.29578"/>
</message>
<message name="BOOZ2_CMD" id="143">
<field name="cmd_roll" type="int32"/>
@@ -810,7 +816,6 @@
<field name="cmd_thrust" type="int32"/>
</message>
<message name="BOOZ2_GUIDANCE" id="144">
<field name="pos_n" type="int32"/>
<field name="pos_e" type="int32"/>
conf/telemetry/telemetry_booz2.xml
+1 -1
View File
@@ -96,7 +96,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="ALIVE" period="0.9"/>
<message name="DL_VALUE" period="0.5"/>
<message name="BOOZ2_STAB_ATTITUDE_HS_ROLL" period="0.02"/>
<!-- <message name="BOOZ2_STAB_ATTITUDE_HS_ROLL" period="0.02"/> -->
</mode>
<mode name="tune_hover">
sw/airborne/booz/booz2_telemetry.h
+76 -52
View File
@@ -158,65 +158,89 @@
&booz_stabilization_cmd[COMMAND_THRUST]); \
}
#ifdef STABILISATION_ATTITUDE_TYPE_INT
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE() { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE(&booz_ahrs.body_rate.p, \
&booz_ahrs.body_rate.q, \
&booz_ahrs.body_rate.r, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi, \
&booz_stabilization_att_sp.phi, \
&booz_stabilization_att_sp.theta, \
&booz_stabilization_att_sp.psi, \
&booz_stabilization_att_sum_err.phi, \
&booz_stabilization_att_sum_err.theta, \
&booz_stabilization_att_sum_err.psi, \
&booz_stabilization_att_fb_cmd[COMMAND_ROLL], \
&booz_stabilization_att_fb_cmd[COMMAND_PITCH], \
&booz_stabilization_att_fb_cmd[COMMAND_YAW], \
&booz_stabilization_att_ff_cmd[COMMAND_ROLL], \
&booz_stabilization_att_ff_cmd[COMMAND_PITCH], \
&booz_stabilization_att_ff_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_ROLL], \
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW]); \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_INT(&booz_ahrs.body_rate.p, \
&booz_ahrs.body_rate.q, \
&booz_ahrs.body_rate.r, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi, \
&booz_stab_att_sp_euler.phi, \
&booz_stab_att_sp_euler.theta, \
&booz_stab_att_sp_euler.psi, \
&booz_stabilization_att_sum_err.phi, \
&booz_stabilization_att_sum_err.theta, \
&booz_stabilization_att_sum_err.psi, \
&booz_stabilization_att_fb_cmd[COMMAND_ROLL], \
&booz_stabilization_att_fb_cmd[COMMAND_PITCH], \
&booz_stabilization_att_fb_cmd[COMMAND_YAW], \
&booz_stabilization_att_ff_cmd[COMMAND_ROLL], \
&booz_stabilization_att_ff_cmd[COMMAND_PITCH], \
&booz_stabilization_att_ff_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_ROLL], \
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW]); \
}
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_REF() { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_REF(&booz_stabilization_accel_ref.x, \
&booz_stabilization_accel_ref.y, \
&booz_stabilization_accel_ref.z, \
&booz_stabilization_rate_ref.x, \
&booz_stabilization_rate_ref.y, \
&booz_stabilization_rate_ref.z, \
&booz_stabilization_att_ref.phi, \
&booz_stabilization_att_ref.theta, \
&booz_stabilization_att_ref.psi ); \
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_REF() { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_REF_INT(&booz_stab_att_sp_euler.phi, \
&booz_stab_att_sp_euler.theta, \
&booz_stab_att_sp_euler.psi, \
&booz_stab_att_ref_euler.phi, \
&booz_stab_att_ref_euler.theta, \
&booz_stab_att_ref_euler.psi, \
&booz_stab_att_ref_rate.p, \
&booz_stab_att_ref_rate.q, \
&booz_stab_att_ref_rate.r, \
&booz_stab_att_ref_accel.p, \
&booz_stab_att_ref_accel.q, \
&booz_stab_att_ref_accel.r); \
}
#endif /* STABILISATION_ATTITUDE_TYPE_INT */
#ifdef STABILISATION_ATTITUDE_TYPE_FLOAT
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE() { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_FLOAT(&booz_ahrs.body_rate.p, \
&booz_ahrs.body_rate.q, \
&booz_ahrs.body_rate.r, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi, \
&booz_stab_att_ref_euler.phi, \
&booz_stab_att_ref_euler.theta, \
&booz_stab_att_ref_euler.psi, \
&booz_stabilization_att_sum_err.phi, \
&booz_stabilization_att_sum_err.theta, \
&booz_stabilization_att_sum_err.psi, \
&booz_stabilization_att_fb_cmd[COMMAND_ROLL], \
&booz_stabilization_att_fb_cmd[COMMAND_PITCH], \
&booz_stabilization_att_fb_cmd[COMMAND_YAW], \
&booz_stabilization_att_ff_cmd[COMMAND_ROLL], \
&booz_stabilization_att_ff_cmd[COMMAND_PITCH], \
&booz_stabilization_att_ff_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_ROLL], \
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW]); \
}
#if defined HS_YAW
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL() { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL(&booz_stabilization_att_ref.psi, \
&booz_stabilization_rate_ref.z, \
&booz_stabilization_accel_ref.z, \
&booz_ahrs.ltp_to_body_euler.psi, \
&booz_ahrs.body_rate.r, \
&booz_stabilization_att_sum_err.psi, \
&booz_stabilization_cmd[COMMAND_YAW]); \
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_REF() { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_REF_FLOAT(&booz_stab_att_sp_euler.phi, \
&booz_stab_att_sp_euler.theta, \
&booz_stab_att_sp_euler.psi, \
&booz_stab_att_ref_euler.phi, \
&booz_stab_att_ref_euler.theta, \
&booz_stab_att_ref_euler.psi, \
&booz_stab_att_ref_rate.p, \
&booz_stab_att_ref_rate.q, \
&booz_stab_att_ref_rate.r, \
&booz_stab_att_ref_accel.p, \
&booz_stab_att_ref_accel.q, \
&booz_stab_att_ref_accel.r); \
}
#else
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL() { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL(&booz_stabilization_att_ref.phi, \
&booz_stabilization_rate_ref.x, \
&booz_stabilization_accel_ref.x, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.body_rate.p, \
&booz_stabilization_att_sum_err.phi, \
&booz_stabilization_cmd[COMMAND_ROLL]); \
}
#endif
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
#include "ahrs/booz_ahrs_aligner.h"
sw/airborne/booz/guidance/booz2_guidance_h.c
+11 -5
View File
@@ -152,12 +152,14 @@ void booz2_guidance_h_run(bool_t in_flight) {
case BOOZ2_GUIDANCE_H_MODE_NAV:
{
if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
booz_stabilization_att_sp.phi = 0;
booz_stabilization_att_sp.theta = 0;
booz_stab_att_sp_euler.phi = 0;
booz_stab_att_sp_euler.theta = 0;
}
else {
INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
booz2_guidance_h_psi_sp = (nav_heading << (ANGLE_REF_RES - INT32_ANGLE_FRAC));
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
#endif
booz2_guidance_h_hover_run();
}
booz_stabilization_attitude_run(in_flight);
@@ -224,9 +226,11 @@ static inline void booz2_guidance_h_hover_run(void) {
booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi;
booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp + booz2_guidance_h_rc_sp.psi;
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
EULERS_COPY(booz_stabilization_att_sp, booz2_guidance_h_command_body);
EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
}
@@ -250,7 +254,9 @@ static inline void booz2_guidance_h_nav_enter(void) {
struct Int32Eulers tmp_sp;
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp );
booz2_guidance_h_psi_sp = tmp_sp.psi;
nav_heading = (booz2_guidance_h_psi_sp >> (ANGLE_REF_RES - INT32_ANGLE_FRAC));
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
nav_heading = (booz2_guidance_h_psi_sp >> (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
booz2_guidance_h_rc_sp.psi = 0;
INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
sw/airborne/booz/stabilization/booz_stabilization_attitude.h
+5 -13
View File
@@ -24,25 +24,17 @@
#ifndef BOOZ_STABILIZATION_ATTITUDE_H
#define BOOZ_STABILIZATION_ATTITUDE_H
#include "math/pprz_algebra_int.h"
#include "airframe.h"
#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_enter(void);
extern void booz_stabilization_attitude_run(bool_t in_flight);
#include "stabilization/booz_stabilization_attitude_ref_traj_euler.h"
extern struct Int32Vect3 booz_stabilization_igain;
extern struct Int32Vect3 booz_stabilization_pgain;
extern struct Int32Vect3 booz_stabilization_dgain;
extern struct Int32Vect3 booz_stabilization_ddgain;
extern struct Int32Eulers booz_stabilization_att_sum_err;
extern int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
extern int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB];
#include "stabilization/booz_stabilization_attitude_ref.h"
#include STABILISATION_ATTITUDE_REF_H
extern void booz_stabilization_attitude_ref_init(void);
extern void booz_stabilization_attitude_ref_update(void);
#define booz_stabilization_attitude_SetKiPhi(_val) { \
sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
+151
View File
@@ -0,0 +1,151 @@
/*
* $Id: booz_stabilization_attitude_euler.c 3795 2009-07-24 23:43:02Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
#include "math/pprz_algebra_float.h"
#include "booz_ahrs.h"
#include "booz_radio_control.h"
#include "airframe.h"
struct FloatVect3 booz_stabilization_pgain;
struct FloatVect3 booz_stabilization_dgain;
struct FloatVect3 booz_stabilization_ddgain;
struct FloatVect3 booz_stabilization_igain;
struct FloatEulers booz_stabilization_att_sum_err;
float booz_stabilization_att_fb_cmd[COMMANDS_NB];
float booz_stabilization_att_ff_cmd[COMMANDS_NB];
void booz_stabilization_attitude_init(void) {
booz_stabilization_attitude_ref_init();
VECT3_ASSIGN(booz_stabilization_pgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(booz_stabilization_dgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(booz_stabilization_igain,
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(booz_stabilization_ddgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
}
void booz_stabilization_attitude_read_rc(bool_t in_flight) {
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
}
void booz_stabilization_attitude_enter(void) {
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
}
#define MAX_SUM_ERR RadOfDeg(56000)
void booz_stabilization_attitude_run(bool_t in_flight) {
booz_stabilization_attitude_ref_update();
/* Compute feedforward */
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
booz_stabilization_ddgain.x * booz_stab_att_ref_accel.p / 32.;
booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
booz_stabilization_ddgain.y * booz_stab_att_ref_accel.q / 32.;
booz_stabilization_att_ff_cmd[COMMAND_YAW] =
booz_stabilization_ddgain.z * booz_stab_att_ref_accel.r / 32.;
/* Compute feedback */
/* attitude error */
struct FloatEulers att_float;
EULERS_FLOAT_OF_BFP(att_float, booz_ahrs.ltp_to_body_euler);
struct FloatEulers att_err;
EULERS_DIFF(att_err, att_float, booz_stab_att_ref_euler);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
/* update integrator */
EULERS_ADD(booz_stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(booz_stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
FLOAT_EULERS_ZERO(booz_stabilization_att_sum_err);
}
/* rate error */
struct FloatRates rate_float;
RATES_FLOAT_OF_BFP(rate_float, booz_ahrs.body_rate);
struct FloatRates rate_err;
RATES_DIFF(rate_err, rate_float, booz_stab_att_ref_rate);
/* PID */
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
booz_stabilization_pgain.x * att_err.phi +
booz_stabilization_dgain.x * rate_err.p +
booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi / 1024.;
booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
booz_stabilization_pgain.y * att_err.theta +
booz_stabilization_dgain.y * rate_err.q +
booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta / 1024.;
booz_stabilization_att_fb_cmd[COMMAND_YAW] =
booz_stabilization_pgain.z * att_err.psi +
booz_stabilization_dgain.z * rate_err.r +
booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi / 1024.;
booz_stabilization_cmd[COMMAND_ROLL] =
(booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
booz_stabilization_cmd[COMMAND_PITCH] =
(booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
booz_stabilization_cmd[COMMAND_YAW] =
(booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW])/16.;
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c → sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
+39 -82
View File
@@ -29,12 +29,6 @@
#include "airframe.h"
struct Int32Eulers booz_stabilization_att_sp;
struct Int32Eulers booz_stabilization_att_ref;
struct Int32Vect3 booz_stabilization_rate_ref;
struct Int32Vect3 booz_stabilization_accel_ref;
struct Int32Vect3 booz_stabilization_pgain;
struct Int32Vect3 booz_stabilization_dgain;
struct Int32Vect3 booz_stabilization_ddgain;
@@ -44,17 +38,11 @@ struct Int32Eulers booz_stabilization_att_sum_err;
int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB];
static inline void booz_stabilization_update_ref(void);
void booz_stabilization_attitude_init(void) {
INT_EULERS_ZERO(booz_stabilization_att_sp);
INT_EULERS_ZERO(booz_stabilization_att_ref);
INT_VECT3_ZERO(booz_stabilization_rate_ref);
INT_VECT3_ZERO(booz_stabilization_accel_ref);
booz_stabilization_attitude_ref_init();
VECT3_ASSIGN(booz_stabilization_pgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
@@ -83,14 +71,14 @@ void booz_stabilization_attitude_init(void) {
void booz_stabilization_attitude_read_rc(bool_t in_flight) {
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stabilization_att_sp, in_flight);
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
}
void booz_stabilization_attitude_enter(void) {
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stabilization_att_sp );
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
INT_EULERS_ZERO( booz_stabilization_att_sum_err );
}
@@ -103,13 +91,24 @@ void booz_stabilization_attitude_enter(void) {
void booz_stabilization_attitude_run(bool_t in_flight) {
booz_stabilization_update_ref();
/* compute attitude error */
/* update reference */
booz_stabilization_attitude_ref_update();
/* compute feedforward command */
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(booz_stabilization_ddgain.x * booz_stab_att_ref_accel.p, 5);
booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND(booz_stabilization_ddgain.y * booz_stab_att_ref_accel.q, 5);
booz_stabilization_att_ff_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND(booz_stabilization_ddgain.z * booz_stab_att_ref_accel.r, 5);
/* compute feedback command */
/* attitude error */
const struct Int32Eulers att_ref_scaled = {
OFFSET_AND_ROUND(booz_stabilization_att_ref.phi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_att_ref.theta, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_att_ref.psi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)) };
OFFSET_AND_ROUND(booz_stab_att_ref_euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(booz_stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(booz_stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) };
struct Int32Eulers att_err;
EULERS_DIFF(att_err, booz_ahrs.ltp_to_body_euler, att_ref_scaled);
INT32_ANGLE_NORMALIZE(att_err.psi);
@@ -123,82 +122,40 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
INT_EULERS_ZERO(booz_stabilization_att_sum_err);
}
/* compute rate error */
/* rate error */
const struct Int32Rates rate_ref_scaled = {
OFFSET_AND_ROUND(booz_stabilization_rate_ref.x, (RATE_REF_RES - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_rate_ref.y, (RATE_REF_RES - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_rate_ref.z, (RATE_REF_RES - INT32_RATE_FRAC)) };
OFFSET_AND_ROUND(booz_stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates rate_err;
RATES_DIFF(rate_err, booz_ahrs.body_rate, rate_ref_scaled);
/* compute PID loop */
/* PID */
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
booz_stabilization_pgain.x * att_err.phi +
booz_stabilization_dgain.x * rate_err.p +
OFFSET_AND_ROUND2((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi), 10);
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x, 5);
booz_stabilization_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL]), 16);
booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
booz_stabilization_pgain.y * att_err.theta +
booz_stabilization_dgain.y * rate_err.q +
((booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta) >> 10);
booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_att_fb_cmd[COMMAND_PITCH] +
((booz_stabilization_ddgain.y * booz_stabilization_accel_ref.y) >> 5);
booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_cmd[COMMAND_PITCH] >> 16;
OFFSET_AND_ROUND2((booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta), 10);
booz_stabilization_att_fb_cmd[COMMAND_YAW] =
booz_stabilization_pgain.z * att_err.psi +
booz_stabilization_dgain.z * rate_err.r +
((booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi) >> 10);
booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_att_fb_cmd[COMMAND_YAW] +
((booz_stabilization_ddgain.z * booz_stabilization_accel_ref.z) >> 5);
booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_cmd[COMMAND_YAW] >> 16;
OFFSET_AND_ROUND2((booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi), 10);
/* sum feedforward and feedback */
booz_stabilization_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL]), 16);
booz_stabilization_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH]), 16);
booz_stabilization_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW]), 16);
}
/*
generation of a saturated linear second order reference trajectory
roll/pitch
omega : 1100 deg s-1
zeta : 0.85
max rotational accel : 128 rad.s-2 ( ~7300 deg s-2 )
max rotational speed : 8 rad/s ( ~ 458 deg s-1 )
yaw
omega : 500 deg s-1
zeta : 0.85
max rotational accel : 32 rad.s-2 ( ~1833 deg s-2 )
max rotational speed : 4 rad/s ( ~ 230 deg s-1 )
representation
accel : 20.12
speed : 16.16
angle : 12.20
*/
#define USE_REF 1
static inline void booz_stabilization_update_ref(void) {
#ifdef USE_REF
BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_UPDATE();
#else
EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp);
INT_VECT3_ZERO(booz_stabilization_rate_ref);
INT_VECT3_ZERO(booz_stabilization_accel_ref);
#endif
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
+41
View File
@@ -0,0 +1,41 @@
/*
* $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_FLOAT_H
#define BOOZ_STABILIZATION_ATTITUDE_FLOAT_H
#include "math/pprz_algebra_float.h"
#include "airframe.h"
extern struct FloatVect3 booz_stabilization_pgain;
extern struct FloatVect3 booz_stabilization_dgain;
extern struct FloatVect3 booz_stabilization_ddgain;
extern struct FloatVect3 booz_stabilization_igain;
extern struct FloatEulers booz_stabilization_att_sum_err;
extern float booz_stabilization_att_fb_cmd[COMMANDS_NB];
extern float booz_stabilization_att_ff_cmd[COMMANDS_NB];
#endif /* BOOZ_STABILIZATION_ATTITUDE_FLOAT_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h
+41
View File
@@ -0,0 +1,41 @@
/*
* $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_INT_H
#define BOOZ_STABILIZATION_ATTITUDE_INT_H
#include "math/pprz_algebra_int.h"
#include "airframe.h"
extern struct Int32Vect3 booz_stabilization_igain;
extern struct Int32Vect3 booz_stabilization_pgain;
extern struct Int32Vect3 booz_stabilization_dgain;
extern struct Int32Vect3 booz_stabilization_ddgain;
extern struct Int32Eulers booz_stabilization_att_sum_err;
extern int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
extern int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB];
#endif /* BOOZ_STABILIZATION_ATTITUDE_INT_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
+84 -115
View File
@@ -22,173 +22,142 @@
*/
#include "booz_stabilization.h"
#include "math/pprz_algebra_float.h"
#include "booz_ahrs.h"
#include "booz_radio_control.h"
#include "airframe.h"
struct Int32Eulers booz_stabilization_att_sp;
struct Int32Eulers booz_stabilization_att_ref;
struct Int32Vect3 booz_stabilization_rate_ref;
struct Int32Vect3 booz_stabilization_accel_ref;
struct FloatVect3 booz_stabilization_pgain;
struct FloatVect3 booz_stabilization_dgain;
struct FloatVect3 booz_stabilization_ddgain;
struct FloatVect3 booz_stabilization_igain;
struct FloatEulers booz_stabilization_att_sum_err;
struct Int32Vect3 booz_stabilization_pgain;
struct Int32Vect3 booz_stabilization_dgain;
struct Int32Vect3 booz_stabilization_ddgain;
struct Int32Vect3 booz_stabilization_igain;
struct Int32Eulers booz_stabilization_att_sum_err;
int32_t booz_stabilization_att_err_cmd[COMMANDS_NB];
static inline void booz_stabilization_update_ref(void);
float booz_stabilization_att_fb_cmd[COMMANDS_NB];
float booz_stabilization_att_ff_cmd[COMMANDS_NB];
void booz_stabilization_attitude_init(void) {
INT_EULERS_ZERO(booz_stabilization_att_sp);
INT_EULERS_ZERO(booz_stabilization_att_ref);
INT_VECT3_ZERO(booz_stabilization_rate_ref);
INT_VECT3_ZERO(booz_stabilization_accel_ref);
booz_stabilization_attitude_ref_init();
VECT3_ASSIGN(booz_stabilization_pgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(booz_stabilization_dgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(booz_stabilization_ddgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
VECT3_ASSIGN(booz_stabilization_igain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
INT_EULERS_ZERO( booz_stabilization_att_sum_err );
VECT3_ASSIGN(booz_stabilization_ddgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
}
void booz_stabilization_attitude_read_rc(bool_t in_flight) {
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stabilization_att_sp, in_flight);
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
}
void booz_stabilization_attitude_enter(void) {
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stabilization_att_sp );
INT_EULERS_ZERO( booz_stabilization_att_sum_err );
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
}
#define MAX_SUM_ERR 4000000
#define MAX_SUM_ERR RadOfDeg(56000)
void booz_stabilization_attitude_run(bool_t in_flight) {
booz_stabilization_update_ref();
/*
* Update reference
*/
booz_stabilization_attitude_ref_update();
/* compute attitude error */
const struct Int32Eulers att_ref_scaled = {
booz_stabilization_att_ref.phi >> (ANGLE_REF_RES - INT32_ANGLE_FRAC),
booz_stabilization_att_ref.theta >> (ANGLE_REF_RES - INT32_ANGLE_FRAC),
booz_stabilization_att_ref.psi >> (ANGLE_REF_RES - INT32_ANGLE_FRAC) };
struct Int32Eulers att_err;
EULERS_DIFF(att_err, booz_ahrs.ltp_to_body_euler, att_ref_scaled);
INT32_ANGLE_NORMALIZE(att_err.psi);
/*
* Compute feedforward
*/
/* FIXME : I don't understand the /32 - should be /256 */
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
booz_stabilization_ddgain.x * booz_stab_att_ref_accel.p / 32.;
booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
booz_stabilization_ddgain.y * booz_stab_att_ref_accel.q / 32.;
booz_stabilization_att_ff_cmd[COMMAND_YAW] =
booz_stabilization_ddgain.z * booz_stab_att_ref_accel.r / 32.;
/*
* Compute feedback
*/
/* attitude error */
struct FloatQuat att_quat_float;
QUAT_FLOAT_OF_BFP(att_quat_float, booz_ahrs.ltp_to_body_quat);
struct FloatQuat att_err;
FLOAT_QUAT_INV_COMP(att_err, att_quat_float, booz_stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
if (in_flight) {
/* update integrator */
EULERS_ADD(booz_stabilization_att_sum_err, att_err);
/* update accumulator */
// EULERS_ADD(booz_stabilization_att_sum_err, err);
EULERS_BOUND_CUBE(booz_stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
INT_EULERS_ZERO(booz_stabilization_att_sum_err);
/* reset accumulator */
FLOAT_EULERS_ZERO(booz_stabilization_att_sum_err);
}
/* compute rate error */
const struct Int32Rates rate_ref_scaled = {
booz_stabilization_rate_ref.x >> (RATE_REF_RES - INT32_RATE_FRAC),
booz_stabilization_rate_ref.y >> (RATE_REF_RES - INT32_RATE_FRAC),
booz_stabilization_rate_ref.z >> (RATE_REF_RES - INT32_RATE_FRAC) };
struct Int32Rates rate_err;
RATES_DIFF(rate_err, booz_ahrs.body_rate, rate_ref_scaled);
/* rate error */
struct FloatRates rate_float;
RATES_FLOAT_OF_BFP(rate_float, booz_ahrs.body_rate);
struct FloatRates rate_err;
RATES_DIFF(rate_err, rate_float, booz_stab_att_ref_rate);
/* compute PID loop */
/* PID */
booz_stabilization_att_err_cmd[COMMAND_ROLL] =
booz_stabilization_pgain.x * att_err.phi +
booz_stabilization_dgain.x * rate_err.p +
((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi) >> 10);
booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_att_err_cmd[COMMAND_ROLL] +
((booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x) >> 5);
booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_cmd[COMMAND_ROLL] >> 16;
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
-2. * booz_stabilization_pgain.x * att_err.qx +
booz_stabilization_dgain.x * rate_err.p +
booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi / 1024.;
booz_stabilization_att_err_cmd[COMMAND_PITCH] =
booz_stabilization_pgain.y * att_err.theta +
booz_stabilization_dgain.y * rate_err.q +
((booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta) >> 10);
booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_att_err_cmd[COMMAND_PITCH] +
((booz_stabilization_ddgain.y * booz_stabilization_accel_ref.y) >> 5);
booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_cmd[COMMAND_PITCH] >> 16;
booz_stabilization_att_err_cmd[COMMAND_YAW] =
booz_stabilization_pgain.z * att_err.psi +
booz_stabilization_dgain.z * rate_err.r +
((booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi) >> 10);
booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_att_err_cmd[COMMAND_YAW] +
((booz_stabilization_ddgain.z * booz_stabilization_accel_ref.z) >> 5);
booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_cmd[COMMAND_YAW] >> 16;
booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
-2. * booz_stabilization_pgain.y * att_err.qy +
booz_stabilization_dgain.y * rate_err.q +
booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta / 1024.;
booz_stabilization_att_fb_cmd[COMMAND_YAW] =
-2. * booz_stabilization_pgain.z * att_err.qz +
booz_stabilization_dgain.z * rate_err.r +
booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi / 1024.;
booz_stabilization_cmd[COMMAND_ROLL] =
(booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
booz_stabilization_cmd[COMMAND_PITCH] =
(booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
booz_stabilization_cmd[COMMAND_YAW] =
(booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW])/16.;
}
/*
generation of a saturated linear second order reference trajectory
roll/pitch
omega : 1100 deg s-1
zeta : 0.85
max rotational accel : 128 rad.s-2 ( ~7300 deg s-2 )
max rotational speed : 8 rad/s ( ~ 458 deg s-1 )
yaw
omega : 500 deg s-1
zeta : 0.85
max rotational accel : 32 rad.s-2 ( ~1833 deg s-2 )
max rotational speed : 4 rad/s ( ~ 230 deg s-1 )
representation
accel : 20.12
speed : 16.16
angle : 12.20
*/
#define USE_REF 1
static inline void booz_stabilization_update_ref(void) {
#ifdef USE_REF
BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_UPDATE();
#else
EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp);
INT_VECT3_ZERO(booz_stabilization_rate_ref);
INT_VECT3_ZERO(booz_stabilization_accel_ref);
#endif
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref.h
+37
View File
@@ -0,0 +1,37 @@
#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_H
#define BOOZ_STABILIZATION_ATTITUDE_REF_H
#define SATURATE_SPEED_TRIM_ACCEL() { \
if (booz_stab_att_ref_rate.p >= REF_RATE_MAX_P) { \
booz_stab_att_ref_rate.p = REF_RATE_MAX_P; \
if (booz_stab_att_ref_accel.p > 0) \
booz_stab_att_ref_accel.p = 0; \
} \
else if (booz_stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \
booz_stab_att_ref_rate.p = -REF_RATE_MAX_P; \
if (booz_stab_att_ref_accel.p < 0) \
booz_stab_att_ref_accel.p = 0; \
} \
if (booz_stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \
booz_stab_att_ref_rate.q = REF_RATE_MAX_Q; \
if (booz_stab_att_ref_accel.q > 0) \
booz_stab_att_ref_accel.q = 0; \
} \
else if (booz_stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \
booz_stab_att_ref_rate.q = -REF_RATE_MAX_Q; \
if (booz_stab_att_ref_accel.q < 0) \
booz_stab_att_ref_accel.q = 0; \
} \
if (booz_stab_att_ref_rate.r >= REF_RATE_MAX_R) { \
booz_stab_att_ref_rate.r = REF_RATE_MAX_R; \
if (booz_stab_att_ref_accel.r > 0) \
booz_stab_att_ref_accel.r = 0; \
} \
else if (booz_stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \
booz_stab_att_ref_rate.r = -REF_RATE_MAX_R; \
if (booz_stab_att_ref_accel.r < 0) \
booz_stab_att_ref_accel.r = 0; \
} \
}
#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c
+88
View File
@@ -0,0 +1,88 @@
#include "booz_stabilization.h"
struct FloatEulers booz_stab_att_sp_euler;
struct FloatEulers booz_stab_att_ref_euler;
struct FloatRates booz_stab_att_ref_rate;
struct FloatRates booz_stab_att_ref_accel;
void booz_stabilization_attitude_ref_init(void) {
FLOAT_EULERS_ZERO(booz_stab_att_sp_euler);
FLOAT_EULERS_ZERO(booz_stab_att_ref_euler);
FLOAT_RATES_ZERO(booz_stab_att_ref_rate);
FLOAT_RATES_ZERO(booz_stab_att_ref_accel);
}
/*
* Reference
*/
#define DT_UPDATE (1./512.)
#define REF_ACCEL_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define REF_ACCEL_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define REF_ACCEL_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT
#define REF_RATE_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P
#define REF_RATE_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q
#define REF_RATE_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R
#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
#define USE_REF 1
void booz_stabilization_attitude_ref_update(void) {
#ifdef USE_REF
/* dumb integrate reference attitude */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, booz_stab_att_ref_rate, DT_UPDATE);
struct FloatEulers delta_angle;
EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
EULERS_ADD(booz_stab_att_ref_euler, delta_angle );
FLOAT_ANGLE_NORMALIZE(booz_stab_att_ref_euler.psi);
/* integrate reference rotational speeds */
struct FloatRates delta_accel;
RATES_SMUL(delta_accel, booz_stab_att_ref_accel, DT_UPDATE);
RATES_ADD(booz_stab_att_ref_rate, delta_accel);
/* compute reference attitude error */
struct FloatEulers ref_err;
EULERS_DIFF(ref_err, booz_stab_att_ref_euler, booz_stab_att_sp_euler);
/* wrap it in the shortest direction */
FLOAT_ANGLE_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
booz_stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*booz_stab_att_ref_rate.p - OMEGA_P*OMEGA_P*ref_err.phi;
booz_stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*booz_stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta;
booz_stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*booz_stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi;
/* saturate acceleration */
const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \
RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp);
FLOAT_RATES_ZERO(booz_stabilization_rate_ref);
FLOAT_RATES_ZERO(booz_stabilization_accel_ref);
#endif /* USE_REF */
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h
+77
View File
@@ -0,0 +1,77 @@
/*
* $Id: booz_stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#define BOOZ_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#include "booz_radio_control.h"
#include "math/pprz_algebra_float.h"
#include "stabilization/booz_stabilization_attitude_ref_float.h"
/*
* Radio Control
*/
#define SP_MAX_PHI BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI
#define SP_MAX_THETA BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA
#define SP_MAX_R BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R
#define RC_UPDATE_FREQ 40.
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_CONTROL_YAW] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
#define BOOZ_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
\
_sp.phi = \
(-radio_control.values[RADIO_CONTROL_ROLL] * SP_MAX_PHI / MAX_PPRZ); \
_sp.theta = \
( radio_control.values[RADIO_CONTROL_PITCH] * SP_MAX_THETA / MAX_PPRZ); \
if (_inflight) { \
if (YAW_DEADBAND_EXCEEDED()) { \
_sp.psi += \
(-radio_control.values[RADIO_CONTROL_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \
FLOAT_ANGLE_NORMALIZE(_sp.psi); \
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi); \
} \
}
#define BOOZ_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
struct FloatEulers add_sp_float; \
EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \
EULERS_ADD(booz_stabilization_att_sp,add_sp_float); \
FLOAT_ANGLE_NORMALIZE(booz_stabilization_att_sp.psi); \
}
#endif /* BOOZ2_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c
+130
View File
@@ -0,0 +1,130 @@
/*
* $Id: booz_stabilization_attitude_ref_euler_int.h -1 $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
struct Int32Eulers booz_stab_att_sp_euler;
struct Int32Eulers booz_stab_att_ref_euler;
struct Int32Rates booz_stab_att_ref_rate;
struct Int32Rates booz_stab_att_ref_accel;
void booz_stabilization_attitude_ref_init(void) {
INT_EULERS_ZERO(booz_stab_att_sp_euler);
INT_EULERS_ZERO(booz_stab_att_ref_euler);
INT_RATES_ZERO( booz_stab_att_ref_rate);
INT_RATES_ZERO( booz_stab_att_ref_accel);
}
#define F_UPDATE_RES 9
#define F_UPDATE (1<<F_UPDATE_RES)
#define REF_ACCEL_MAX_P BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC)
#define REF_ACCEL_MAX_Q BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC)
#define REF_ACCEL_MAX_R BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC)
#define REF_RATE_MAX_P BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P, REF_RATE_FRAC)
#define REF_RATE_MAX_Q BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC)
#define REF_RATE_MAX_R BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC)
#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
#define ZETA_OMEGA_P_RES 10
#define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
#define OMEGA_2_P_RES 7
#define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_OMEGA_Q_RES 10
#define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
#define OMEGA_2_Q_RES 7
#define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
#define ZETA_OMEGA_R_RES 10
#define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
#define OMEGA_2_R_RES 7
#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
#define USE_REF 1
void booz_stabilization_attitude_ref_update(void) {
#ifdef USE_REF
/* dumb integrate reference attitude */
const struct Int32Eulers d_angle = {
booz_stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
booz_stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
booz_stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)};
EULERS_ADD(booz_stab_att_ref_euler, d_angle );
ANGLE_REF_NORMALIZE(booz_stab_att_ref_euler.psi);
/* integrate reference rotational speeds */
const struct Int32Rates d_rate = {
booz_stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
booz_stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
booz_stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)};
RATES_ADD(booz_stab_att_ref_rate, d_rate);
/* compute reference attitude error */
struct Int32Eulers ref_err;
EULERS_DIFF(ref_err, booz_stab_att_ref_euler, booz_stab_att_sp_euler);
/* wrap it in the shortest direction */
ANGLE_REF_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
const struct Int32Rates accel_rate = {
((int32_t)(-2.*ZETA_OMEGA_P) * (booz_stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_P_RES),
((int32_t)(-2.*ZETA_OMEGA_Q) * (booz_stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_Q_RES),
((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_R_RES) };
const struct Int32Rates accel_angle = {
((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
RATES_SUM(booz_stab_att_ref_accel, accel_rate, accel_angle);
/* saturate acceleration */
const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp);
INT_RATES_ZERO(booz_stabilization_rate_ref);
INT_RATES_ZERO(booz_stabilization_accel_ref);
#endif /* USE_REF */
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h
+94
View File
@@ -0,0 +1,94 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_EULER_INT_H
#define BOOZ_STABILIZATION_ATTITUDE_REF_EULER_INT_H
#include "stabilization/booz_stabilization_attitude_ref_int.h"
#include "booz_radio_control.h"
#define REF_ACCEL_FRAC 12
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define ANGLE_REF_NORMALIZE(_a) { \
while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
}
/*
* Radio Control
*/
#define SP_MAX_PHI ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI)
#define SP_MAX_THETA ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA)
#define SP_MAX_R ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R)
#define RC_UPDATE_FREQ 40
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_CONTROL_YAW] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
#define BOOZ_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
\
_sp.phi = \
((int32_t)-radio_control.values[RADIO_CONTROL_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
_sp.theta = \
((int32_t) radio_control.values[RADIO_CONTROL_PITCH] * (int32_t)SP_MAX_THETA / MAX_PPRZ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
if (_inflight) { \
if (YAW_DEADBAND_EXCEEDED()) { \
_sp.psi += \
((int32_t)-radio_control.values[RADIO_CONTROL_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
ANGLE_REF_NORMALIZE(_sp.psi); \
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = (booz_ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \
} \
}
#define BOOZ_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
EULERS_ADD(booz_stab_att_sp_euler,_add_sp); \
ANGLE_REF_NORMALIZE(booz_stab_att_sp_euler.psi); \
}
#define BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \
_sp.psi = booz_ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
booz_stab_att_ref_euler.psi = _sp.psi; \
booz_stab_att_ref_rate.r = 0; \
}
#endif /* BOOZ2_STABILIZATION_ATTITUDE_REF_EULER_INT_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
#define BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
extern struct FloatEulers booz_stab_att_sp_euler;
extern struct FloatQuat booz_stab_att_sp_quat;
extern struct FloatEulers booz_stab_att_ref_euler;
extern struct FloatQuat booz_stab_att_ref_quat;
extern struct FloatRates booz_stab_att_ref_rate;
extern struct FloatRates booz_stab_att_ref_accel;
#endif /* BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
+11
View File
@@ -0,0 +1,11 @@
#ifndef BOOZ_STABILISATION_ATTITUDE_REF_INT_H
#define BOOZ_STABILISATION_ATTITUDE_REF_INT_H
extern struct Int32Eulers booz_stab_att_sp_euler;
extern struct Int32Quat booz_stab_att_sp_quat;
extern struct Int32Eulers booz_stab_att_ref_euler;
extern struct Int32Quat booz_stab_att_ref_quat;
extern struct Int32Rates booz_stab_att_ref_rate;
extern struct Int32Rates booz_stab_att_ref_accel;
#endif /* BOOZ_STABILISATION_ATTITUDE_REF_INT_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
+88
View File
@@ -0,0 +1,88 @@
/*
* $Id: booz_stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
struct FloatEulers booz_stab_att_sp_euler;
struct FloatQuat booz_stab_att_sp_quat;
struct FloatEulers booz_stab_att_ref_euler;
struct FloatQuat booz_stab_att_ref_quat;
struct FloatRates booz_stab_att_ref_rate;
struct FloatRates booz_stab_att_ref_accel;
void booz_stabilization_attitude_ref_init(void) {
FLOAT_EULERS_ZERO(booz_stab_att_sp_euler);
FLOAT_QUAT_ZERO( booz_stab_att_sp_quat);
FLOAT_EULERS_ZERO(booz_stab_att_ref_euler);
FLOAT_QUAT_ZERO( booz_stab_att_ref_quat);
FLOAT_RATES_ZERO( booz_stab_att_ref_rate);
FLOAT_RATES_ZERO( booz_stab_att_ref_accel);
}
/*
* Reference
*/
#define DT_UPDATE (1./512.)
#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
void booz_stabilization_attitude_ref_update(void) {
/* integrate reference attitude */
struct FloatQuat qdot;
FLOAT_QUAT_DERIVATIVE(qdot, booz_stab_att_ref_rate, booz_stab_att_ref_quat);
QUAT_SMUL(qdot, qdot, DT_UPDATE);
QUAT_ADD(booz_stab_att_ref_quat, qdot);
FLOAT_QUAT_NORMALISE(booz_stab_att_ref_quat);
/* integrate reference rotational speeds */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, booz_stab_att_ref_accel, DT_UPDATE);
RATES_ADD(booz_stab_att_ref_rate, delta_rate);
/* compute reference angular accelerations */
struct FloatQuat err;
/* compute reference attitude error */
FLOAT_QUAT_INV_COMP(err, booz_stab_att_sp_quat, booz_stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(err);
/* propagate the 2nd order linear model */
booz_stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*booz_stab_att_ref_rate.p - OMEGA_P*OMEGA_P*err.qx;
booz_stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_Q*booz_stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*err.qy;
booz_stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_R*booz_stab_att_ref_rate.r - OMEGA_R*OMEGA_R*err.qz;
/* compute ref_euler */
FLOAT_EULERS_OF_QUAT(booz_stab_att_ref_euler, booz_stab_att_ref_quat);
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
+78
View File
@@ -0,0 +1,78 @@
/*
* $Id: booz_stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H
#define BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H
#include "booz_stabilization.h"
#include "booz_radio_control.h"
#include "math/pprz_algebra_float.h"
#include "stabilization/booz_stabilization_attitude_ref_float.h"
#define RC_UPDATE_FREQ 40.
#define ROLL_COEF (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ)
#define PITCH_COEF ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ)
#define YAW_COEF (- BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ)
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_CONTROL_YAW] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
#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); \
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.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);*/ \
}
#define BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \
_sp.psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi); \
booz_stab_att_ref_euler.psi = _sp.psi; \
booz_stab_att_ref_rate.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_EULERS_OF_QUAT(sp_euler321, sp_quat);*/ \
}
#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h
-201
View File
@@ -1,201 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_H
#define BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_H
#include "booz_radio_control.h"
extern struct Int32Eulers booz_stabilization_att_sp;
extern struct Int32Eulers booz_stabilization_att_ref;
extern struct Int32Vect3 booz_stabilization_rate_ref;
extern struct Int32Vect3 booz_stabilization_accel_ref;
#define F_UPDATE_RES 9
#define F_UPDATE (1<<F_UPDATE_RES)
#define ACCEL_REF_RES 12
#define ACCEL_REF_MAX_P BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT, ACCEL_REF_RES)
#define ACCEL_REF_MAX_Q BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT, ACCEL_REF_RES)
#define ACCEL_REF_MAX_R BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT, ACCEL_REF_RES)
#define RATE_REF_RES 16
#define RATE_REF_MAX_P BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P, RATE_REF_RES)
#define RATE_REF_MAX_Q BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q, ACCEL_REF_RES)
#define RATE_REF_MAX_R BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R, ACCEL_REF_RES)
#define ANGLE_REF_RES 20
#define PI_ANGLE_REF BFP_OF_REAL(3.1415926535897932384626433832795029, ANGLE_REF_RES)
#define TWO_PI_ANGLE_REF BFP_OF_REAL(2.*3.1415926535897932384626433832795029, ANGLE_REF_RES)
#define ANGLE_REF_NORMALIZE(_a) { \
while (_a > PI_ANGLE_REF) _a -= TWO_PI_ANGLE_REF; \
while (_a < -PI_ANGLE_REF) _a += TWO_PI_ANGLE_REF; \
}
#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
#define ZETA_OMEGA_P_RES 10
#define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
#define OMEGA_2_P_RES 7
#define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_OMEGA_Q_RES 10
#define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
#define OMEGA_2_Q_RES 7
#define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
#define ZETA_OMEGA_R_RES 10
#define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
#define OMEGA_2_R_RES 7
#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
#define BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_UPDATE() { \
\
/* dumb integrate reference attitude */ \
const struct Int32Eulers d_angle = { \
booz_stabilization_rate_ref.x >> ( F_UPDATE_RES + RATE_REF_RES - ANGLE_REF_RES), \
booz_stabilization_rate_ref.y >> ( F_UPDATE_RES + RATE_REF_RES - ANGLE_REF_RES), \
booz_stabilization_rate_ref.z >> ( F_UPDATE_RES + RATE_REF_RES - ANGLE_REF_RES)}; \
EULERS_ADD(booz_stabilization_att_ref, d_angle ); \
ANGLE_REF_NORMALIZE(booz_stabilization_att_ref.psi); \
\
/* integrate reference rotational speeds */ \
const struct Int32Vect3 d_rate = { \
booz_stabilization_accel_ref.x >> ( F_UPDATE_RES + ACCEL_REF_RES - RATE_REF_RES), \
booz_stabilization_accel_ref.y >> ( F_UPDATE_RES + ACCEL_REF_RES - RATE_REF_RES), \
booz_stabilization_accel_ref.z >> ( F_UPDATE_RES + ACCEL_REF_RES - RATE_REF_RES)}; \
VECT3_ADD(booz_stabilization_rate_ref, d_rate); \
\
/* compute reference attitude error */ \
struct Int32Eulers ref_err; \
EULERS_DIFF(ref_err, booz_stabilization_att_ref, booz_stabilization_att_sp); \
/* wrap it in the shortest direction */ \
ANGLE_REF_NORMALIZE(ref_err.psi); \
\
/* compute reference angular accelerations */ \
const struct Int32Vect3 accel_rate = { \
((int32_t)(-2.*ZETA_OMEGA_P) * (booz_stabilization_rate_ref.x >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_P_RES), \
((int32_t)(-2.*ZETA_OMEGA_Q) * (booz_stabilization_rate_ref.y >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_Q_RES), \
((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stabilization_rate_ref.z >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_R_RES) }; \
\
const struct Int32Vect3 accel_angle = { \
((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_P_RES), \
((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_Q_RES), \
((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_R_RES) }; \
\
VECT3_SUM(booz_stabilization_accel_ref, accel_rate, accel_angle); \
\
/* saturate acceleration */ \
const struct Int32Vect3 MIN_ACCEL = { -ACCEL_REF_MAX_P, -ACCEL_REF_MAX_Q, -ACCEL_REF_MAX_R }; \
const struct Int32Vect3 MAX_ACCEL = { ACCEL_REF_MAX_P, ACCEL_REF_MAX_Q, ACCEL_REF_MAX_R }; \
VECT3_BOUND_BOX(booz_stabilization_accel_ref, MIN_ACCEL, MAX_ACCEL); \
\
/* saturate speed and trim accel accordingly */ \
if (booz_stabilization_rate_ref.x >= RATE_REF_MAX_P) { \
booz_stabilization_rate_ref.x = RATE_REF_MAX_P; \
if (booz_stabilization_accel_ref.x > 0) \
booz_stabilization_accel_ref.x = 0; \
} \
else if (booz_stabilization_rate_ref.x <= -RATE_REF_MAX_P) { \
booz_stabilization_rate_ref.x = -RATE_REF_MAX_P; \
if (booz_stabilization_accel_ref.x < 0) \
booz_stabilization_accel_ref.x = 0; \
} \
if (booz_stabilization_rate_ref.y >= RATE_REF_MAX_Q) { \
booz_stabilization_rate_ref.y = RATE_REF_MAX_Q; \
if (booz_stabilization_accel_ref.y > 0) \
booz_stabilization_accel_ref.y = 0; \
} \
else if (booz_stabilization_rate_ref.y <= -RATE_REF_MAX_Q) { \
booz_stabilization_rate_ref.y = -RATE_REF_MAX_Q; \
if (booz_stabilization_accel_ref.y < 0) \
booz_stabilization_accel_ref.y = 0; \
} \
if (booz_stabilization_rate_ref.z >= RATE_REF_MAX_R) { \
booz_stabilization_rate_ref.z = RATE_REF_MAX_R; \
if (booz_stabilization_accel_ref.z > 0) \
booz_stabilization_accel_ref.z = 0; \
} \
else if (booz_stabilization_rate_ref.z <= -RATE_REF_MAX_R) { \
booz_stabilization_rate_ref.z = -RATE_REF_MAX_R; \
if (booz_stabilization_accel_ref.z < 0) \
booz_stabilization_accel_ref.z = 0; \
} \
\
}
#define RC_UPDATE_FREQ 40
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_CONTROL_YAW] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
#define BOOZ_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
\
_sp.phi = \
((int32_t)-radio_control.values[RADIO_CONTROL_ROLL] * BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ) \
<< (ANGLE_REF_RES - INT32_ANGLE_FRAC); \
_sp.theta = \
((int32_t) radio_control.values[RADIO_CONTROL_PITCH] * BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) \
<< (ANGLE_REF_RES - INT32_ANGLE_FRAC); \
if (_inflight) { \
if (YAW_DEADBAND_EXCEEDED()) { \
_sp.psi += \
((int32_t)-radio_control.values[RADIO_CONTROL_YAW] * BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \
<< (ANGLE_REF_RES - INT32_ANGLE_FRAC); \
ANGLE_REF_NORMALIZE(_sp.psi); \
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = (booz_ahrs.ltp_to_body_euler.psi << (ANGLE_REF_RES - INT32_ANGLE_FRAC)); \
} \
}
#define BOOZ_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
EULERS_ADD(booz_stabilization_att_sp,_add_sp); \
ANGLE_REF_NORMALIZE(booz_stabilization_att_sp.psi); \
}
#define BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \
_sp.psi = booz_ahrs.ltp_to_body_euler.psi << (ANGLE_REF_RES - INT32_ANGLE_FRAC); \
booz_stabilization_att_ref.psi = _sp.psi; \
booz_stabilization_rate_ref.z = 0; \
}
#endif /* BOOZ2_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_H */
sw/airborne/math/pprz_algebra.h
+30 -8
View File
@@ -215,6 +215,13 @@
}
/* _vo = _vi * _s */
#define EULERS_SMUL(_eo, _ei, _s) { \
(_eo).phi = (_ei).phi * (_s); \
(_eo).theta = (_ei).theta * (_s); \
(_eo).psi = (_ei).psi * (_s); \
}
/* _vo = _vi / _s */
#define EULERS_SDIV(_eo, _ei, _s) { \
(_eo).phi = (_ei).phi / (_s); \
@@ -297,6 +304,21 @@
}
/* _v = Bound(_v, _min, _max) */
#define RATES_BOUND_CUBE(_v, _min, _max) { \
(_v).p = (_v).p < _min ? _min : (_v).p > _max ? _max : (_v).p; \
(_v).q = (_v).q < _min ? _min : (_v).q > _max ? _max : (_v).q; \
(_v).r = (_v).r < _min ? _min : (_v).r > _max ? _max : (_v).r; \
}
#define RATES_BOUND_BOX(_v, _v_min, _v_max) { \
if ((_v).p > (_v_max.p)) (_v).p = (_v_max.p); else if ((_v).p < (_v_min.p)) (_v).p = (_v_min.p); \
if ((_v).q > (_v_max.q)) (_v).q = (_v_max.q); else if ((_v).q < (_v_min.q)) (_v).q = (_v_min.q); \
if ((_v).r > (_v_max.r)) (_v).r = (_v_max.r); else if ((_v).r < (_v_min.r)) (_v).r = (_v_min.r); \
}
/*
* 3x3 matrices
*/
@@ -476,16 +498,16 @@
(_qi).qz = QUAT1_BFP_OF_REAL((_qf).qz); \
}
#define RATES_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).phi = RATE_FLOAT_OF_BFP((_ei).phi); \
(_ef).theta = RATE_FLOAT_OF_BFP((_ei).theta); \
(_ef).psi = RATE_FLOAT_OF_BFP((_ei).psi); \
#define RATES_FLOAT_OF_BFP(_rf, _ri) { \
(_rf).p = RATE_FLOAT_OF_BFP((_ri).p); \
(_rf).q = RATE_FLOAT_OF_BFP((_ri).q); \
(_rf).r = RATE_FLOAT_OF_BFP((_ri).r); \
}
#define RATES_BFP_OF_REAL(_ei, _ef) { \
(_ei).phi = RATE_BFP_OF_REAL((_ef).phi); \
(_ei).theta = RATE_BFP_OF_REAL((_ef).theta); \
(_ei).psi = RATE_BFP_OF_REAL((_ef).psi); \
#define RATES_BFP_OF_REAL(_ri, _rf) { \
(_ri).p = RATE_BFP_OF_REAL((_rf).p); \
(_ri).q = RATE_BFP_OF_REAL((_rf).q); \
(_ri).r = RATE_BFP_OF_REAL((_rf).r); \
}
sw/airborne/math/pprz_algebra_float.h
+6
View File
@@ -67,6 +67,12 @@ struct FloatRates {
float r;
};
#define FLOAT_ANGLE_NORMALIZE(_a) { \
while (_a > M_PI) _a -= (2.*M_PI); \
while (_a < -M_PI) _a += (2.*M_PI); \
}
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.) \
sw/airborne/math/pprz_algebra_int.h
+3 -3
View File
@@ -158,9 +158,9 @@ struct Int64Vect3 {
#define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
#define INT32_VECT2_NORM(n, v) { \
int32_t n2 = v.x*v.x + v.y*v.y; \
INT32_SQRT(n, n2); \
#define INT32_VECT2_NORM(n, v) { \
int32_t n2 = v.x*v.x + v.y*v.y; \
INT32_SQRT(n, n2); \
}
#define INT32_VECT2_RSHIFT(_o, _i, _r) { \
sw/simulator/nps_autopilot_booz.c
+1 -1
View File
@@ -55,7 +55,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
booz2_main_periodic();
/* 25 */
if (time < 5) {
double hover = 0.25;
//double hover = 0.2493;