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"|]