diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index 378238291a2..2fa27fd0100 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -210,8 +210,6 @@ - - diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 985f80a9375..0272f455b08 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -111,39 +111,6 @@ nps.srcs += $(SRC_FIRMWARE)/stabilization.c nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c - -NUM_TYPE=integer -#NUM_TYPE=float - -STAB_TYPE=euler -#STAB_TYPE=quaternion - -ifeq ($(NUM_TYPE), integer) - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" - ifeq ($(STAB_TYPE), euler) - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\" - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c - else ifeq ($(STAB_TYPE), quaternion) - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c - endif -else ifeq ($(NUM_TYPE), float) - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_FLOAT - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\" - ifeq ($(STAB_TYPE), euler) - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_float.h\" - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_float.c - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_float.c - else ifeq ($(STAB_TYPE), quaternion) - nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_float.h\" - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c - nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c - endif -endif - nps.CFLAGS += -DUSE_NAVIGATION nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile index 3fc38bf53e3..e62c41b6875 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile @@ -1,5 +1,12 @@ -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\" -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c +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 + +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_float_euler.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile new file mode 100644 index 00000000000..4f3b11e76d2 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile @@ -0,0 +1,12 @@ +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_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 + +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_float_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile index 770097569d1..ba005a992d5 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile @@ -1,6 +1,13 @@ -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_FLOAT -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\" -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_float.h\" -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c +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_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 + +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 775dc7ce34b..10e015f4a2b 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile @@ -1,6 +1,13 @@ -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" -ap.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_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 +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_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 + +ap.CFLAGS += $(STAB_ATT_CFLAGS) +ap.srcs += $(STAB_ATT_SRCS) + +nps.CFLAGS += $(STAB_ATT_CFLAGS) +nps.srcs += $(STAB_ATT_SRCS) diff --git a/conf/messages.xml b/conf/messages.xml index fa4252a470d..733ca40e930 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1821,10 +1821,10 @@ - + - + diff --git a/conf/settings/rotorcraft_basic.xml b/conf/settings/rotorcraft_basic.xml index a053af6a4c1..00883dccf2e 100644 --- a/conf/settings/rotorcraft_basic.xml +++ b/conf/settings/rotorcraft_basic.xml @@ -4,7 +4,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 0f029292023..c2a07cdf0fd 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -130,77 +130,82 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { if (new_autopilot_mode != autopilot_mode) { /* horizontal mode */ switch (new_autopilot_mode) { - case AP_MODE_FAILSAFE: + case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE - stab_att_sp_euler.phi = 0; - stab_att_sp_euler.theta = 0; - guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); - break; + stab_att_sp_euler.phi = 0; + stab_att_sp_euler.theta = 0; + guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); + break; #endif - case AP_MODE_KILL: - autopilot_set_motors_on(FALSE); - autopilot_in_flight = FALSE; - autopilot_in_flight_counter = 0; - guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); - break; - case AP_MODE_RC_DIRECT: - guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT); - break; - case AP_MODE_RATE_DIRECT: - case AP_MODE_RATE_Z_HOLD: - guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); - break; - case AP_MODE_ATTITUDE_DIRECT: - case AP_MODE_ATTITUDE_CLIMB: - case AP_MODE_ATTITUDE_Z_HOLD: - guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); - break; - case AP_MODE_HOVER_DIRECT: - case AP_MODE_HOVER_CLIMB: - case AP_MODE_HOVER_Z_HOLD: - guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); - break; - case AP_MODE_NAV: - guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); - break; - default: - break; + case AP_MODE_KILL: + autopilot_set_motors_on(FALSE); + autopilot_in_flight = FALSE; + autopilot_in_flight_counter = 0; + guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); + break; + case AP_MODE_RC_DIRECT: + guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT); + break; + case AP_MODE_RATE_DIRECT: + case AP_MODE_RATE_Z_HOLD: + guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); + break; + case AP_MODE_ATTITUDE_RC_CLIMB: + case AP_MODE_ATTITUDE_DIRECT: + case AP_MODE_ATTITUDE_CLIMB: + case AP_MODE_ATTITUDE_Z_HOLD: + guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); + break; + case AP_MODE_CARE_FREE_DIRECT: + guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE); + break; + case AP_MODE_HOVER_DIRECT: + case AP_MODE_HOVER_CLIMB: + case AP_MODE_HOVER_Z_HOLD: + guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); + break; + case AP_MODE_NAV: + guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); + break; + default: + break; } /* vertical mode */ switch (new_autopilot_mode) { - case AP_MODE_FAILSAFE: + case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE - guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); - guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); - break; + guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); + guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); + break; #endif - case AP_MODE_KILL: - guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); - break; - case AP_MODE_RC_DIRECT: - case AP_MODE_RATE_DIRECT: - case AP_MODE_ATTITUDE_DIRECT: - case AP_MODE_HOVER_DIRECT: - guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); - break; - case AP_MODE_RATE_RC_CLIMB: - case AP_MODE_ATTITUDE_RC_CLIMB: - guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); - break; - case AP_MODE_ATTITUDE_CLIMB: - case AP_MODE_HOVER_CLIMB: - guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); - break; - case AP_MODE_RATE_Z_HOLD: - case AP_MODE_ATTITUDE_Z_HOLD: - case AP_MODE_HOVER_Z_HOLD: - guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); - break; - case AP_MODE_NAV: - guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); - break; - default: - break; + case AP_MODE_KILL: + guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); + break; + case AP_MODE_RC_DIRECT: + case AP_MODE_RATE_DIRECT: + case AP_MODE_ATTITUDE_DIRECT: + case AP_MODE_HOVER_DIRECT: + case AP_MODE_CARE_FREE_DIRECT: + guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); + break; + case AP_MODE_RATE_RC_CLIMB: + case AP_MODE_ATTITUDE_RC_CLIMB: + guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); + break; + case AP_MODE_ATTITUDE_CLIMB: + case AP_MODE_HOVER_CLIMB: + guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); + break; + case AP_MODE_RATE_Z_HOLD: + case AP_MODE_ATTITUDE_Z_HOLD: + case AP_MODE_HOVER_Z_HOLD: + guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); + break; + case AP_MODE_NAV: + guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); + break; + default: + break; } autopilot_mode = new_autopilot_mode; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index b65f56fb3d2..78d9cf310ad 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -49,8 +49,8 @@ #define AP_MODE_HOVER_CLIMB 10 #define AP_MODE_HOVER_Z_HOLD 11 #define AP_MODE_NAV 12 -#define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13" - +#define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13" +#define AP_MODE_CARE_FREE_DIRECT 14 extern uint8_t autopilot_mode; extern uint8_t autopilot_mode_auto2; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 0c2ebc0ac7b..75c60529a69 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -59,8 +59,8 @@ int32_t guidance_h_igain; int32_t guidance_h_again; /* warn if some gains are still negative */ -#if (GUIDANCE_H_PGAIN < 0) || \ - (GUIDANCE_H_DGAIN < 0) || \ +#if (GUIDANCE_H_PGAIN < 0) || \ + (GUIDANCE_H_DGAIN < 0) || \ (GUIDANCE_H_IGAIN < 0) #warning "ALL control gains are now positive!!!" #endif @@ -84,10 +84,10 @@ static inline void guidance_h_hover_enter(void); static inline void guidance_h_nav_enter(void); #define GuidanceHSetRef(_pos, _speed, _accel) { \ - b2_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); \ + b2_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); \ } @@ -111,28 +111,29 @@ void guidance_h_mode_changed(uint8_t new_mode) { return; switch (new_mode) { + case GUIDANCE_H_MODE_RC_DIRECT: + stabilization_none_enter(); + break; - case GUIDANCE_H_MODE_RC_DIRECT: - stabilization_none_enter(); - break; - - case GUIDANCE_H_MODE_RATE: - stabilization_rate_enter(); - break; + case GUIDANCE_H_MODE_RATE: + stabilization_rate_enter(); + break; - case GUIDANCE_H_MODE_ATTITUDE: - stabilization_attitude_enter(); - break; + case GUIDANCE_H_MODE_CARE_FREE: + stabilization_attitude_reset_care_free_heading(); + case GUIDANCE_H_MODE_ATTITUDE: + stabilization_attitude_enter(); + break; - case GUIDANCE_H_MODE_HOVER: - guidance_h_hover_enter(); - break; + case GUIDANCE_H_MODE_HOVER: + guidance_h_hover_enter(); + break; - case GUIDANCE_H_MODE_NAV: - guidance_h_nav_enter(); - break; - default: - break; + case GUIDANCE_H_MODE_NAV: + guidance_h_nav_enter(); + break; + default: + break; } guidance_h_mode = new_mode; @@ -144,32 +145,33 @@ void guidance_h_read_rc(bool_t in_flight) { switch ( guidance_h_mode ) { - case GUIDANCE_H_MODE_RC_DIRECT: - stabilization_none_read_rc(); - break; - - case GUIDANCE_H_MODE_RATE: - stabilization_rate_read_rc(); - break; + case GUIDANCE_H_MODE_RC_DIRECT: + stabilization_none_read_rc(); + break; - case GUIDANCE_H_MODE_ATTITUDE: - stabilization_attitude_read_rc(in_flight); - break; + case GUIDANCE_H_MODE_RATE: + stabilization_rate_read_rc(); + break; - case GUIDANCE_H_MODE_HOVER: - stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight); - break; + case GUIDANCE_H_MODE_CARE_FREE: + case GUIDANCE_H_MODE_ATTITUDE: + stabilization_attitude_read_rc(in_flight); + break; - case GUIDANCE_H_MODE_NAV: - if (radio_control.status == RC_OK) { + case GUIDANCE_H_MODE_HOVER: stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight); - } - else { - INT_EULERS_ZERO(guidance_h_rc_sp); - } - break; - default: - break; + break; + + case GUIDANCE_H_MODE_NAV: + if (radio_control.status == RC_OK) { + stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight); + } + else { + INT_EULERS_ZERO(guidance_h_rc_sp); + } + break; + default: + break; } } @@ -178,31 +180,31 @@ void guidance_h_read_rc(bool_t in_flight) { void guidance_h_run(bool_t in_flight) { switch ( guidance_h_mode ) { - case GUIDANCE_H_MODE_RC_DIRECT: - stabilization_none_run(in_flight); - break; + case GUIDANCE_H_MODE_RC_DIRECT: + stabilization_none_run(in_flight); + break; - case GUIDANCE_H_MODE_RATE: - stabilization_rate_run(in_flight); - break; + case GUIDANCE_H_MODE_RATE: + stabilization_rate_run(in_flight); + break; - case GUIDANCE_H_MODE_ATTITUDE: - stabilization_attitude_run(in_flight); - break; + case GUIDANCE_H_MODE_CARE_FREE: + case GUIDANCE_H_MODE_ATTITUDE: + stabilization_attitude_run(in_flight); + break; - case GUIDANCE_H_MODE_HOVER: - guidance_h_update_reference(FALSE); + case GUIDANCE_H_MODE_HOVER: + guidance_h_update_reference(FALSE); - /* set psi command */ - guidance_h_command_body.psi = guidance_h_rc_sp.psi; - /* compute roll and pitch commands and set final attitude setpoint */ - guidance_h_traj_run(in_flight); + /* set psi command */ + guidance_h_command_body.psi = guidance_h_rc_sp.psi; + /* compute roll and pitch commands and set final attitude setpoint */ + guidance_h_traj_run(in_flight); - stabilization_attitude_run(in_flight); - break; + stabilization_attitude_run(in_flight); + break; - case GUIDANCE_H_MODE_NAV: - { + case GUIDANCE_H_MODE_NAV: if (!in_flight) guidance_h_nav_enter(); if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { @@ -230,9 +232,9 @@ void guidance_h_run(bool_t in_flight) { } stabilization_attitude_run(in_flight); break; - } - default: - break; + + default: + break; } } @@ -306,7 +308,7 @@ static inline void guidance_h_traj_run(bool_t in_flight) { // Restore angle ref resolution after rotation guidance_h_command_body.phi = - ( - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; + ( - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; guidance_h_command_body.theta = - ( c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index f24ea8c3508..aec9bcf361b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -38,6 +38,7 @@ #define GUIDANCE_H_MODE_HOVER 3 #define GUIDANCE_H_MODE_NAV 4 #define GUIDANCE_H_MODE_RC_DIRECT 5 +#define GUIDANCE_H_MODE_CARE_FREE 6 extern uint8_t guidance_h_mode; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 7c5fcb5f6da..9ee532725f9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -24,6 +24,7 @@ #include STABILIZATION_ATTITUDE_TYPE_H + extern void stabilization_attitude_init(void); extern void stabilization_attitude_read_rc(bool_t in_flight); extern void stabilization_attitude_enter(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c new file mode 100644 index 00000000000..95f236e09c6 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -0,0 +1,251 @@ +/* + * Copyright (C) 2012-2013 Felix Ruess + * + * 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_rc_setpoint.c + * Read an attitude setpoint from the RC. + */ + +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" + +#include "subsystems/radio_control.h" +#include "state.h" +#include "firmwares/rotorcraft/guidance/guidance_h.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref.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 + +float care_free_heading = 0; + +/// reset the heading for care-free mode to current heading +void stabilization_attitude_reset_care_free_heading(void) { + care_free_heading = stateGetNedToBodyEulers_f()->psi; +} + +/** Read attitude setpoint from RC as euler angles. + * @param[in] in_flight true if in flight + * @param[out] sp attitude setpoint as euler angles + */ +void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) { + + 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); + + if (in_flight) { + if (YAW_DEADBAND_EXCEEDED()) { + sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); + INT32_ANGLE_NORMALIZE(sp->psi); + } +#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 + int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi; + 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 = stateGetNedToBodyEulers_i()->psi + delta_limit; + } + else if (delta_psi < -delta_limit){ + sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit; + } + INT32_ANGLE_NORMALIZE(sp->psi); +#endif + //Care Free mode + if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) { + //care_free_heading has been set to current psi when entering care free mode. + int32_t cos_psi; + int32_t sin_psi; + int32_t temp_theta; + int32_t care_free_delta_psi_i; + + care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading); + + INT32_ANGLE_NORMALIZE(care_free_delta_psi_i); + + PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i); + PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i); + + temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC); + sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC); + + sp->theta = temp_theta; + } + } + else { /* if not flying, use current yaw as setpoint */ + sp->psi = stateGetNedToBodyEulers_i()->psi; + } +} + + +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); + + if (in_flight) { + if (YAW_DEADBAND_EXCEEDED()) { + sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); + FLOAT_ANGLE_NORMALIZE(sp->psi); + } +#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 + float delta_psi = sp->psi - stateGetNedToBodyEulers_f()->psi; + FLOAT_ANGLE_NORMALIZE(delta_psi); + if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){ + sp->psi = stateGetNedToBodyEulers_f()->psi + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT; + } + else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){ + sp->psi = stateGetNedToBodyEulers_f()->psi - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT; + } + FLOAT_ANGLE_NORMALIZE(sp->psi); +#endif + //Care Free mode + if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) { + //care_free_heading has been set to current psi when entering care free mode. + float cos_psi; + float sin_psi; + float temp_theta; + + float care_free_delta_psi_f = sp->psi - care_free_heading; + + FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f); + + sin_psi = sin(care_free_delta_psi_f); + cos_psi = cos(care_free_delta_psi_f); + + temp_theta = cos_psi*sp->theta - sin_psi*sp->phi; + sp->phi = cos_psi*sp->phi - sin_psi*sp->theta; + + sp->theta = temp_theta; + } + } + else { /* if not flying, use current yaw as setpoint */ + sp->psi = stateGetNedToBodyEulers_f()->psi; + } +} + + +/** Read roll/pitch command from RC as quaternion. + * Interprets the stick positions as axes. + * @param[out] q quaternion representing the RC roll/pitch input + */ +void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { + q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; + q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; + q->qz = 0.0; + + /* normalize */ + float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy)); + q->qi = 1.0 / norm; + q->qx /= norm; + q->qy /= norm; +} + +/** Read roll/pitch command from RC as quaternion. + * Both angles are are interpreted relative to to the horizontal plane (earth bound). + * @param[out] q quaternion representing the RC roll/pitch input + */ +void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q) { + /* only non-zero entries for roll quaternion */ + float roll2 = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; + float qx_roll = sinf(roll2); + float qi_roll = cosf(roll2); + + /* only non-zero entries for pitch quaternion */ + float pitch2 = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; + float qy_pitch = sinf(pitch2); + float qi_pitch = cosf(pitch2); + + /* only multiply non-zero entries of FLOAT_QUAT_COMP(*q, q_roll, q_pitch) */ + q->qi = qi_roll * qi_pitch; + q->qx = qx_roll * qi_pitch; + q->qy = qi_roll * qy_pitch; + q->qz = qx_roll * qy_pitch; +} + +void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight) { + + // FIXME: remove me, do in quaternion directly + // is currently still needed, since the yaw setpoint integration is done in eulers +#if defined STABILIZATION_ATTITUDE_TYPE_INT + stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight); +#else + stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); +#endif + + struct FloatQuat q_rp_cmd; +#if USE_EARTH_BOUND_RC_SETPOINT + stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); +#else + stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd); +#endif + + /* get current heading */ + const struct FloatVect3 zaxis = {0., 0., 1.}; + struct FloatQuat q_yaw; + + //Care Free mode + if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) { + //care_free_heading has been set to current psi when entering care free mode. + FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, care_free_heading); + } + else { + FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi); + } + + /* roll/pitch commands applied to to current heading */ + struct FloatQuat q_rp_sp; + FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd); + FLOAT_QUAT_NORMALIZE(q_rp_sp); + + if (in_flight) + { + /* get current heading setpoint */ + struct FloatQuat q_yaw_sp; +#if defined STABILIZATION_ATTITUDE_TYPE_INT + FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); +#else + FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi); +#endif + + /* rotation between current yaw and yaw setpoint */ + struct FloatQuat q_yaw_diff; + FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw); + + /* compute final setpoint with yaw */ + FLOAT_QUAT_COMP_NORM_SHORTEST(*q_sp, q_rp_sp, q_yaw_diff); + } else { + QUAT_COPY(*q_sp, q_rp_sp); + } +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h index 5712726256d..f9df2a76f88 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h @@ -34,21 +34,6 @@ #include "subsystems/radio_control.h" #include "state.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 #ifdef STABILIZATION_ATTITUDE_DEADBAND_A #define ROLL_DEADBAND_EXCEEDED() \ @@ -70,148 +55,11 @@ (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \ radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R) +extern void stabilization_attitude_reset_care_free_heading(void); +extern void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight); +extern void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight); +extern void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q); +extern void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q); +extern void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight); -static inline void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) { - - 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); - - if (in_flight) { - if (YAW_DEADBAND_EXCEEDED()) { - sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); - INT32_ANGLE_NORMALIZE(sp->psi); - } -#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 - int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi; - 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 = stateGetNedToBodyEulers_i()->psi + delta_limit; - } - else if (delta_psi < -delta_limit){ - sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit; - } - INT32_ANGLE_NORMALIZE(sp->psi); -#endif - } - else { /* if not flying, use current yaw as setpoint */ - sp->psi = stateGetNedToBodyEulers_i()->psi; - } -} - - -static inline 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); - - if (in_flight) { - if (YAW_DEADBAND_EXCEEDED()) { - sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); - FLOAT_ANGLE_NORMALIZE(sp->psi); - } -#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 - float delta_psi = sp->psi - stateGetNedToBodyEulers_f()->psi; - FLOAT_ANGLE_NORMALIZE(delta_psi); - if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){ - sp->psi = stateGetNedToBodyEulers_f()->psi + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT; - } - else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){ - sp->psi = stateGetNedToBodyEulers_f()->psi - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT; - } - FLOAT_ANGLE_NORMALIZE(sp->psi); -#endif - } - else { /* if not flying, use current yaw as setpoint */ - sp->psi = stateGetNedToBodyEulers_f()->psi; - } -} - - -/** Read roll/pitch command from RC as quaternion. - * Interprets the stick positions as axes. - * @param[out] q quaternion representing the RC roll/pitch input - */ -static inline void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { - q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; - q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; - q->qz = 0.0; - - /* normalize */ - float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy)); - q->qi = 1.0 / norm; - q->qx /= norm; - q->qy /= norm; -} - -/** Read roll/pitch command from RC as quaternion. - * Both angles are are interpreted relative to to the horizontal plane (earth bound). - * @param[out] q quaternion representing the RC roll/pitch input - */ -static inline void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q) { - /* only non-zero entries for roll quaternion */ - float roll2 = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; - float qx_roll = sinf(roll2); - float qi_roll = cosf(roll2); - - /* only non-zero entries for pitch quaternion */ - float pitch2 = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; - float qy_pitch = sinf(pitch2); - float qi_pitch = cosf(pitch2); - - /* only multiply non-zero entries of FLOAT_QUAT_COMP(*q, q_roll, q_pitch) */ - q->qi = qi_roll * qi_pitch; - q->qx = qx_roll * qi_pitch; - q->qy = qi_roll * qy_pitch; - q->qz = qx_roll * qy_pitch; -} - -static inline void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight) { - - // FIXME: remove me, do in quaternion directly - // is currently still needed, since the yaw setpoint integration is done in eulers -#if defined STABILIZATION_ATTITUDE_TYPE_INT - stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight); -#else - stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); -#endif - - struct FloatQuat q_rp_cmd; -#if USE_EARTH_BOUND_RC_SETPOINT - stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); -#else - stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd); -#endif - - /* get current heading */ - const struct FloatVect3 zaxis = {0., 0., 1.}; - struct FloatQuat q_yaw; - FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi); - - /* roll/pitch commands applied to to current heading */ - struct FloatQuat q_rp_sp; - FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd); - FLOAT_QUAT_NORMALIZE(q_rp_sp); - - if (in_flight) - { - /* get current heading setpoint */ - struct FloatQuat q_yaw_sp; -#if defined STABILIZATION_ATTITUDE_TYPE_INT - FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); -#else - FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi); -#endif - - /* rotation between current yaw and yaw setpoint */ - struct FloatQuat q_yaw_diff; - FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw); - - /* compute final setpoint with yaw */ - FLOAT_QUAT_COMP_NORM_SHORTEST(*q_sp, q_rp_sp, q_yaw_diff); - } else { - QUAT_COPY(*q_sp, q_rp_sp); - } -} #endif /* STABILIZATION_ATTITUDE_RC_SETPOINT_H */ diff --git a/sw/airborne/modules/gain_scheduling/gain_scheduling.c b/sw/airborne/modules/gain_scheduling/gain_scheduling.c index deb68c6fc3a..e90663c4ff8 100644 --- a/sw/airborne/modules/gain_scheduling/gain_scheduling.c +++ b/sw/airborne/modules/gain_scheduling/gain_scheduling.c @@ -27,6 +27,7 @@ //Include for scheduling on transition_status #include "firmwares/rotorcraft/guidance/guidance_h.h" +#include "firmwares/rotorcraft/stabilization.h" // #include "state.h" #include "math/pprz_algebra_int.h" diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml index fb6d60214bd..0d81357ce9c 100644 --- a/sw/ground_segment/tmtc/server_globals.ml +++ b/sw/ground_segment/tmtc/server_globals.ml @@ -4,7 +4,7 @@ let hostname = ref "localhost" (** FIXME: Should be read from messages.xml *) let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|] -let rotorcraft_ap_modes = [|"SAFE";"KILL";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D"|] +let rotorcraft_ap_modes = [|"SAFE";"KILL";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF"|] let _AUTO2 = 2 let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|] let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]