diff --git a/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile b/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile new file mode 100644 index 0000000000..2403f25272 --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile @@ -0,0 +1,7 @@ +ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT +ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" +ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" +ap.CFLAGS += -DUSE_SETPOINTS_WITH_TRANSITIONS +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint_int.c diff --git a/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile b/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile index 965ebaf7b1..344044bbe6 100644 --- a/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile +++ b/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile @@ -1,6 +1 @@ -ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT -ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" -ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c -ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint_int.c +$(error The stabilization quaternion subsystem has been changed, for normal rotorcrafts please use , for transitioning rotorcrafts (quadshot) use ) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c index bac79356d4..10b3029f05 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c @@ -55,21 +55,7 @@ static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initia QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2); } -static void update_sp_quat_from_eulers(void) { - struct Int32RMat sp_rmat; - -#ifdef STICKS_RMAT312 - INT32_RMAT_OF_EULERS_312(sp_rmat, stab_att_sp_euler); -#else - INT32_RMAT_OF_EULERS_321(sp_rmat, stab_att_sp_euler); -#endif - INT32_QUAT_OF_RMAT(stab_att_sp_quat, sp_rmat); - INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); -} - - -// FIXME: function parameter sp not used -void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight) { +void stabilization_attitude_read_rc_absolute(bool_t in_flight) { // FIXME: wtf??? #ifdef AIRPLANE_STICKS @@ -108,7 +94,7 @@ void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_fl INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat); } -void stabilization_attitude_sp_enter() +void stabilization_attitude_sp_enter(void) { // reset setpoint to "hover" reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h index 2525f94b23..da4b8b08ac 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h @@ -13,6 +13,6 @@ void stabilization_attitude_sp_enter(void); -void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight); +void stabilization_attitude_read_rc_absolute(bool_t in_flight); #endif 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 0a2ed1933c..58ef79cce4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -25,7 +25,10 @@ */ #include "firmwares/rotorcraft/stabilization.h" + +#if USE_SETPOINTS_WITH_TRANSITIONS #include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h" +#endif #include #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 a17a919cbb..be4c3218d5 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 @@ -32,6 +32,10 @@ #include "stabilization_attitude_ref_int.h" +#if USE_SETPOINTS_WITH_TRANSITIONS +#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h" +#endif + #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC) #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC) #define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC) @@ -110,8 +114,15 @@ void stabilization_attitude_ref_init(void) { void stabilization_attitude_ref_enter() { reset_psi_ref_from_body(); + +#if USE_SETPOINTS_WITH_TRANSITIONS + stabilization_attitude_sp_enter(); + memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat)); +#else update_quat_from_eulers(&stab_att_ref_quat, &stab_att_ref_euler); - //memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat)); +#endif + + /* set reference rate and acceleration to zero */ memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates)); memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates)); } 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 0ff87e8d3e..a878c855f0 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 @@ -22,7 +22,6 @@ #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H #include "firmwares/rotorcraft/stabilization.h" -//#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h" #include "subsystems/radio_control.h" #include "math/pprz_algebra_float.h"