diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index 92ed1bc162..e9491e181a 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -185,7 +185,6 @@
-
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index a37d1d6f2c..671309a1a6 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -192,7 +192,6 @@
- diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile index e62c41b687..70d8683f79 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile @@ -1,12 +1,3 @@ -STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_INT -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\" -STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c -STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c -STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c +$(warning Warning: The stabilization euler subsystem has been renamed, please replace with in your airframe file.) -ap.CFLAGS += $(STAB_ATT_CFLAGS) -ap.srcs += $(STAB_ATT_SRCS) - -nps.CFLAGS += $(STAB_ATT_CFLAGS) -nps.srcs += $(STAB_ATT_SRCS) +include $(PAPARAZZI_SRC)/conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile index 4f3b11e76d..5733e554e5 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile @@ -1,6 +1,5 @@ STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_FLOAT -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\" -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_float.h\" +STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_euler_float.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_float.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_float.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile index ba005a992d..d79598d8eb 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile @@ -1,7 +1,6 @@ STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_FLOAT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\" -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_float.h\" +STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_float.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile new file mode 100644 index 0000000000..cefbb41959 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile @@ -0,0 +1,11 @@ +STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_INT +STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_euler_int.h\" +STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c + +ap.CFLAGS += $(STAB_ATT_CFLAGS) +ap.srcs += $(STAB_ATT_SRCS) + +nps.CFLAGS += $(STAB_ATT_CFLAGS) +nps.srcs += $(STAB_ATT_SRCS) diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile index 10e015f4a2..317172085a 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile @@ -1,7 +1,6 @@ STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_INT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" +STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_int.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_passthrough.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_passthrough.makefile index 80af925973..c746f28480 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_passthrough.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_passthrough.makefile @@ -1,6 +1,5 @@ STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_INT -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" -STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\" +STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_passthrough.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_passthrough.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c ap.CFLAGS += $(STAB_ATT_CFLAGS) diff --git a/conf/settings/control/stabilization_att_int.xml b/conf/settings/control/stabilization_att_int.xml index 7f0aa9a42f..4b3966baa3 100644 --- a/conf/settings/control/stabilization_att_int.xml +++ b/conf/settings/control/stabilization_att_int.xml @@ -4,18 +4,18 @@ - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 385581719f..fa4538cfda 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -132,8 +132,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE - stab_att_sp_euler.phi = 0; - stab_att_sp_euler.theta = 0; + stabilization_attitude_set_failsafe_setpoint(); guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; #endif diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 4d07646f7a..9248591946 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -82,19 +82,13 @@ int32_t guidance_h_again; int32_t transition_percentage; int32_t transition_theta_offset; + static void guidance_h_update_reference(void); static void guidance_h_traj_run(bool_t in_flight); static void guidance_h_hover_enter(void); static void guidance_h_nav_enter(void); static inline void transition_run(void); -#define GuidanceHSetRef(_pos, _speed, _accel) { \ - gh_set_ref(_pos, _speed, _accel); \ - VECT2_COPY(guidance_h_pos_ref, _pos); \ - VECT2_COPY(guidance_h_speed_ref, _speed); \ - VECT2_COPY(guidance_h_accel_ref, _accel); \ - } - void guidance_h_init(void) { @@ -114,6 +108,20 @@ void guidance_h_init(void) { } +static inline void reset_reference_from_current_position(void) { + + VECT2_COPY(guidance_h_pos_ref, *stateGetPositionNed_i()); + VECT2_COPY(guidance_h_speed_ref, *stateGetSpeedNed_i()); + INT_VECT2_ZERO(guidance_h_accel_ref); + gh_set_ref(guidance_h_pos_ref, guidance_h_speed_ref, guidance_h_accel_ref); + + /* reset attitude psi reference, set psi setpoint to current psi */ + // FIXME + //reset_psi_ref_from_body(); + + INT_VECT2_ZERO(guidance_h_pos_err_sum); +} + void guidance_h_mode_changed(uint8_t new_mode) { if (new_mode == guidance_h_mode) return; @@ -217,6 +225,9 @@ void guidance_h_run(bool_t in_flight) { break; case GUIDANCE_H_MODE_HOVER: + if (!in_flight) + guidance_h_hover_enter(); + guidance_h_update_reference(); /* set psi command */ @@ -228,17 +239,16 @@ void guidance_h_run(bool_t in_flight) { break; case GUIDANCE_H_MODE_NAV: - if (!in_flight) guidance_h_nav_enter(); + if (!in_flight) + guidance_h_nav_enter(); if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { - stab_att_sp_euler.phi = nav_roll; - stab_att_sp_euler.theta = nav_pitch; + struct Int32Eulers sp_euler_i; + sp_euler_i.phi = nav_roll; + sp_euler_i.theta = nav_pitch; /* FIXME: heading can't be set via attitude block yet, use current heading for now */ - stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; -#ifdef STABILIZATION_ATTITUDE_TYPE_QUAT - INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler); - INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); -#endif + sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi; + stabilization_attitude_set_from_eulers_i(&sp_euler_i); } else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); @@ -256,9 +266,9 @@ void guidance_h_run(bool_t in_flight) { default: break; } - } + static void guidance_h_update_reference(void) { /* compute reference even if usage temporarily disabled via guidance_h_use_ref */ #if GUIDANCE_H_USE_REF @@ -342,42 +352,28 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_command_body.phi += guidance_h_rc_sp.phi; guidance_h_command_body.theta += guidance_h_rc_sp.theta; - /* Set attitude setpoint in eulers and as quaternion */ - EULERS_COPY(stab_att_sp_euler, guidance_h_command_body); - -#ifdef STABILIZATION_ATTITUDE_TYPE_QUAT - INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler); - INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); -#endif /* STABILIZATION_ATTITUDE_TYPE_QUAT */ - + /* Set attitude setpoint from pseudo-eulers */ + stabilization_attitude_set_from_eulers_i(&guidance_h_command_body); } static void guidance_h_hover_enter(void) { + /* set horizontal setpoint to current position */ VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i()); + reset_reference_from_current_position(); + guidance_h_rc_sp.psi = stateGetNedToBodyEulers_i()->psi; - reset_psi_ref_from_body(); - - INT_VECT2_ZERO(guidance_h_pos_err_sum); - } static void guidance_h_nav_enter(void) { + /* horizontal position setpoint from navigation/flightplan */ INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); - struct Int32Vect2 pos,speed,zero; - INT_VECT2_ZERO(zero); - VECT2_COPY(pos, *stateGetPositionNed_i()); - VECT2_COPY(speed, *stateGetSpeedNed_i()); - GuidanceHSetRef(pos, speed, zero); - /* reset psi reference, set psi setpoint to current psi */ - reset_psi_ref_from_body(); + reset_reference_from_current_position(); + nav_heading = stateGetNedToBodyEulers_i()->psi; - - INT_VECT2_ZERO(guidance_h_pos_err_sum); - } static inline void transition_run(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 9ee532725f..09777e371b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -19,6 +19,11 @@ * Boston, MA 02111-1307, USA. */ +/** @file firmwares/rotorcraft/stabilization_attitude.h + * General attitude stabilization interface for rotorcrafts. + * The actual implementation is automatically included. + */ + #ifndef STABILIZATION_ATTITUDE_H #define STABILIZATION_ATTITUDE_H @@ -28,15 +33,9 @@ extern void stabilization_attitude_init(void); extern void stabilization_attitude_read_rc(bool_t in_flight); extern void stabilization_attitude_enter(void); +extern void stabilization_attitude_set_failsafe_setpoint(void); +extern void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler); extern void stabilization_attitude_run(bool_t in_flight); -#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h" -extern void stabilization_attitude_ref_init(void); -extern void stabilization_attitude_ref_update(void); - -#define stabilization_attitude_SetKiPhi(_val) { \ - stabilization_gains.i.x = _val; \ - stabilization_att_sum_err.phi = 0; \ - } #endif /* STABILIZATION_ATTITUDE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h similarity index 74% rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h index 18b5e521d7..76f48194e5 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h @@ -19,20 +19,20 @@ * Boston, MA 02111-1307, USA. */ -#ifndef STABILIZATION_ATTITUDE_FLOAT_H -#define STABILIZATION_ATTITUDE_FLOAT_H +/** + * @file stabilization_attitude_common_float.h + * + * Common data structures shared by euler and quaternion float implementations. + */ + + +#ifndef STABILIZATION_ATTITUDE_COMMON_FLOAT_H +#define STABILIZATION_ATTITUDE_COMMON_FLOAT_H #include "math/pprz_algebra_float.h" #include "generated/airframe.h" -#ifndef STABILIZATION_ATTITUDE_FLOAT_GAIN_NB -#define STABILIZATION_ATTITUDE_FLOAT_GAIN_NB 1 -#endif - -#ifndef STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT -#define STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT 0 -#endif struct FloatAttitudeGains { struct FloatVect3 p; @@ -46,12 +46,9 @@ struct FloatAttitudeGains { struct FloatVect3 surface_i; }; -extern struct FloatAttitudeGains stabilization_gains[]; extern struct FloatEulers stabilization_att_sum_err_eulers; extern float stabilization_att_fb_cmd[COMMANDS_NB]; extern float stabilization_att_ff_cmd[COMMANDS_NB]; -void stabilization_attitude_gain_schedule(uint8_t idx); - -#endif /* STABILIZATION_ATTITUDE_FLOAT_H */ +#endif /* STABILIZATION_ATTITUDE_COMMON_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h similarity index 68% rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h index 2a9128af05..689c5d0ffa 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h @@ -19,8 +19,14 @@ * Boston, MA 02111-1307, USA. */ -#ifndef STABILIZATION_ATTITUDE_INT_H -#define STABILIZATION_ATTITUDE_INT_H +/** + * @file stabilization_attitude_common_int.h + * + * Common data structures shared by euler and quaternion int implementations. + */ + +#ifndef STABILIZATION_ATTITUDE_COMMON_INT_H +#define STABILIZATION_ATTITUDE_COMMON_INT_H #include "math/pprz_algebra_int.h" @@ -39,4 +45,12 @@ extern struct Int32Eulers stabilization_att_sum_err; extern int32_t stabilization_att_fb_cmd[COMMANDS_NB]; extern int32_t stabilization_att_ff_cmd[COMMANDS_NB]; -#endif /* STABILIZATION_ATTITUDE_INT_H */ +// common so it can be used for downlink/debug +extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC + +#define stabilization_attitude_common_int_SetKiPhi(_val) { \ + stabilization_gains.i.x = _val; \ + stabilization_att_sum_err.phi = 0; \ + } + +#endif /* STABILIZATION_ATTITUDE_COMMON_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index b8f33b7103..b2f9f24178 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -20,6 +20,7 @@ */ #include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "math/pprz_algebra_float.h" #include "state.h" @@ -40,24 +41,24 @@ void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); VECT3_ASSIGN(stabilization_gains.p, - STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN, - STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN, - STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN); + STABILIZATION_ATTITUDE_PHI_PGAIN, + STABILIZATION_ATTITUDE_THETA_PGAIN, + STABILIZATION_ATTITUDE_PSI_PGAIN); VECT3_ASSIGN(stabilization_gains.d, - STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN, - STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN, - STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN); + STABILIZATION_ATTITUDE_PHI_DGAIN, + STABILIZATION_ATTITUDE_THETA_DGAIN, + STABILIZATION_ATTITUDE_PSI_DGAIN); VECT3_ASSIGN(stabilization_gains.i, - STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN, - STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN, - STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN); + STABILIZATION_ATTITUDE_PHI_IGAIN, + STABILIZATION_ATTITUDE_THETA_IGAIN, + STABILIZATION_ATTITUDE_PSI_IGAIN); VECT3_ASSIGN(stabilization_gains.dd, - STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN, - STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN, - STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN); + STABILIZATION_ATTITUDE_PHI_DDGAIN, + STABILIZATION_ATTITUDE_THETA_DDGAIN, + STABILIZATION_ATTITUDE_PSI_DDGAIN); FLOAT_EULERS_ZERO( stabilization_att_sum_err ); @@ -65,17 +66,28 @@ void stabilization_attitude_init(void) { void stabilization_attitude_read_rc(bool_t in_flight) { - stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); - } void stabilization_attitude_enter(void) { - STABILIZATION_ATTITUDE_FLOAT_RESET_PSI_REF( stab_att_sp_euler ); - FLOAT_EULERS_ZERO( stabilization_att_sum_err ); + /* reset psi setpoint to current psi angle */ + stab_att_sp_euler.psi = stabilization_attitude_get_heading_f(); + stabilization_attitude_ref_enter(); + + FLOAT_EULERS_ZERO(stabilization_att_sum_err); +} + +void stabilization_attitude_set_failsafe_setpoint(void) { + stab_att_sp_euler.phi = 0.0; + stab_att_sp_euler.theta = 0.0; + stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi; +} + +void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler); } @@ -95,7 +107,7 @@ void stabilization_attitude_run(bool_t in_flight) { /* Compute feedback */ /* attitude error */ - struct FloatEulers att_float* = stateGetNedToBodyEulers_f(); + struct FloatEulers *att_float = stateGetNedToBodyEulers_f(); struct FloatEulers att_err; EULERS_DIFF(att_err, stab_att_ref_euler, *att_float); FLOAT_ANGLE_NORMALIZE(att_err.psi); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h new file mode 100644 index 0000000000..9b72570412 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h @@ -0,0 +1,37 @@ +/* + * 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. + */ + +/** + * @file stabilization_attitude_euler_float.h + * + * Rotorcraft attitude stabilization in euler float version. + */ + +#ifndef STABILIZATION_ATTITUDE_EULER_FLOAT_H +#define STABILIZATION_ATTITUDE_EULER_FLOAT_H + +#include "math/pprz_algebra_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h" + +extern struct FloatAttitudeGains stabilization_gains; + +#endif /* STABILIZATION_ATTITUDE_EULER_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index c204263705..12effe492a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -47,6 +47,13 @@ struct Int32Eulers stabilization_att_sum_err; int32_t stabilization_att_fb_cmd[COMMANDS_NB]; int32_t stabilization_att_ff_cmd[COMMANDS_NB]; +static inline void reset_psi_ref_from_body(void) { + //sp has been set from body using stabilization_attitude_get_yaw_i, use that value + stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; +} + void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); @@ -79,18 +86,23 @@ void stabilization_attitude_init(void) { void stabilization_attitude_read_rc(bool_t in_flight) { - stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight); - } - void stabilization_attitude_enter(void) { - stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; reset_psi_ref_from_body(); INT_EULERS_ZERO( stabilization_att_sum_err ); +} +void stabilization_attitude_set_failsafe_setpoint(void) { + stab_att_sp_euler.phi = 0; + stab_att_sp_euler.theta = 0; + stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; +} + +void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { + memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); } @@ -101,7 +113,6 @@ void stabilization_attitude_enter(void) { void stabilization_attitude_run(bool_t in_flight) { - /* update reference */ stabilization_attitude_ref_update(); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h new file mode 100644 index 0000000000..9488c542de --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h @@ -0,0 +1,28 @@ +/* + * 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 STABILIZATION_ATTITUDE_EULER_INT_H +#define STABILIZATION_ATTITUDE_EULER_INT_H + +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h" + +#endif /* STABILIZATION_ATTITUDE_EULER_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c index 7142c44803..9bda1b8820 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c @@ -32,27 +32,15 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "paparazzi.h" -#include "subsystems/radio_control.h" #include "generated/airframe.h" #define TRAJ_MAX_BANK (int32_t)ANGLE_BFP_OF_REAL(GUIDANCE_H_MAX_BANK) struct Int32Eulers stab_att_sp_euler; -//STUB -struct Int32Eulers stabilization_att_sum_err; -struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC -struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC -struct Int32Rates stab_att_ref_accel; ///< with #REF_ACCEL_FRAC -int32_t stabilization_att_fb_cmd[COMMANDS_NB]; -int32_t stabilization_att_ff_cmd[COMMANDS_NB]; void stabilization_attitude_init(void) { - INT_EULERS_ZERO(stabilization_att_sum_err); INT_EULERS_ZERO(stab_att_sp_euler); - INT_EULERS_ZERO(stab_att_ref_euler); - INT_RATES_ZERO(stab_att_ref_rate); - INT_RATES_ZERO(stab_att_ref_accel); } @@ -67,13 +55,14 @@ void stabilization_attitude_enter(void) { } void stabilization_attitude_run(bool_t in_flight __attribute__ ((unused))) { - /* For roll an pitch we pass truough the desired angles as stabilization command */ - EULERS_SMUL(stab_att_ref_euler, stab_att_sp_euler, MAX_PPRZ/TRAJ_MAX_BANK); - stabilization_cmd[COMMAND_ROLL] = stab_att_ref_euler.phi; - stabilization_cmd[COMMAND_PITCH] = stab_att_ref_euler.theta; + + /* For roll and pitch we pass trough the desired angles as stabilization command */ + const int32_t angle2cmd = (MAX_PPRZ/TRAJ_MAX_BANK); + stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd; + stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd; //TODO: Fix yaw with PID controller - int32_t yaw_error = stateGetNedToBodyEulers_i()->psi-stab_att_sp_euler.psi; + int32_t yaw_error = stateGetNedToBodyEulers_i()->psi - stab_att_sp_euler.psi; INT32_ANGLE_NORMALIZE(yaw_error); // stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI; @@ -83,10 +72,3 @@ void stabilization_attitude_run(bool_t in_flight __attribute__ ((unused))) { BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); } -void stabilization_attitude_ref_init(void) { - -} - -void stabilization_attitude_ref_update(void) { - -} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h new file mode 100644 index 0000000000..72031365c0 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h @@ -0,0 +1,33 @@ +/* + * 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 STABILIZATION_ATTITUDE_PASSTHROUGH_H +#define STABILIZATION_ATTITUDE_PASSTHROUGH_H + +#include "math/pprz_algebra_int.h" + +#include "generated/airframe.h" + + +extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC + + +#endif /* STABILIZATION_ATTITUDE_PASSTHROUGH_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 392a6acf7e..e546692a87 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -24,16 +24,15 @@ */ #include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include #include "math/pprz_algebra_float.h" #include "math/pprz_algebra_int.h" #include "state.h" #include "generated/airframe.h" -#include "stabilization_attitude_float.h" -#include "stabilization_attitude_rc_setpoint.h" -struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB]; +struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB]; struct FloatQuat stabilization_att_sum_err_quat; struct FloatEulers stabilization_att_sum_err_eulers; @@ -43,27 +42,27 @@ struct FloatRates last_body_rate; float stabilization_att_fb_cmd[COMMANDS_NB]; float stabilization_att_ff_cmd[COMMANDS_NB]; -static int gain_idx = STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT; +static int gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT; -static const float phi_pgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN; -static const float theta_pgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN; -static const float psi_pgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN; +static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN; +static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN; +static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN; -static const float phi_dgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN; -static const float theta_dgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN; -static const float psi_dgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN; +static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN; +static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN; +static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN; -static const float phi_igain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN; -static const float theta_igain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN; -static const float psi_igain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN; +static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN; +static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN; +static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN; -static const float phi_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN; -static const float theta_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN; -static const float psi_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN; +static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN; +static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN; +static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN; -static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_D; -static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_D; -static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_D; +static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D; +static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D; +static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D; #if defined COMMAND_ROLL_SURFACE && defined COMMAND_PITCH_SURFACE && defined COMMAND_YAW_SURFACE @@ -71,21 +70,21 @@ static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_D; #endif #ifdef HAS_SURFACE_COMMANDS -static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN_SURFACE; -static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN_SURFACE; -static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN_SURFACE; +static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE; +static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE; +static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE; -static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_SURFACE; -static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_SURFACE; -static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_SURFACE; +static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE; +static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE; +static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE; -static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN_SURFACE; -static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN_SURFACE; -static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN_SURFACE; +static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE; +static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE; +static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE; -static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN_SURFACE; -static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN_SURFACE; -static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN_SURFACE; +static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE; +static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE; +static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE; #endif #define IERROR_SCALE 1024 @@ -94,7 +93,7 @@ void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); - for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) { + for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); @@ -115,7 +114,7 @@ void stabilization_attitude_init(void) { void stabilization_attitude_gain_schedule(uint8_t idx) { - if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) { + if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) { // This could be bad -- Just say no. return; } @@ -125,10 +124,8 @@ void stabilization_attitude_gain_schedule(uint8_t idx) void stabilization_attitude_enter(void) { - float heading = stabilization_attitude_get_heading_f(); - /* reset psi setpoint to current psi angle */ - stab_att_sp_euler.psi = heading; + stab_att_sp_euler.psi = stabilization_attitude_get_heading_f(); stabilization_attitude_ref_enter(); @@ -136,6 +133,21 @@ void stabilization_attitude_enter(void) { FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } +void stabilization_attitude_set_failsafe_setpoint(void) { + /* set failsafe to zero roll/pitch and current heading */ + float heading2 = stabilization_attitude_get_heading_f() / 2; + stab_att_sp_quat.qi = cosf(heading2); + stab_att_sp_quat.qx = 0.0; + stab_att_sp_quat.qy = 0.0; + stab_att_sp_quat.qz = sinf(heading2); +} + +void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler); + FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler); + FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); +} + #ifndef GAIN_PRESCALER_FF #define GAIN_PRESCALER_FF 1 #endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h new file mode 100644 index 0000000000..9077bbd8cb --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h @@ -0,0 +1,46 @@ +/* + * 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. + */ + +/** + * @file stabilization_attitude_quat_float.h + * + * Rotorcraft attitude stabilization in quaternion float version. + */ + +#ifndef STABILIZATION_ATTITUDE_QUAT_FLOAT_H +#define STABILIZATION_ATTITUDE_QUAT_FLOAT_H + +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" + +#ifndef STABILIZATION_ATTITUDE_GAIN_NB +#define STABILIZATION_ATTITUDE_GAIN_NB 1 +#endif + +#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT +#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0 +#endif + +extern struct FloatAttitudeGains stabilization_gains[]; + +void stabilization_attitude_gain_schedule(uint8_t idx); + +#endif /* STABILIZATION_ATTITUDE_QUAT_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 437fa6c846..773bd481e8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -74,10 +74,8 @@ void stabilization_attitude_init(void) { void stabilization_attitude_enter(void) { - int32_t heading = stabilization_attitude_get_heading_i(); - /* reset psi setpoint to current psi angle */ - stab_att_sp_euler.psi = heading; + stab_att_sp_euler.psi = stabilization_attitude_get_heading_i(); stabilization_attitude_ref_enter(); @@ -85,6 +83,22 @@ void stabilization_attitude_enter(void) { INT_EULERS_ZERO(stabilization_att_sum_err); } +void stabilization_attitude_set_failsafe_setpoint(void) { + /* set failsafe to zero roll/pitch and current heading */ + int32_t heading2 = stabilization_attitude_get_heading_i() / 2; + PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2); + stab_att_sp_quat.qx = 0; + stab_att_sp_quat.qy = 0; + PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2); +} + +void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { + // copy euler setpoint for debugging + memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); + INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler); + INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); +} + #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h new file mode 100644 index 0000000000..1f9f78fc4f --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h @@ -0,0 +1,32 @@ +/* + * 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 STABILIZATION_ATTITUDE_QUAT_INT_H +#define STABILIZATION_ATTITUDE_QUAT_INT_H + +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h" + +#include "math/pprz_algebra_int.h" + +extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC + +#endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 86838e2619..b7b216771e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -28,21 +28,9 @@ #include "subsystems/radio_control.h" #include "state.h" #include "firmwares/rotorcraft/guidance/guidance_h.h" -#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" #include "firmwares/rotorcraft/autopilot.h" -#if defined STABILIZATION_ATTITUDE_TYPE_INT -#define SP_MAX_PHI (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) -#define SP_MAX_THETA (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) -#define SP_MAX_R (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) -#elif defined STABILIZATION_ATTITUDE_TYPE_FLOAT -#define SP_MAX_PHI STABILIZATION_ATTITUDE_SP_MAX_PHI -#define SP_MAX_THETA STABILIZATION_ATTITUDE_SP_MAX_THETA -#define SP_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R -#else -#error "STABILIZATION_ATTITUDE_TYPE not defined" -#endif - #ifndef RC_UPDATE_FREQ #define RC_UPDATE_FREQ 40 #endif @@ -96,13 +84,16 @@ float stabilization_attitude_get_heading_f(void) { * @param[out] sp attitude setpoint as euler angles */ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) { + const int32_t max_phi_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) / MAX_PPRZ; + const int32_t max_theta_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) / MAX_PPRZ; + const int32_t max_r_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) / MAX_PPRZ; - sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); - sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); + sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * max_phi_scale); + sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * max_theta_scale); if (in_flight) { if (YAW_DEADBAND_EXCEEDED()) { - sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); + sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ); INT32_ANGLE_NORMALIZE(sp->psi); } if (autopilot_mode == AP_MODE_FORWARD) { @@ -121,10 +112,11 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT // Make sure the yaw setpoint does not differ too much from the real yaw // to prevent a sudden switch at 180 deg + const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT); + int32_t heading = stabilization_attitude_get_heading_i(); int32_t delta_psi = sp->psi - heading; - int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT); INT32_ANGLE_NORMALIZE(delta_psi); if (delta_psi > delta_limit){ sp->psi = heading + delta_limit; @@ -162,12 +154,12 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight) { - sp->phi = (radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); - sp->theta = (radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); + sp->phi = (radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ); + sp->theta = (radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ); if (in_flight) { if (YAW_DEADBAND_EXCEEDED()) { - sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); + sp->psi += (radio_control.values[RADIO_YAW] * STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); FLOAT_ANGLE_NORMALIZE(sp->psi); } if (autopilot_mode == AP_MODE_FORWARD) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h similarity index 90% rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h index 9a8a3df9ed..b5076ff7cf 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h @@ -19,15 +19,12 @@ * Boston, MA 02111-1307, USA. */ -/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h - * Top level Rotorcraft attitude reference generation include. - * Automatically includes the actual implementation headers. +/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h + * Common rotorcraft attitude euler reference generation include. */ -#ifndef STABILIZATION_ATTITUDE_REF_H -#define STABILIZATION_ATTITUDE_REF_H - -#include STABILIZATION_ATTITUDE_REF_TYPE_H +#ifndef STABILIZATION_ATTITUDE_REF_EULER_H +#define STABILIZATION_ATTITUDE_REF_EULER_H #define SATURATE_SPEED_TRIM_ACCEL() { \ if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ @@ -62,4 +59,5 @@ } \ } -#endif /* STABILIZATION_ATTITUDE_REF_H */ + +#endif /* STABILIZATION_ATTITUDE_REF_EULER_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index 188a05479f..a70e780560 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -1,5 +1,5 @@ -#include "firmwares/rotorcraft/stabilization.h" - +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "generated/airframe.h" struct FloatEulers stab_att_sp_euler; struct FloatEulers stab_att_ref_euler; @@ -21,25 +21,37 @@ void stabilization_attitude_ref_init(void) { */ #define DT_UPDATE (1./PERIODIC_FREQUENCY) -#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT -#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT -#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_RDOT +#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT +#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT +#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT -#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_P -#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_Q -#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_R +#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P +#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q +#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R -#define OMEGA_P STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_P -#define OMEGA_Q STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_Q -#define OMEGA_R STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R +#define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P +#define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q +#define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R -#define ZETA_P STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_P -#define ZETA_Q STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_Q -#define ZETA_R STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R +#define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P +#define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q +#define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R #define USE_REF 1 +static inline void reset_psi_ref_from_body(void) { + //sp has been set from body using stabilization_attitude_get_yaw_f, use that value + stab_att_ref_euler.psi = stab_att_sp_euler.psi; + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; +} + +void stabilization_attitude_ref_enter() +{ + reset_psi_ref_from_body(); +} + void stabilization_attitude_ref_update() { #if USE_REF diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index 687508f596..fbb74093de 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -22,20 +22,12 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H -#include "subsystems/radio_control.h" #include "math/pprz_algebra_float.h" - #include "stabilization_attitude_ref_float.h" +#include "stabilization_attitude_ref_euler.h" - -#define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) { \ - struct FloatEulers add_sp_float; \ - EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \ - EULERS_ADD(stabilization_att_sp,add_sp_float); \ - FLOAT_ANGLE_NORMALIZE(stabilization_att_sp.psi); \ - } - +void stabilization_attitude_ref_enter(void); #endif /* STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index bbb464a251..60d24517b8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -24,7 +24,8 @@ * */ -#include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "generated/airframe.h" struct Int32Eulers stab_att_sp_euler; struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index 8312acf2ff..e4dee67c49 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -28,6 +28,7 @@ #define STABILIZATION_ATTITUDE_REF_EULER_INT_H #include "stabilization_attitude_ref_int.h" +#include "stabilization_attitude_ref_euler.h" #endif /* STABILIZATION_ATTITUDE_REF_EULER_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h index d11bbf30fa..2faf9e0bc9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h @@ -18,6 +18,12 @@ * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ + +/** @file stabilization_attitude_ref_float.h + * Rotorcraft attitude reference generation API. + * Common to all floating-point reference generators (euler and quaternion) + */ + #ifndef STABILIZATION_ATTITUDE_REF_FLOAT_H #define STABILIZATION_ATTITUDE_REF_FLOAT_H @@ -39,11 +45,7 @@ struct FloatRefModel { extern struct FloatRefModel stab_att_ref_model[]; -static inline void reset_psi_ref_from_body(void) { -//sp has been set from body using stabilization_attitude_get_yaw_f, use that value - stab_att_ref_euler.psi = stab_att_sp_euler.psi; - stab_att_ref_rate.r = 0; - stab_att_ref_accel.r = 0; -} +extern void stabilization_attitude_ref_init(void); +extern void stabilization_attitude_ref_update(void); #endif /* STABILIZATION_ATTITUDE_REF_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index 4e8724dff6..cbaf015206 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -31,8 +31,6 @@ #include "state.h" -extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC -extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC @@ -56,12 +54,7 @@ extern struct Int32RefModel stab_att_ref_model; while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \ } - -static inline void reset_psi_ref_from_body(void) { -//sp has been set from body using stabilization_attitude_get_yaw_i, use that value - stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); - stab_att_ref_rate.r = 0; - stab_att_ref_accel.r = 0; -} +extern void stabilization_attitude_ref_init(void); +extern void stabilization_attitude_ref_update(void); #endif /* STABILIZATION_ATTITUDE_REF_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index b8e0b41330..6bd8ded0b5 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -33,9 +33,9 @@ #include "stabilization_attitude_ref_float.h" -#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT -#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT -#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_RDOT +#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT +#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT +#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT struct FloatEulers stab_att_sp_euler; struct FloatQuat stab_att_sp_quat; @@ -44,16 +44,23 @@ struct FloatQuat stab_att_ref_quat; struct FloatRates stab_att_ref_rate; struct FloatRates stab_att_ref_accel; -struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB]; +struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_GAIN_NB]; -static int ref_idx = STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT; +static int ref_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT; -static const float omega_p[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_P; -static const float zeta_p[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_P; -static const float omega_q[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_Q; -static const float zeta_q[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_Q; -static const float omega_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R; -static const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R; +static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P; +static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P; +static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q; +static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q; +static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R; +static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R; + +static inline void reset_psi_ref_from_body(void) { + //sp has been set from body using stabilization_attitude_get_yaw_f, use that value + stab_att_ref_euler.psi = stab_att_sp_euler.psi; + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; +} static void update_ref_quat_from_eulers(void) { struct FloatRMat ref_rmat; @@ -76,7 +83,7 @@ void stabilization_attitude_ref_init(void) { FLOAT_RATES_ZERO( stab_att_ref_rate); FLOAT_RATES_ZERO( stab_att_ref_accel); - for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) { + for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]); RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]); } @@ -102,14 +109,14 @@ void stabilization_attitude_ref_enter() #define DT_UPDATE (1./PERIODIC_FREQUENCY) // default to fast but less precise quaternion integration -#ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_INFINITESIMAL_STEP -#define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_INFINITESIMAL_STEP TRUE +#ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP +#define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE #endif void stabilization_attitude_ref_update() { /* integrate reference attitude */ -#if STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_INFINITESIMAL_STEP +#if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP struct FloatQuat qdot; FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); QUAT_SMUL(qdot, qdot, DT_UPDATE); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h index f4df2351d1..83323d06dd 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h @@ -30,8 +30,6 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H -#include "firmwares/rotorcraft/stabilization.h" - #include "subsystems/radio_control.h" #include "math/pprz_algebra_float.h" diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index e948896ffe..b74c0f0ee6 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -74,6 +74,12 @@ struct Int32RefModel stab_att_ref_model = { {STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R} }; +static inline void reset_psi_ref_from_body(void) { + //sp has been set from body using stabilization_attitude_get_yaw_i, use that value + stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; +} void stabilization_attitude_ref_init(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h index a99a8044b9..4fe03f3218 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h @@ -30,10 +30,8 @@ #ifndef STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H -#include "firmwares/rotorcraft/stabilization.h" #include "stabilization_attitude_ref_int.h" - void stabilization_attitude_ref_enter(void); #endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 39f06618a7..05639f2ca2 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -223,6 +223,7 @@ &stabilization_cmd[COMMAND_THRUST]); \ } +#ifdef STABILIZATION_ATTITUDE_REF_TYPE_H #ifdef STABILIZATION_ATTITUDE_TYPE_INT #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ struct Int32Rates* body_rate = stateGetBodyRates_i(); \ @@ -263,9 +264,9 @@ &stab_att_ref_accel.q, \ &stab_att_ref_accel.r); \ } -#endif /* STABILIZATION_ATTITUDE_TYPE_INT */ -#ifdef STABILIZATION_ATTITUDE_TYPE_FLOAT +#elif defined STABILIZATION_ATTITUDE_TYPE_FLOAT + #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ struct FloatRates* body_rate = stateGetBodyRates_f(); \ struct FloatEulers* att = stateGetNedToBodyEulers_f(); \ @@ -308,6 +309,10 @@ } #endif /* STABILIZATION_ATTITUDE_TYPE_FLOAT */ +#else /* STABILIZATION_ATTITUDE_REF_TYPE_H */ +#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) {} +#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) {} +#endif #include "subsystems/ahrs/ahrs_aligner.h"