Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 06:54:49 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[rotorcraft] big fat attempt to clean stabilization and guidance

Browse Source 
- No more STABILIZATION_ATTITUDE_FLOAT_x in airframe file.
- Hopefully seamlessly switch between int and float.
- Also make it possible to not use a reference at all, e.g. for passthrough
This commit is contained in:
Felix Ruess
2013-05-02 22:09:41 +02:00
parent 2556e6e84b
commit 1de769b6f2
37 changed files with 484 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/esden/gain_scheduling_example.xml
-1
View File
@@ -185,7 +185,6 @@
<section name="MISC">
<define name="FACE_REINJ_1" value="1024"/>
<define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
conf/airframes/esden/qs_asp22.xml
-1
View File
@@ -192,7 +192,6 @@
</section>
<section name="MISC">
<define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/>
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- <define name="USE_REFERENCE_SYSTEM" value="FALSE"/> -->
<define name="SWITCH_STICKS_FOR_RATE_CONTROL" value="TRUE"/>
conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile
+2 -11
View File
@@ -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 <subsystem name="stabilization" type="euler"/> with <subsystem name="stabilization" type="int_euler"/> 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
conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile
+1 -2
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile
+1 -2
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile
+11
View File
@@ -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)
conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile
+1 -2
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/stabilization_passthrough.makefile
+1 -2
View File
@@ -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)
conf/settings/control/stabilization_att_int.xml
+12 -12
View File
@@ -4,18 +4,18 @@
<dl_settings>
<dl_settings NAME="Att Loop">
<dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000" module="stabilization/stabilization_attitude" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
<dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
<dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000" module="stabilization/stabilization_attitude" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
<dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
<dl_setting var="stabilization_gains.i.y" min="0" step="1" max="800" module="stabilization/stabilization_attitude" shortname="igain theta" param="STABILIZATION_ATTITUDE_THETA_IGAIN"/>
<dl_setting var="stabilization_gains.dd.y" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN"/>
<dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN"/>
<dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN"/>
<dl_setting var="stabilization_gains.i.z" min="0" step="1" max="400" module="stabilization/stabilization_attitude" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN"/>
<dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN"/>
<dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
<dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
<dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
<dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
<dl_setting var="stabilization_gains.i.y" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain theta" param="STABILIZATION_ATTITUDE_THETA_IGAIN"/>
<dl_setting var="stabilization_gains.dd.y" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN"/>
<dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN"/>
<dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN"/>
<dl_setting var="stabilization_gains.i.z" min="0" step="1" max="400" module="stabilization/stabilization_attitude_common_int" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN"/>
<dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN"/>
</dl_settings>
</dl_settings>
sw/airborne/firmwares/rotorcraft/autopilot.c
+1 -2
View File
@@ -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
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+34 -38
View File
@@ -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) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+7 -8
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h → sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h
+10 -13
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h → sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h
+17 -3
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+29 -17
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h
+37
View File
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+16 -5
View File
@@ -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();
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h
+28
View File
@@ -0,0 +1,28 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+5 -23
View File
@@ -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,10 +55,11 @@ 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;
@@ -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) {
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
+33
View File
@@ -0,0 +1,33 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+48 -36
View File
@@ -24,16 +24,15 @@
*/
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include <stdio.h>
#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
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h
+46
View File
@@ -0,0 +1,46 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+17 -3
View File
@@ -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))
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
+32
View File
@@ -0,0 +1,32 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
+12 -20
View File
@@ -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) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h → sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h
+6 -8
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
+26 -14
View File
@@ -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
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
+2 -10
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
+2 -1
View File
@@ -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
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
+1
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
+8 -6
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+2 -9
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
+22 -15
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
-2
View File
@@ -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"
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+6
View File
@@ -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) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
-2
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/telemetry.h
+7 -2
View File
@@ -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"