diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml index ea2001e3cc..d709910960 100644 --- a/conf/airframes/booz2_a1.xml +++ b/conf/airframes/booz2_a1.xml @@ -92,9 +92,9 @@
- - - + + + @@ -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 diff --git a/conf/airframes/booz2_a2.xml b/conf/airframes/booz2_a2.xml index 2aa774b62f..f019444dda 100644 --- a/conf/airframes/booz2_a2.xml +++ b/conf/airframes/booz2_a2.xml @@ -72,21 +72,46 @@
- - - + + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + - + + + + +
@@ -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 diff --git a/conf/airframes/booz2_a3.xml b/conf/airframes/booz2_a3.xml index b63d54b2a2..ebeb6cb4b8 100644 --- a/conf/airframes/booz2_a3.xml +++ b/conf/airframes/booz2_a3.xml @@ -1,4 +1,4 @@ - + @@ -72,21 +72,46 @@
- - - + + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + - + + + + +
@@ -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 diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index af0d1d2bff..9d217986fe 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -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 diff --git a/conf/airframes/booz2_g1.xml b/conf/airframes/booz2_g1.xml index 430d50fc8d..cae8d7ea15 100644 --- a/conf/airframes/booz2_g1.xml +++ b/conf/airframes/booz2_g1.xml @@ -78,21 +78,46 @@
- - - + + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + - + + + + +
diff --git a/conf/airframes/booz2_s1.xml b/conf/airframes/booz2_s1.xml index b643ddcb64..dbba726dcc 100644 --- a/conf/airframes/booz2_s1.xml +++ b/conf/airframes/booz2_s1.xml @@ -66,8 +66,6 @@ - - @@ -89,21 +87,46 @@
- - - + + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + - + + + + +
diff --git a/conf/airframes/booz2_x1.xml b/conf/airframes/booz2_x1.xml index f6a4d1b861..b65be75483 100644 --- a/conf/airframes/booz2_x1.xml +++ b/conf/airframes/booz2_x1.xml @@ -92,21 +92,46 @@
- - - + + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + - + + + + +
diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile index 54daae1aca..2aaf19bca8 100644 --- a/conf/autopilot/booz2_autopilot.makefile +++ b/conf/autopilot/booz2_autopilot.makefile @@ -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 diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile index b85e5a14db..ed5024ca56 100644 --- a/conf/autopilot/booz2_simulator_nps.makefile +++ b/conf/autopilot/booz2_simulator_nps.makefile @@ -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 diff --git a/conf/autopilot/subsystems/booz2_radio_control_spektrum.makefile b/conf/autopilot/subsystems/booz2_radio_control_spektrum.makefile index 4b2eaf84e0..46b2327a7c 100644 --- a/conf/autopilot/subsystems/booz2_radio_control_spektrum.makefile +++ b/conf/autopilot/subsystems/booz2_radio_control_spektrum.makefile @@ -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 \ diff --git a/conf/messages.xml b/conf/messages.xml index 033c136152..5e53495d9e 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -644,30 +644,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - @@ -755,53 +731,83 @@ - - - - - - - - - - + + + + + + + + + + - - - - - - + + + + + + - - - - - - - - - - + + + + + + + + + + + + + - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -810,7 +816,6 @@ - diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index e3232c32fb..e166ba4671 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -96,7 +96,7 @@ - + diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index c0eca581d7..8e0436f365 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -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" diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.c b/sw/airborne/booz/guidance/booz2_guidance_h.c index 53178e0d9c..09e3cde37c 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_h.c +++ b/sw/airborne/booz/guidance/booz2_guidance_h.c @@ -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); diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude.h index 77e7a129c3..e14ac9a45d 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude.h +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude.h @@ -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) { \ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c new file mode 100644 index 0000000000..dac5236587 --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c @@ -0,0 +1,151 @@ +/* + * $Id: booz_stabilization_attitude_euler.c 3795 2009-07-24 23:43:02Z poine $ + * + * Copyright (C) 2008-2009 Antoine Drouin + * + * 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.; + +} + + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c similarity index 57% rename from sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c rename to sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c index 2249ba3985..f0eab4795a 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c @@ -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 - - -} - - diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h new file mode 100644 index 0000000000..21fa6b0437 --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h @@ -0,0 +1,41 @@ +/* + * $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $ + * + * Copyright (C) 2008-2009 Antoine Drouin + * + * 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 */ + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h new file mode 100644 index 0000000000..4d77454c1a --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h @@ -0,0 +1,41 @@ +/* + * $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $ + * + * Copyright (C) 2008-2009 Antoine Drouin + * + * 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 */ + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c index ad9d0742df..7ca24a0488 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c @@ -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 - - -} - diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref.h new file mode 100644 index 0000000000..3df59d5d0f --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref.h @@ -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 */ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c new file mode 100644 index 0000000000..dcbb6b22cd --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c @@ -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 */ + + +} + + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h new file mode 100644 index 0000000000..b808b3c920 --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h @@ -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 + * + * 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 */ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c new file mode 100644 index 0000000000..ed3b6d9787 --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c @@ -0,0 +1,130 @@ +/* + * $Id: booz_stabilization_attitude_ref_euler_int.h -1 $ + * + * Copyright (C) 2008-2009 Antoine Drouin + * + * 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 + 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 */ + +} + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h new file mode 100644 index 0000000000..58ea1f8cc7 --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h @@ -0,0 +1,94 @@ +/* + * $Id$ + * + * Copyright (C) 2008-2009 Antoine Drouin + * + * 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 */ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h new file mode 100644 index 0000000000..9e41d875a9 --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h @@ -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 */ + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h new file mode 100644 index 0000000000..b9bd4dbdc6 --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h @@ -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 */ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c new file mode 100644 index 0000000000..e0cbff9f5c --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c @@ -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 + * + * 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); + +} + + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h new file mode 100644 index 0000000000..2a542fc83d --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h @@ -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 + * + * 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 */ + + diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h deleted file mode 100644 index 051163ffd0..0000000000 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h +++ /dev/null @@ -1,201 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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< 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 */ diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index 38ca8765fe..b2486b4a22 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -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); \ } diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 1d2b558eb4..326fd1d545 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -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.) \ diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 500205b927..e79a962969 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -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) { \ diff --git a/sw/simulator/nps_autopilot_booz.c b/sw/simulator/nps_autopilot_booz.c index 580efcf2e6..44a36d3fea 100644 --- a/sw/simulator/nps_autopilot_booz.c +++ b/sw/simulator/nps_autopilot_booz.c @@ -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;